Coding Engineers

Code for distance and rotation Calibration – adjust speed as needed. during the 10000ms delay record the distance traveled or degrees at whatever the chosen speed is and put into a spreadsheet.

#include
Servo cServo;

// Ultrasonic sensor for the sensor attached to the servo
int tEcho = A4;
int tTrig = A5;

// Ultrasonic sensor for the left side of the vehicle
int lTrig = 30;
int lEcho = 31;

// Ultrasoinc sensor for the right side of the vehicle
int rTrig = 38;
int rEcho = 39;

// Pins for the motor control
int ENA = 5;
int ENB = 6;
int IN1 = 7;
int IN2 = 8;
int IN3 = 9;
int IN4 = 11;

int carSpeed = 250;

float rightDistance = 0, leftDistance = 0, middleDistance = 0;

void forward() {
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
Serial.println(“Forward”);
}

void back() {
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
Serial.println(“Back”);
}

void left() {
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
Serial.println(“Left”);
}

void right() {
analogWrite(ENA, carSpeed);
analogWrite(ENB, carSpeed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
Serial.println(“Right”);
}

void stop() {
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
Serial.println(“Stop!”);
}

//Ultrasonic distance measurement Sub function
int Distance_test(int echoPin, int trigPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
float Fdistance = pulseIn(echoPin, HIGH);
Fdistance = (Fdistance / 2) * 0.0344;
return (float)Fdistance;
}

void setup() {
cServo.attach(3); // attach servo on pin 3 to servo object
Serial.begin(9600);
pinMode(tEcho, INPUT);
pinMode(tTrig, OUTPUT);
pinMode(lEcho, INPUT);
pinMode(lTrig, OUTPUT);
pinMode(rEcho, INPUT);
pinMode(rTrig, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
stop();
}

void loop() {
forward();
delay(500);
stop();
delay(10000);
forward();
delay(1000);
stop();
delay(10000);
forward();
delay(2000);
stop();
delay(10000);
left();
delay(360);
stop();
delay(10000);
left();
delay(180);
stop();
delay(10000);
right();
delay(360);
stop();
delay(10000);
right();
delay(180);
stop();
delay(10000);
/*
cServo.write(90); //setservo position according to scaled value
leftDistance = Distance_test(lEcho, lTrig);
middleDistance = Distance_test(tEcho, tTrig);
rightDistance = Distance_test(rEcho, rTrig);

Serial.print(“\nL :”);
Serial.print(leftDistance);
Serial.print(“\nM: “);
Serial.print(middleDistance);
Serial.print(“\nR: “);
Serial.println(rightDistance);

// Checks to see if the vehicle is too close to the wall
if (leftDistance <= 20 || middleDistance <= 20 || rightDistance <= 20) {
carSpeed = 1000;
stop();
delay(500);
cServo.write(10);
delay(1000);
rightDistance = Distance_test(rEcho, rTrig);

delay(500);
cServo.write(90);
delay(1000);
cServo.write(180);
delay(1000);
leftDistance = Distance_test(lEcho, lTrig);

delay(500);
cServo.write(90);
delay(1000);
// Checks to see what way is longer then goes that way
if (rightDistance < leftDistance) { left(); delay(360); carSpeed = 100; } else if (rightDistance > leftDistance) {
right();
delay(360);
carSpeed = 100;
}

else {
forward();
}
}
else {
forward();
}
*/
}