From 7f94677b1b32ca31180428a4a236bb90eff47cc4 Mon Sep 17 00:00:00 2001 From: Gab_S Date: Wed, 22 Jan 2020 16:16:48 +0000 Subject: [PATCH] Upload files to '' --- esp.ino | 257 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 257 insertions(+) create mode 100644 esp.ino diff --git a/esp.ino b/esp.ino new file mode 100644 index 0000000..57b5922 --- /dev/null +++ b/esp.ino @@ -0,0 +1,257 @@ +#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; + } +}