Testing sensor
Motor Testing
Power Testing
Sensor Motor Connection Testing
Prototype All Together Without Battery
#include <AFMotor.h>
#include <NewPing.h>
#include <Servo.h>
#define TRIG_PIN A0
#define ECHO_PIN A1
#define MAX_DISTANCE 100
#define MAX_SPEED 190
#define STOP_DISTANCE 5
#define SERVO_PIN 10
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR34_1KHZ);
AF_DCMotor motor4(4, MOTOR34_1KHZ);
Servo myservo;
int currentAngle = 90;
int initialDistance = 0;
unsigned long forwardStartTime = 0;
unsigned long millisAtStart =0;
void setup() {
Serial.begin( 9600 );
myservo.attach(SERVO_PIN);
myservo.write(currentAngle);
delay(2000);
motor1.setSpeed(MAX_SPEED);
motor2.setSpeed(MAX_SPEED);
motor3.setSpeed(MAX_SPEED);
motor4.setSpeed(MAX_SPEED);
initialDistance = readPing();
}
void loop() {
int distanceR = 0;
int distanceL = 0;
// initialDistance = readPing();
delay(40);
int distance = readPing();
if (distance <= STOP_DISTANCE) {
delay(100);
if (initialDistance == 0) {
moveBackward(16); // You can set the desired distance to move backward
} else {
moveBackward(initialDistance);
}
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
delay(300);
moveStop();
delay(200);
distanceR = lookRight();
delay(200);
distanceL = lookLeft();
delay(200);
if (distanceR >= distanceL) {
turnRight();
moveStop();
} else {
turnLeft();
moveStop();
}
} else {
moveForward();
}
distance = readPing();
}
int lookRight() {
myservo.write(50);
delay(500);
int distance = readPing();
delay(100);
myservo.write(115);
return distance;
}
int lookLeft() {
myservo.write(170);
delay(500);
int distance = readPing();
delay(100);
myservo.write(115);
return distance;
}
int readPing() {
delay(70);
int cm = sonar.ping_cm();
if (cm == 0) {
cm = MAX_DISTANCE;
}
return cm;
}
void moveStop() {
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
void moveForward() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
// unsigned long millisAtStart = millis();
forwardStartTime = millis();
Serial.println(“Forward start time: ” + String(forwardStartTime));
}
void moveBackward(int distance) {
Serial.println(“end”+String(millisAtStart));
// int initialDistance = readPing();
int traveledDistance = 0;
unsigned long startTime = millis(); // Record the start time
// Initialize millis() to zero before entering the loop
while (traveledDistance < distance) {
int currentDistance = readPing();
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
// current time- the time when backward function start > = the time when forward funciton end -the time when backward function end
if (millis() – startTime>= forwardStartTime-millisAtStart ) {
// Obstacle detected during backward movement, stop moving backward
millisAtStart = millis();
break;
}
// else if(millis() – startTime>= startTime – forwardStartTime){
// break;
// }
// Keep moving backward
traveledDistance = initialDistance – currentDistance;
// Check for a timeout condition (e.g., 5 seconds)
// if (millis() – startTime>= startTime – forwardStartTime) {
// //forwardStartTime =0;
// break;
// }
Serial.println(“current” + String(millis()));
Serial.println(startTime);
Serial.println(forwardStartTime);
Serial.println(millisAtStart);
Serial.println(“distance” + String(distance));
Serial.println(“current” + String(currentDistance));
}
motor1.run(RELEASE);
motor2.run(RELEASE);
motor3.run(RELEASE);
motor4.run(RELEASE);
}
void turnLeft() {
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delay(500);
moveStop();
}
void turnRight() {
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
delay(500);
moveStop();
}