This is an autonomous robot sketch!! It is not scanning correctly.
#include <NewPing.h>
#include <Servo.h>
//Declare Servos
Servo scanservo; //Ping Sensor Servo
const int scanservopin = 4; // Pin number for scan servo
const int TRIGGER_PIN = 8; //Pin that the Ping sensor is attached to
const int ECHO_PIN = 9; //Pin that echo returns to
const int distancelimit = 20; //If something gets this many inches from
// the robot it stops and looks for where to go.
int scantime = 0;
int lastscantime = 0;
char sensorpos = 'L';
long oldtime = 0;
long timesinceturnedleft = 0;
long timesinceturnedright = 0;
const int A1A = 5; //(pwm) pin 5 to pin A-1A
const int A1B = 6; //(pwm) pin 6 to pin A-1B
const int B1A = 10; //(pwm) pin 10 to pin B-1A
const int B1B = 11; //(pwm) pin 11 to pin B-1B
byte speed = 240; //change this (0-255) to control motor speed
//Setup function. Runs once when Arduino is turned on or restarted
void setup()
{
scanservo.attach(scanservopin); // Attach the scan servo
Serial.begin(9600);
delay(2000); // wait two seconds
pinMode(A1A, OUTPUT); // set pins to output or input
pinMode(A1B, OUTPUT);
pinMode(B1A, OUTPUT);
pinMode(B1B, OUTPUT);
pinMode(9, INPUT_PULLUP);
pinMode(8, OUTPUT);
pinMode(ECHO_PIN, INPUT);
pinMode(TRIGGER_PIN, OUTPUT);
}
void loop(){
int leftdistance = 180;
int rightdistance = 180;
go(); // if nothing is wrong then go forward using go() function below.
if(millis()>oldtime+300){
if(sensorpos == 'L'){
leftdistance = ping();
sensorpos = 'R';
sensorpos = 'C';
}
else{
rightdistance = ping();
sensorpos = 'L';
}
oldtime = millis();
}
switch (sensorpos){
case 'L':
scanservo.write(20);
break;
case 'R':
scanservo.write(180);
break;
}
if(leftdistance<distancelimit){
if (millis()<timesinceturnedleft + 500){
backward(1000);
turnright(200);
}
else{
turnright(50);
}
timesinceturnedright = millis();
}
if(rightdistance<distancelimit){
if(millis()<timesinceturnedright + 500){
backward(1000);
turnleft(200);
}
else{
turnleft(50);
}
timesinceturnedleft = millis();
}
}
if(centerdistance<distancelimit){
moveBackward
}
else{
moveForward
}
int ping(){
long duration, inches, cm;
//Send Pulse
pinMode(TRIGGER_PIN, OUTPUT);
digitalWrite(TRIGGER_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGGER_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGGER_PIN, LOW);
//Read Echo
pinMode(ECHO_PIN, INPUT);
duration = pulseIn(ECHO_PIN, HIGH);
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print("Ping: ");
Serial.println(inches);
return round(inches);
}
void go(){
analogWrite(A1A, speed);
analogWrite(A1B, 0);
analogWrite(B1A, speed);
analogWrite(B1B, 0);
}
void turnleft(float t){
analogWrite(A1A, speed);
analogWrite(A1B, 0);
analogWrite(B1A, 0);
analogWrite(B1B, speed);
t=(t/120)*1000;
Serial.print("Turning left for this many seconds: ");
Serial.println(t);
delay(t);
}
void turnright(float t){
analogWrite(A1A, 0);
analogWrite(A1B, speed);
analogWrite(B1A, speed);
analogWrite(B1B, 0);
t=(t/120)*1000;
Serial.print("Turning right for this many seconds: ");
Serial.println(t);
delay(t);
}
void forward(int t){
analogWrite(A1A, speed);
analogWrite(A1B, 0);
analogWrite(B1A, speed);
analogWrite(B1B, 0);
delay(t);
}
void backward(int t){
analogWrite(A1A, 0);
analogWrite(A1B,speed);
analogWrite(B1A, 0);
analogWrite(B1B, speed);
delay(t);
}
void stopmotors(){
analogWrite(A1A, 0);
analogWrite(A1B, 0);
analogWrite(B1A, 0);
analogWrite(B1B, 0);
}
char scan(){
int lval, lcval, cval, rcval, rval;
int maximum = 0;
int choice;
scanservo.write(20); //Look left
delay(300);
lval = ping();
if(lval>maximum){
maximum = lval;
choice = 1;
}
scanservo.write(45); //Look left center
delay(300);
lcval = ping();
if(lcval>maximum){
maximum = lcval;
choice = 2;
}
scanservo.write(90); //Look center
delay(300);
cval = ping();
if(cval>maximum){
maximum = cval;
choice = 3;
}
scanservo.write(135); //Look right center
delay(300);
rcval = ping();
if(rcval>maximum){
maximum = rcval;
choice = 4;
}
scanservo.write(170); //Look right
delay(300);
rval = ping();
if(rval>maximum){
maximum = rval;
choice = 5;
}
if(maximum<=distancelimit){choice = 6;}
scanservo.write(88); //center scan servo
delay(300);
Serial.print("Choice: ");
Serial.println(choice);
return choice;
}
long microsecondsToInches(long microseconds){
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds){
return microseconds / 29 / 2;
}