Created
October 4, 2025 19:13
-
-
Save dubistweltmeister05/5a291dbbd815b0320a29932200297b01 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // Motor driver pins | |
| const int PWMA = 9; // Left motor PWM | |
| const int AIN1 = 5; // Left motor direction 1 | |
| const int AIN2 = 6; // Left motor direction 2 | |
| const int STBY = 8; // Standby pin | |
| const int BIN1 = 3; // Right motor direction 1 | |
| const int BIN2 = 11; // Right motor direction 2 | |
| const int PWMB = 10; // Right motor PWM | |
| const int D2 = 2; // Finish LED | |
| const int switchPin = 7; // On-Off toggle switch pin (used instead of DIP) | |
| // Sensor pins (6 analog + 1 digital center) | |
| const int sensorPins[6] = {A0, A1, A2, A3, A4, A5}; | |
| const int frontSensor = 12; | |
| // Calibration arrays | |
| int mini[6] = {1023, 1023, 1023, 1023, 1023, 1023}; | |
| int maxi[6] = {0, 0, 0, 0, 0, 0}; | |
| int threshold[6]; | |
| // PID parameters (adjust as needed) | |
| double kp = 0.07; | |
| double ki = 0.000; | |
| double kd = 0.0002; | |
| int lastError = 0; | |
| long integral = 0; | |
| const int maxSpeed = 190; | |
| const int minSpeed = 65; | |
| String path = ""; | |
| String str = ""; // For shortest path replay | |
| int finish = 0; | |
| int key = 0; // Controlled by on-off switch on switchPin | |
| int lsp = 0; | |
| int rsp = 0; | |
| double adjustment = 0; | |
| unsigned long lastLoopTime = 0; | |
| const unsigned long loopInterval = 10; // PID update diff ms | |
| unsigned long lastFinishTime = 0; | |
| const unsigned long finishCooldown = 600; // ms finish detection cooldown | |
| int spIndex = 0; // Shortest path step index | |
| // Variables for switch debounce | |
| bool lastSwitchState = HIGH; | |
| unsigned long lastDebounceTime = 0; | |
| const unsigned long debounceDelay = 50; | |
| void setup() { | |
| Serial.begin(9600); | |
| pinMode(switchPin, INPUT_PULLUP); | |
| pinMode(D2, OUTPUT); | |
| pinMode(PWMA, OUTPUT); | |
| pinMode(AIN1, OUTPUT); | |
| pinMode(AIN2, OUTPUT); | |
| pinMode(PWMB, OUTPUT); | |
| pinMode(BIN1, OUTPUT); | |
| pinMode(BIN2, OUTPUT); | |
| pinMode(STBY, OUTPUT); | |
| digitalWrite(STBY, HIGH); | |
| for (int i = 0; i < 6; i++) pinMode(sensorPins[i], INPUT); | |
| pinMode(frontSensor, INPUT); | |
| calibrateSensors(); | |
| } | |
| void calibrateSensors() { | |
| // Serial.println("Starting sensor calibration..."); | |
| delay(100); | |
| for (int t = 0; t < 200; t++) { | |
| analogWrite(PWMA, 100); | |
| analogWrite(PWMB, 100); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| digitalWrite(BIN1, LOW); | |
| delay(10); | |
| for (int i = 0; i < 6; i++) { | |
| int val = analogRead(sensorPins[i]); | |
| if (val > maxi[i]) maxi[i] = val; | |
| if (val < mini[i]) mini[i] = val; | |
| } | |
| } | |
| analogWrite(PWMA, 0); | |
| analogWrite(PWMB, 0); | |
| for (int i = 0; i < 6; i++) { | |
| threshold[i] = (mini[i] + maxi[i]) / 2; | |
| // Serial.print("Sensor "); | |
| // Serial.print(i); | |
| // Serial.print(" min="); | |
| // Serial.print(mini[i]); | |
| // Serial.print(" max="); | |
| // Serial.print(maxi[i]); | |
| // Serial.print(" th="); | |
| // Serial.println(threshold[i]); | |
| } | |
| // Serial.println("Calibration complete."); | |
| delay(1000); | |
| } | |
| // Path shortening helper | |
| String shortPath(String path, int index) { | |
| String a; | |
| if (path[index - 1] == 'L' && path[index + 1] == 'R') a = 'B'; | |
| else if (path[index - 1] == 'L' && path[index + 1] == 'S') a = 'R'; | |
| else if (path[index - 1] == 'R' && path[index + 1] == 'L') a = 'B'; | |
| else if (path[index - 1] == 'S' && path[index + 1] == 'L') a = 'R'; | |
| else if (path[index - 1] == 'S' && path[index + 1] == 'S') a = 'B'; | |
| else if (path[index - 1] == 'L' && path[index + 1] == 'L') a = 'S'; | |
| path = path.substring(0, index - 1) + a + path.substring(index + 2); | |
| return path; | |
| } | |
| void shortestPathPlay(String &str) { | |
| if (spIndex >= str.length()) return; | |
| char cmd = str[spIndex]; | |
| if (cmd == 'L') { | |
| leftturn(); | |
| spIndex++; | |
| // Serial.print(" L "); | |
| } | |
| else if (cmd == 'S') { | |
| unsigned long time = millis(); | |
| while (millis() - time <= 200) lineFollow(); | |
| while ((analogRead(sensorPins[3]) < threshold[3] || analogRead(sensorPins[2]) < threshold[2]) && | |
| (analogRead(sensorPins[0]) < threshold[0] || analogRead(sensorPins[5]) < threshold[5])) { | |
| lineFollow(); | |
| } | |
| spIndex++; | |
| // Serial.print(" S "); | |
| } | |
| else if (cmd == 'R') { | |
| rightturn(); | |
| spIndex++; | |
| // Serial.print(" R "); | |
| } | |
| // Serial.print(spIndex); | |
| // Serial.println(""); | |
| } | |
| void loop() { | |
| // Read and debounce on/off switch to set key | |
| int reading = digitalRead(switchPin); | |
| if (reading != lastSwitchState) { | |
| lastDebounceTime = millis(); | |
| } | |
| if ((millis() - lastDebounceTime) > debounceDelay) { | |
| if (reading == LOW) { // Switch is ON (pressed) | |
| key = 1; | |
| } else { | |
| key = 0; | |
| // Reset shortest path playback when switch off | |
| spIndex = 0; | |
| path = ""; | |
| str = ""; | |
| } | |
| } | |
| lastSwitchState = reading; | |
| unsigned long currentTime = millis(); | |
| if (currentTime - lastLoopTime < loopInterval) return; | |
| lastLoopTime = currentTime; | |
| if (!finish && !key) { | |
| if ((analogRead(sensorPins[0]) > 500 && analogRead(sensorPins[5]) > 500) && | |
| (digitalRead(frontSensor) == 0 || analogRead(sensorPins[2]) < 500 || analogRead(sensorPins[3]) < 500)) { | |
| lineFollow(); | |
| } | |
| else { | |
| lineFollow(); | |
| lsrb(); | |
| } | |
| } | |
| if (finish) { | |
| stopMotors(); | |
| digitalWrite(D2, HIGH); | |
| while (digitalRead(frontSensor) == 0) { | |
| delay(10); | |
| } | |
| } | |
| // Shortest path replay triggered by on-off switch (key) | |
| if (key == 1 && finish == 0) { | |
| static bool optimized = false; | |
| if (!optimized) { | |
| for (int i = 1; i < path.length() - 1; i++) { | |
| if (path[i] == 'B') { | |
| path = shortPath(path, i); | |
| i = max(1, i - 2); | |
| } | |
| } | |
| str = path; | |
| spIndex = 0; | |
| optimized = true; | |
| // Serial.print("Shortest Path: "); | |
| // Serial.println(str); | |
| } | |
| shortestPathPlay(str); | |
| } | |
| } | |
| void lineFollow() { | |
| digitalWrite(STBY, HIGH); | |
| int error = (analogRead(sensorPins[1]) + analogRead(sensorPins[2])) - (analogRead(sensorPins[3]) + analogRead(sensorPins[4])); | |
| integral += error; | |
| if (integral > 1000) integral = 1000; | |
| if (integral < -1000) integral = -1000; | |
| int derivative = error - lastError; | |
| lastError = error; | |
| adjustment = kp * error + ki * integral + kd * derivative; | |
| lsp = constrain(maxSpeed + adjustment, minSpeed, maxSpeed); | |
| rsp = constrain(maxSpeed - adjustment, minSpeed, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| analogWrite(PWMA, lsp); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| analogWrite(PWMB, rsp); | |
| static unsigned long finishStart = 0; | |
| if ((analogRead(sensorPins[0]) < threshold[0] && analogRead(sensorPins[5]) < threshold[5]) && | |
| (digitalRead(frontSensor) == 0 && analogRead(sensorPins[2]) < threshold[2] && | |
| analogRead(sensorPins[3]) < threshold[3] && analogRead(sensorPins[1]) < threshold[1])) { | |
| if (finishStart == 0) finishStart = millis(); | |
| if (millis() - finishStart >= 180 && millis() - lastFinishTime > finishCooldown) { | |
| finish = 1; | |
| lastFinishTime = millis(); | |
| finishStart = 0; | |
| } | |
| } | |
| else { | |
| finishStart = 0; | |
| } | |
| } | |
| void lsrb() { | |
| if (analogRead(sensorPins[0]) < threshold[0]) { | |
| leftturn(); | |
| path += "L"; | |
| } | |
| else if (digitalRead(frontSensor) == 0) { | |
| unsigned long startTime = millis(); | |
| while (millis() - startTime < 200) { | |
| lineFollow(); | |
| } | |
| while ((analogRead(sensorPins[3]) < threshold[3] || analogRead(sensorPins[2]) < threshold[2]) && | |
| (analogRead(sensorPins[0]) < threshold[0] || analogRead(sensorPins[5]) < threshold[5])) { | |
| lineFollow(); | |
| } | |
| path += "S"; | |
| } | |
| else if (analogRead(sensorPins[5]) < threshold[5]) { | |
| rightturn(); | |
| path += "R"; | |
| } | |
| else { | |
| uturn(); | |
| path += "B"; | |
| } | |
| } | |
| void stopMotors() { | |
| analogWrite(PWMA, 0); | |
| analogWrite(PWMB, 0); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, LOW); | |
| } | |
| void leftturn() { | |
| analogWrite(PWMA, 100); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| analogWrite(PWMB, maxSpeed); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(40); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMA, 100); | |
| analogWrite(PWMB, 180); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| } | |
| analogWrite(PWMA, 60); | |
| analogWrite(PWMB, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(25); | |
| } | |
| void rightturn() { | |
| analogWrite(PWMB, 100); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| analogWrite(PWMA, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| delay(40); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMB, 100); | |
| analogWrite(PWMA, 180); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| } | |
| analogWrite(PWMB, 60); | |
| analogWrite(PWMA, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(25); | |
| } | |
| void uturn() { | |
| analogWrite(PWMB, 80); | |
| analogWrite(PWMA, 80); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| delay(20); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMB, 180); | |
| analogWrite(PWMA, 180); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| } | |
| analogWrite(PWMB, 80); | |
| analogWrite(PWMA, 80); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(20); | |
| }// Motor driver pins | |
| const int PWMA = 9; // Left motor PWM | |
| const int AIN1 = 5; // Left motor direction 1 | |
| const int AIN2 = 6; // Left motor direction 2 | |
| const int STBY = 8; // Standby pin | |
| const int BIN1 = 3; // Right motor direction 1 | |
| const int BIN2 = 11; // Right motor direction 2 | |
| const int PWMB = 10; // Right motor PWM | |
| const int D2 = 2; // Finish LED | |
| const int switchPin = 7; // On-Off toggle switch pin (used instead of DIP) | |
| // Sensor pins (6 analog + 1 digital center) | |
| const int sensorPins[6] = {A0, A1, A2, A3, A4, A5}; | |
| const int frontSensor = 12; | |
| // Calibration arrays | |
| int mini[6] = {1023, 1023, 1023, 1023, 1023, 1023}; | |
| int maxi[6] = {0, 0, 0, 0, 0, 0}; | |
| int threshold[6]; | |
| // PID parameters (adjust as needed) | |
| double kp = 0.07; | |
| double ki = 0.000; | |
| double kd = 0.0002; | |
| int lastError = 0; | |
| long integral = 0; | |
| const int maxSpeed = 190; | |
| const int minSpeed = 65; | |
| String path = ""; | |
| String str = ""; // For shortest path replay | |
| int finish = 0; | |
| int key = 0; // Controlled by on-off switch on switchPin | |
| int lsp = 0; | |
| int rsp = 0; | |
| double adjustment = 0; | |
| unsigned long lastLoopTime = 0; | |
| const unsigned long loopInterval = 10; // PID update diff ms | |
| unsigned long lastFinishTime = 0; | |
| const unsigned long finishCooldown = 600; // ms finish detection cooldown | |
| int spIndex = 0; // Shortest path step index | |
| // Variables for switch debounce | |
| bool lastSwitchState = HIGH; | |
| unsigned long lastDebounceTime = 0; | |
| const unsigned long debounceDelay = 50; | |
| void setup() { | |
| Serial.begin(9600); | |
| pinMode(switchPin, INPUT_PULLUP); | |
| pinMode(D2, OUTPUT); | |
| pinMode(PWMA, OUTPUT); | |
| pinMode(AIN1, OUTPUT); | |
| pinMode(AIN2, OUTPUT); | |
| pinMode(PWMB, OUTPUT); | |
| pinMode(BIN1, OUTPUT); | |
| pinMode(BIN2, OUTPUT); | |
| pinMode(STBY, OUTPUT); | |
| digitalWrite(STBY, HIGH); | |
| for (int i = 0; i < 6; i++) pinMode(sensorPins[i], INPUT); | |
| pinMode(frontSensor, INPUT); | |
| calibrateSensors(); | |
| } | |
| void calibrateSensors() { | |
| // Serial.println("Starting sensor calibration..."); | |
| delay(100); | |
| for (int t = 0; t < 200; t++) { | |
| analogWrite(PWMA, 100); | |
| analogWrite(PWMB, 100); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| digitalWrite(BIN1, LOW); | |
| delay(10); | |
| for (int i = 0; i < 6; i++) { | |
| int val = analogRead(sensorPins[i]); | |
| if (val > maxi[i]) maxi[i] = val; | |
| if (val < mini[i]) mini[i] = val; | |
| } | |
| } | |
| analogWrite(PWMA, 0); | |
| analogWrite(PWMB, 0); | |
| for (int i = 0; i < 6; i++) { | |
| threshold[i] = (mini[i] + maxi[i]) / 2; | |
| // Serial.print("Sensor "); | |
| // Serial.print(i); | |
| // Serial.print(" min="); | |
| // Serial.print(mini[i]); | |
| // Serial.print(" max="); | |
| // Serial.print(maxi[i]); | |
| // Serial.print(" th="); | |
| // Serial.println(threshold[i]); | |
| } | |
| // Serial.println("Calibration complete."); | |
| delay(1000); | |
| } | |
| // Path shortening helper | |
| String shortPath(String path, int index) { | |
| String a; | |
| if (path[index - 1] == 'L' && path[index + 1] == 'R') a = 'B'; | |
| else if (path[index - 1] == 'L' && path[index + 1] == 'S') a = 'R'; | |
| else if (path[index - 1] == 'R' && path[index + 1] == 'L') a = 'B'; | |
| else if (path[index - 1] == 'S' && path[index + 1] == 'L') a = 'R'; | |
| else if (path[index - 1] == 'S' && path[index + 1] == 'S') a = 'B'; | |
| else if (path[index - 1] == 'L' && path[index + 1] == 'L') a = 'S'; | |
| path = path.substring(0, index - 1) + a + path.substring(index + 2); | |
| return path; | |
| } | |
| void shortestPathPlay(String &str) { | |
| if (spIndex >= str.length()) return; | |
| char cmd = str[spIndex]; | |
| if (cmd == 'L') { | |
| leftturn(); | |
| spIndex++; | |
| // Serial.print(" L "); | |
| } | |
| else if (cmd == 'S') { | |
| unsigned long time = millis(); | |
| while (millis() - time <= 200) lineFollow(); | |
| while ((analogRead(sensorPins[3]) < threshold[3] || analogRead(sensorPins[2]) < threshold[2]) && | |
| (analogRead(sensorPins[0]) < threshold[0] || analogRead(sensorPins[5]) < threshold[5])) { | |
| lineFollow(); | |
| } | |
| spIndex++; | |
| // Serial.print(" S "); | |
| } | |
| else if (cmd == 'R') { | |
| rightturn(); | |
| spIndex++; | |
| // Serial.print(" R "); | |
| } | |
| // Serial.print(spIndex); | |
| // Serial.println(""); | |
| } | |
| void loop() { | |
| // Read and debounce on/off switch to set key | |
| int reading = digitalRead(switchPin); | |
| if (reading != lastSwitchState) { | |
| lastDebounceTime = millis(); | |
| } | |
| if ((millis() - lastDebounceTime) > debounceDelay) { | |
| if (reading == LOW) { // Switch is ON (pressed) | |
| key = 1; | |
| } else { | |
| key = 0; | |
| // Reset shortest path playback when switch off | |
| spIndex = 0; | |
| path = ""; | |
| str = ""; | |
| } | |
| } | |
| lastSwitchState = reading; | |
| unsigned long currentTime = millis(); | |
| if (currentTime - lastLoopTime < loopInterval) return; | |
| lastLoopTime = currentTime; | |
| if (!finish && !key) { | |
| if ((analogRead(sensorPins[0]) > 500 && analogRead(sensorPins[5]) > 500) && | |
| (digitalRead(frontSensor) == 0 || analogRead(sensorPins[2]) < 500 || analogRead(sensorPins[3]) < 500)) { | |
| lineFollow(); | |
| } | |
| else { | |
| lineFollow(); | |
| lsrb(); | |
| } | |
| } | |
| if (finish) { | |
| stopMotors(); | |
| digitalWrite(D2, HIGH); | |
| while (digitalRead(frontSensor) == 0) { | |
| delay(10); | |
| } | |
| } | |
| // Shortest path replay triggered by on-off switch (key) | |
| if (key == 1 && finish == 0) { | |
| static bool optimized = false; | |
| if (!optimized) { | |
| for (int i = 1; i < path.length() - 1; i++) { | |
| if (path[i] == 'B') { | |
| path = shortPath(path, i); | |
| i = max(1, i - 2); | |
| } | |
| } | |
| str = path; | |
| spIndex = 0; | |
| optimized = true; | |
| // Serial.print("Shortest Path: "); | |
| // Serial.println(str); | |
| } | |
| shortestPathPlay(str); | |
| } | |
| } | |
| void lineFollow() { | |
| digitalWrite(STBY, HIGH); | |
| int error = (analogRead(sensorPins[1]) + analogRead(sensorPins[2])) - (analogRead(sensorPins[3]) + analogRead(sensorPins[4])); | |
| integral += error; | |
| if (integral > 1000) integral = 1000; | |
| if (integral < -1000) integral = -1000; | |
| int derivative = error - lastError; | |
| lastError = error; | |
| adjustment = kp * error + ki * integral + kd * derivative; | |
| lsp = constrain(maxSpeed + adjustment, minSpeed, maxSpeed); | |
| rsp = constrain(maxSpeed - adjustment, minSpeed, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| analogWrite(PWMA, lsp); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| analogWrite(PWMB, rsp); | |
| static unsigned long finishStart = 0; | |
| if ((analogRead(sensorPins[0]) < threshold[0] && analogRead(sensorPins[5]) < threshold[5]) && | |
| (digitalRead(frontSensor) == 0 && analogRead(sensorPins[2]) < threshold[2] && | |
| analogRead(sensorPins[3]) < threshold[3] && analogRead(sensorPins[1]) < threshold[1])) { | |
| if (finishStart == 0) finishStart = millis(); | |
| if (millis() - finishStart >= 180 && millis() - lastFinishTime > finishCooldown) { | |
| finish = 1; | |
| lastFinishTime = millis(); | |
| finishStart = 0; | |
| } | |
| } | |
| else { | |
| finishStart = 0; | |
| } | |
| } | |
| void lsrb() { | |
| if (analogRead(sensorPins[0]) < threshold[0]) { | |
| leftturn(); | |
| path += "L"; | |
| } | |
| else if (digitalRead(frontSensor) == 0) { | |
| unsigned long startTime = millis(); | |
| while (millis() - startTime < 200) { | |
| lineFollow(); | |
| } | |
| while ((analogRead(sensorPins[3]) < threshold[3] || analogRead(sensorPins[2]) < threshold[2]) && | |
| (analogRead(sensorPins[0]) < threshold[0] || analogRead(sensorPins[5]) < threshold[5])) { | |
| lineFollow(); | |
| } | |
| path += "S"; | |
| } | |
| else if (analogRead(sensorPins[5]) < threshold[5]) { | |
| rightturn(); | |
| path += "R"; | |
| } | |
| else { | |
| uturn(); | |
| path += "B"; | |
| } | |
| } | |
| void stopMotors() { | |
| analogWrite(PWMA, 0); | |
| analogWrite(PWMB, 0); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, LOW); | |
| } | |
| void leftturn() { | |
| analogWrite(PWMA, 100); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| analogWrite(PWMB, maxSpeed); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(40); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMA, 100); | |
| analogWrite(PWMB, 180); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| } | |
| analogWrite(PWMA, 60); | |
| analogWrite(PWMB, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(25); | |
| } | |
| void rightturn() { | |
| analogWrite(PWMB, 100); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| analogWrite(PWMA, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| delay(40); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMB, 100); | |
| analogWrite(PWMA, 180); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| } | |
| analogWrite(PWMB, 60); | |
| analogWrite(PWMA, maxSpeed); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(25); | |
| } | |
| void uturn() { | |
| analogWrite(PWMB, 80); | |
| analogWrite(PWMA, 80); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| delay(20); | |
| unsigned long startTime = millis(); | |
| while (digitalRead(frontSensor) == 1 && millis() - startTime < 1000) { | |
| analogWrite(PWMB, 180); | |
| analogWrite(PWMA, 180); | |
| digitalWrite(AIN1, HIGH); | |
| digitalWrite(AIN2, LOW); | |
| digitalWrite(BIN1, LOW); | |
| digitalWrite(BIN2, HIGH); | |
| } | |
| analogWrite(PWMB, 80); | |
| analogWrite(PWMA, 80); | |
| digitalWrite(AIN1, LOW); | |
| digitalWrite(AIN2, HIGH); | |
| digitalWrite(BIN1, HIGH); | |
| digitalWrite(BIN2, LOW); | |
| delay(20); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment