ok so heres the beef, all my sensors are reading correctly, but on my left front when the sensor detects an object within range, the wheel just twitches, while right sensor steers away as it should, my ESC values are currently disabled for steering debugging
can anyone point out where it i am screwing up the steering on the left side?
#include <Servo.h>
#include "ESC.h"
#define trigPin 13
#define echoPin 12
#define trigPin1 11
#define echoPin1 10
#define trigPin2 9
#define echoPin2 8
Servo myservo;
ESC myESC (6, 1000, 2000, 500);
int val;
int val_rev;
void setup() {
Serial.begin (9600);
myESC.arm();
myservo.attach(3);
myservo.write(100);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
val = 0;
val = map(val, 0, 1023, 1000, 2000);
val_rev = 0;
val_rev = map(val_rev, 0, 1023, 1000, 2000);
delay(5000);
}
void loop() {
long duration1, distance1;
long duration2, distance2;
long duration, distance;
//center
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
delay(50);
//right
digitalWrite(trigPin1, LOW);
delayMicroseconds(2);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
distance1 = (duration1/2) / 29.1;
delay(50);
//left
digitalWrite(trigPin2, LOW);
delayMicroseconds(2);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
distance2 = (duration2/2) / 29.1;
delay(50);
Serial.print(distance);
Serial.println(" cm");
Serial.print(distance1);
Serial.println(" cm");
Serial.print(distance2);
Serial.println(" cm");
delay(500);
if (distance >= 10){
myESC.speed(val);
delay(15);
}
if (distance < 10){
myESC.speed(val_rev);
delay(15);
}
if (distance1 >= 10){
myservo.write(100);
delay(15);
}
if ((distance1 < 15) && (distance2 > 15)){
myservo.write(10);
delay(15);
}
if (distance2 >= 15){
myservo.write(100);
delay(15);
}
if ((distance2 < 15)&&(distance1>15)){
myservo.write(170);
delay(15);
}
}