I wrote this code so my bot can approach an object, look left then right and turn toward the direction with the most space. Pretty new to code writing so im sure its not written too well. i uploaded it and all my bot does is look left then right and always turn left. Any Help please??? or any links to similar codes i can use for guidance? any help would be greatly appreciated. thanks
#include <Servo.h>
Servo servo;
int pingpin = 13;
int leftmotor1 = 6;
int leftmotor2 = 5;
int leftenable = 10;
int rightmotor1 = 4;
int rightmotor2 = 3;
int rightenable = 11;
int turn_time = 320;
int time = 1200;
void setup() {
servo.attach(12);
Serial.begin(115200);
pinMode(pingpin,OUTPUT);
pinMode(pingpin,INPUT);
pinMode(leftmotor1, OUTPUT);
pinMode(leftmotor2, OUTPUT);
pinMode(leftenable, OUTPUT);
pinMode(rightmotor1, OUTPUT);
pinMode(rightmotor2, OUTPUT);
pinMode(rightenable, OUTPUT);
analogWrite(leftenable, 255);
analogWrite(rightenable, 255);
}
void loop() {
int cm = distance();
Serial.println(cm);
if (cm < 280){
off();
measure();}
else {
front();
}
}
int off () {
digitalWrite(leftmotor1, LOW);
digitalWrite(leftmotor2, LOW);
digitalWrite(rightmotor1, LOW);
digitalWrite(rightmotor2, LOW);}
int measure () {
int a = servo_left();
int b = servo_front();
int c = servo_right();
if (a > c) {
Serial.println("Left is Greater");
left();}
if (c > a) {
Serial.println("Right is Greater");
right();}
if (c == a) {
Serial.println("Sides Equal");
back();}
}
int distance() {
long duration, inches, cm;
// The Ping is triggered by a HIGH pulse of 2 or more microseconds.
// We give a short LOW pulse beforehand to ensure a clean HIGH pulse.
pinMode(pingpin, OUTPUT);
digitalWrite(pingpin, LOW);
delayMicroseconds(2);
digitalWrite(pingpin, HIGH);
delayMicroseconds(5);
digitalWrite(pingpin, LOW);
// The same pin is used to read the signal from the Ping: a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingpin, INPUT);
duration = pulseIn(pingpin, HIGH);
// Convert the time into a distance.
cm = microsecondsToCentimeters(duration);
return(cm);
}
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
return microseconds / 29 / 2;
}
int servo_front () {
servo.write(90);
delay(time);
int front = distance();
return(front);}
int servo_right (){
servo.write(179);
delay(time);
int right = distance();
return(right);}
int servo_left (){
servo.write(0);
delay(time);
int left = distance();
return(left);}
int front () {
off();
digitalWrite(leftmotor2, HIGH);
digitalWrite(rightmotor2,HIGH);}
int right () {
off();
digitalWrite(leftmotor1, HIGH);
digitalWrite(rightmotor1, HIGH);}
int left () {
off();
digitalWrite(leftmotor1, HIGH);
digitalWrite(rightmotor2, HIGH);
delay(turn_time);}
int back () {
off();
digitalWrite(leftmotor1, HIGH);
digitalWrite(rightmotor2, HIGH);
delay(650);}