This is what I'm trying now
I don't think Its having it though
#include <Servo.h>
Servo scanservo; //Ping Sensor Servo
#define txPin 4
#define rxPin A4
const int motor1 = 12;
const int motor2 = 9;
const int steer1 = 13;
const int steer2 = 8;
const int PWM_M = 3;
int up = 0;
int down = 0;
int left = 0;
int right = 0;
const int a = 800;
const int turntime = 500; //Number of milliseconds to turn when turning
const int pingPin = 6; //Pin that the Ping sensor is attached to.
const int echoPin = 5; //Pin that the Ping sensor is attached to.
const int scanservopin = 7; // Pin number for scan servo
const int distancelimit = 30;
long previousMillis = 0;
long interval = 1000;
void setup()
{
Serial.begin (9600);
scanservo.attach(scanservopin); // Attach the scan servo
pinMode(echoPin, INPUT);
pinMode(rxPin, INPUT);
pinMode(txPin, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(steer1, OUTPUT);
pinMode(steer2, OUTPUT);
digitalWrite(motor1,LOW);
digitalWrite(motor2,LOW);
digitalWrite(steer1,LOW);
digitalWrite(steer2,LOW);
}
void loop(){
left = analogRead(A0);
Serial.print("left: ");
Serial.println(left);
right = analogRead(A1);
Serial.print("right: ");
Serial.println(right);
down = analogRead(A3);
Serial.print("down: ");
Serial.println(down);
up = analogRead(A2);
Serial.print("up: ");
Serial.println(up);
int sensor = pingforward();
Serial.print("sensor: ");
Serial.println(sensor);
int leftscanval = scanleft();
Serial.print("leftscanval: ");
Serial.println(leftscanval);
int rightscanval = scanright();
Serial.print("rightscanval: ");
Serial.println(rightscanval);
int centerscanval = scancenter();
Serial.print("centrescanval: ");
Serial.println(centerscanval);
if (down < 400 && centerscanval > distancelimit){
backward(1);
}
if (up < 400 && sensor > 150){
forward(1);
}
if (left > 400 && leftscanval > distancelimit){
turnleft(1);
}
if (right > 400 && rightscanval > distancelimit){
turnright(1);
}
if (down > 400 && up > 400 && left < 400 && right < 400){
stopmotors(1);
}
}
void turnleft(int t){
digitalWrite (13, HIGH);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delayMicroseconds(t);
}
void turnright(int t){
digitalWrite (13, LOW);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delayMicroseconds(t);
}
void stopmotors(int t){
digitalWrite (9, LOW);
digitalWrite (8, LOW);
digitalWrite (3, LOW);
digitalWrite (11, LOW);
delayMicroseconds(t);
}
void forward(int t){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(PWM_M, HIGH);
delay(t);
}
void backward(int t){
digitalWrite (9, LOW);
digitalWrite (12, LOW);
digitalWrite(PWM_M, HIGH);
delay(t);
}
int ping(){
long distance, duration;
//Send Pulse
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//Read Echo
pinMode(echoPin, INPUT);
duration = pulseIn(echoPin, HIGH);
distance =(duration/2)*0.0341;
return distance;
}
int pingforward(){
long senseforward;
for (int i = 0; i < 8; i++) {
digitalWrite(txPin, LOW); // this sends out a short pulse for 10ms
delayMicroseconds(9);
digitalWrite(txPin, HIGH);
delayMicroseconds(9);
}
digitalWrite(txPin, LOW);
delay(2);
senseforward = analogRead(A4); // us the ping() function to see if anything is ahead.
return senseforward;
}
int scanleft(){
long leftscanval;
scanservo.write(30);
unsigned long currentMillis = millis();
if(currentMillis - previousMillis > interval) {
previousMillis = currentMillis;
leftscanval = ping();
}
return leftscanval;
}
int scanright(){
long rightscanval;
scanservo.write(150);
unsigned long currentMillis = millis();
if(currentMillis - previousMillis > interval) {
previousMillis = currentMillis;
rightscanval = ping();
}
return rightscanval;
}
int scancenter(){
long centerscanval;
scanservo.write(90);
unsigned long currentMillis = millis();
if(currentMillis - previousMillis > interval) {
previousMillis = currentMillis;
centerscanval = ping();
}
return centerscanval;
}