#include #include #include #define TRIG_PIN A4 #define ECHO_PIN A5 #define MAX_DISTANCE_POSSIBLE 4000 #define MAX_SPEED 150 // #define MOTORS_CALIBRATION_OFFSET 3 #define COLL_DIST 10 #define TURN_DIST COLL_DIST+03 #define LedPin 3 #define Led2Pin 2 NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE_POSSIBLE); AF_DCMotor leftMotor(4, MOTOR12_8KHZ); AF_DCMotor rightMotor(1, MOTOR12_8KHZ); Servo neckControllerServoMotor; int pos = 0; int maxDist = 0; int maxAngle = 0; int maxRight = 0; int maxLeft = 0; int maxFront = 0; int course = 0; int curDist = 0; String motorSet = ""; int speedSet = 0; int Led = 3; int Led2 = 2; void setup() { neckControllerServoMotor.attach(10); neckControllerServoMotor.write(90); delay(2000); checkPath(); motorSet = "FORWARD"; neckControllerServoMotor.write(90); moveForward(); pinMode(Led, OUTPUT); pinMode(Led2, OUTPUT); } void loop() { checkForward(); checkPath(); } void checkPath() { int curLeft = 0; int curFront = 0; int curRight = 0; int curDist = 0; neckControllerServoMotor.write(144); delay(100); for (pos = 134; pos >= 02; pos -= 26) { neckControllerServoMotor.write(pos); delay(200); checkForward(); curDist = readPing(); if (curDist < COLL_DIST) { checkCourse(); break; } if (curDist < TURN_DIST) { changePath(); } if (curDist > curDist) { maxAngle = pos; } if (curDist > TURN_DIST) { changePath(); } if (curDist < curDist) { maxAngle = pos; } if (pos == 90 && curDist > curFront) { curFront = curDist; } if (pos < 90 && curDist < curRight) { curLeft = curDist; } if (pos > 90 && curDist > curLeft) { curRight = curDist; } } maxLeft = curLeft; maxRight = curRight; maxFront = curFront; } void setCourse() { if (maxAngle < 90) { turnRight(); } if (maxAngle > 90) { turnLeft(); } maxLeft = 0; maxRight = 0; maxFront = 0; } void checkCourse() { moveBackward(); delay(700); moveStop(); setCourse(); } void changePath() { if (pos < 90) { lookLeft(); } if (pos > 90) { lookRight(); } } int readPing() { delay(200); unsigned int uS = sonar.ping(); int cm = uS / US_ROUNDTRIP_CM; return cm; } void checkForward() { if (motorSet == "FORWARD") { leftMotor.run(FORWARD); rightMotor.run(FORWARD); digitalWrite(Led, HIGH); digitalWrite(Led2, LOW); } } void checkBackward() { if (motorSet == "BACKWARD") { leftMotor.run(BACKWARD); rightMotor.run(BACKWARD); digitalWrite(Led, LOW); digitalWrite(Led2, HIGH); } } void moveStop() { leftMotor.run(RELEASE); rightMotor.run(RELEASE); } void moveForward() { motorSet = "FORWARD"; leftMotor.run(FORWARD); rightMotor.run(FORWARD); for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 6) { leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET); rightMotor.setSpeed(speedSet+ MOTORS_CALIBRATION_OFFSET); delay(10); } } void moveBackward() { motorSet = "BACKWARD"; leftMotor.run(BACKWARD); rightMotor.run(BACKWARD); for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 8) { leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET); rightMotor.setSpeed(speedSet+ MOTORS_CALIBRATION_OFFSET); delay(8); } } void turnRight() { motorSet = "RIGHT"; leftMotor.run(FORWARD); rightMotor.run(BACKWARD); delay(400); motorSet = "FORWARD"; leftMotor.run(FORWARD); rightMotor.run(FORWARD); } void turnLeft() { motorSet = "LEFT"; leftMotor.run(BACKWARD); rightMotor.run(FORWARD); delay(400); motorSet = "FORWARD"; leftMotor.run(FORWARD); rightMotor.run(FORWARD); } void lookRight() { rightMotor.run(FORWARD); delay(400); rightMotor.run(BACKWARD); } void lookLeft() { leftMotor.run(BACKWARD); delay(400); leftMotor.run(FORWARD); }