diff --git a/esp.ino b/esp.ino deleted file mode 100644 index 57b5922..0000000 --- a/esp.ino +++ /dev/null @@ -1,257 +0,0 @@ -#include -#include -#include -#include - -const char * networkName = "OTEfd57e0"; -const char * networkPswd = "akup8407"; -const char * udpAddress = "192.168.1.2"; -const int udpPort = 6000; -SFEVL53L1X distanceSensor; -WiFiUDP udp; -boolean connected = false; - -#define MOTOR_PHASE_A 26 -#define MOTOR_PHASE_B 25 -#define MOTOR_PHASE_C 33 -#define MOTOR_PHASE_D 32 -#define HOMING_PIN 35 -unsigned long starttime; -unsigned long endtime; - -int count = 0; -int flag = 0; - -int circle = 2048 * 3.25; - -char *buffer1; -char *buffer2; -float distance; -float x,y; -int angle; -int max_angle,max_d,f_d; -int flag_lastTurn[2]; - -int step(); - -void setup() { - flag_lastTurn[0]=0; - flag_lastTurn[1]=1; - Wire.begin(); - - if (distanceSensor.begin() == 0) //Begin returns 0 on a good init - { - Serial.println("Sensor online!"); - } - //distanceSensor.setDistanceModeShort();//Sets it to max 1.3 works better with diffrent ambient lights - distanceSensor.setTimingBudgetInMs(120); - distanceSensor.setIntermeasurementPeriod(150); - connectToWiFi(networkName, networkPswd); - Serial2.begin(9600); - Serial.begin(9600); - Serial.println("Setup"); - pinMode(MOTOR_PHASE_A, OUTPUT); - pinMode(MOTOR_PHASE_B, OUTPUT); - pinMode(MOTOR_PHASE_C, OUTPUT); - pinMode(MOTOR_PHASE_D, OUTPUT); - pinMode(HOMING_PIN, INPUT); - starttime = millis(); - buffer1 = (char*) malloc((sizeof(char) + sizeof(unsigned long)) * sizeof(char) + 1); - buffer2 = (char*) malloc((sizeof(int)+(3*sizeof(float)) ) * sizeof(char) + 1); - while (digitalRead(HOMING_PIN) == HIGH)step(); - while (digitalRead(HOMING_PIN) == LOW) { - step(); - } - x=0; - y=0; -} - -void loop() { - sprintf (buffer1, "%c%lu",'F',1000); - Serial2.print(buffer1); - max_angle=-1; - max_d=-1; - f_d=0; - //if(count%circle==0)delay(5000); - for(count=0;countmax_d){ - max_d=distance; - max_angle=angle; - } - } - if(count==0){ - if(distance>18||distanceSensor.getRangeStatus()==4){ - f_d=1; - } - } - // Serial.print("ANGLE:"); - // Serial.print(angle); - // Serial.print(" DISTANCE:"); - // Serial.println(distance/10); - // Serial.println(buffer2); - } - - } - count = count % circle-1; - if(f_d!=1){ - if(max_angle>180){ - sprintf (buffer1, "%c%lu",'L',850); - Serial2.print(buffer1); - } - else - { - sprintf (buffer1, "%c%lu",'R',850); - Serial2.print(buffer1); - } - } - delay(1000); - /* - if(f_d!=1){ - if(max_angle>180){ - sprintf (buffer1, "%c%lu",'L',850); - Serial2.print(buffer1); - delay(500); - if(flag_lastTurn[0]==1){ - switch(flag_lastTurn[1]){ - case 1: - // x-=12.5; - break; - case 0: - x+=12.5; - break; - } - } - else - //y-=12.5; - sprintf (buffer1, "%c%lu",'F',1000); - Serial2.print(buffer1); - } - else{ - sprintf (buffer1, "%c%lu",'R',850); - Serial2.print(buffer1); - delay(500); - if(flag_lastTurn[0]==1){ - switch(flag_lastTurn[1]){ - case 1: - x+=12.5; - break; - case 0: - // x-=12.5; - break; - } - } - else - y+=12.5; - } - sprintf (buffer1, "%c%lu",'F',1000); - Serial2.print(buffer1); - } - else{ - sprintf (buffer1, "%c%lu",'F',1000); - Serial2.print(buffer1); - x+=12.5; - flag_lastTurn[0]=0; - flag_lastTurn[1]=0; - }*/ - -} - - -int step() { - static int count; - - switch (count) { - case 0: - { - digitalWrite(MOTOR_PHASE_D, HIGH); - digitalWrite(MOTOR_PHASE_C, LOW); - digitalWrite(MOTOR_PHASE_B, LOW); - digitalWrite(MOTOR_PHASE_A, LOW); - } - break; - case 1: - { - digitalWrite(MOTOR_PHASE_D, LOW); - digitalWrite(MOTOR_PHASE_C, HIGH); - digitalWrite(MOTOR_PHASE_B, LOW); - digitalWrite(MOTOR_PHASE_A, LOW); - } - break; - case 2: - { - digitalWrite(MOTOR_PHASE_D, LOW); - digitalWrite(MOTOR_PHASE_C, LOW); - digitalWrite(MOTOR_PHASE_B, HIGH); - digitalWrite(MOTOR_PHASE_A, LOW); - } - break; - case 3: - { - digitalWrite(MOTOR_PHASE_D, LOW); - digitalWrite(MOTOR_PHASE_C, LOW); - digitalWrite(MOTOR_PHASE_B, LOW); - digitalWrite(MOTOR_PHASE_A, HIGH); - } - break; - - - } - delay(8); - count++; - count = count % 4; - - return 1; -} - - - -void connectToWiFi(const char * ssid, const char * pwd) { - Serial.println("Connecting to WiFi network: " + String(ssid)); - - // delete old config - WiFi.disconnect(true); - //register event handler - WiFi.onEvent(WiFiEvent); - - //Initiate connection - WiFi.begin(ssid, pwd); - - Serial.println("Waiting for WIFI connection..."); -} - -//wifi event handler -void WiFiEvent(WiFiEvent_t event) { - switch (event) { - case SYSTEM_EVENT_STA_GOT_IP: - //When connected set - Serial.print("WiFi connected! IP address: "); - Serial.println(WiFi.localIP()); - //initializes the UDP state - //This initializes the transfer buffer - udp.begin(WiFi.localIP(), udpPort); - connected = true; - break; - case SYSTEM_EVENT_STA_DISCONNECTED: - Serial.println("WiFi lost connection"); - connected = false; - break; - default: break; - } -}