Hello,
I am taking a remote control car and hacking it so it has an collision avoidance system, so I can drive it normally but when an object is in the way, the arduino avoidance system will take over and take evasive actions. I plan to do this by taking the output from the car circuit and taking it to the arduino which then can process it and decide on what action to make (with outputs then going form the arduino to the motors)
So i'm starting to form the code for this and would like to know if i'm going about it in the right fashion.
This is the part of the code that takes what comes from the car circuitry outputs and decides whether to forward that signal to the motors (if not object is in the way) or take evasive actions:
void loop(){
go(); // if nothing is wrong the go forward using go() function below.
int distance = ping(); // us the ping() function to see if anything is ahead.
drivevalue1 = analogRead(drivein1);
if(drivevalue1 > a && distance > distancelimit){
forward(600);
return;
}
drivevalue2 = analogRead(drivein2);
if(drivevalue2 > a && distance > distancelimit){
backward(600);
return;
}
steervalue1 = analogRead(steerin1);
if(steervalue1 > a && distance > distancelimit){
turnleft(600);
return;
}
steervalue2 = analogRead(steerin2);
if(steervalue2 > a && distance > distancelimit){
backward(600);
return;
}
Full code below:
#include <Servo.h>
#include<AFMotor.h>
//Declare Servos
Servo scanservo; //Ping Sensor Servo
const int motor1 = 12;
const int motor2 = 9;
const int steer1 = 13;
const int steer2 = 8;
const int a = 800;
const int turntime = 500; //Number of milliseconds to turn when turning
const int pingPin = 4; //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 = 10; //If something gets this many inched from
// the robot it stops and looks for where to go.
int steervalue1 = 0;
int steervalue2 = 0;
int drivevalue1 = 0;
int drivevalue2 = 0;
int steerin1 = 14;
int steerin2 = 15;
int drivein1 = 16;
int drivein2 = 17;
//Setup function. Runs once when Arduino is turned on or restarted
void setup()
{
Serial.begin (9600);
scanservo.attach(scanservopin); // Attach the scan servo
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(steer1, OUTPUT);
pinMode(steer2, OUTPUT);
pinMode(drivein1, INPUT);
pinMode(drivein2, INPUT);
pinMode(steerin1, INPUT);
pinMode(steerin2, INPUT);
//Variable inicialization
digitalWrite(motor1,LOW);
digitalWrite(motor2,LOW);
digitalWrite(steer1,LOW);
digitalWrite(steer2,LOW);
delay(2000); // wait two seconds
}
void loop(){
go(); // if nothing is wrong the go forward using go() function below.
int distance = ping(); // us the ping() function to see if anything is ahead.
drivevalue1 = analogRead(drivein1);
if(drivevalue1 > a && distance > distancelimit){
forward(600);
return;
}
drivevalue2 = analogRead(drivein2);
if(drivevalue2 > a && distance > distancelimit){
backward(600);
return;
}
steervalue1 = analogRead(steerin1);
if(steervalue1 > a && distance > distancelimit){
turnleft(600);
return;
}
steervalue2 = analogRead(steerin2);
if(steervalue2 > a && distance > distancelimit){
backward(600);
return;
}
if (distance < distancelimit){
stopmotors(); // If something is ahead, stop the motors.
char turndirection = scan(); //Decide which direction to turn.
switch (turndirection){
case 'l':
turnleft(turntime);
break;
case 'r':
turnright(turntime);
break;
case 's':
turnleft(turntime);
break;
}
}
}
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;
Serial.print("Ping: ");
Serial.println(distance);
}
void go(){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(3, HIGH);
delay(600);
}
void turnleft(int t){
digitalWrite (13, HIGH);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delay(t);
}
void turnright(int t){
digitalWrite (13, LOW);
digitalWrite (8, LOW);
digitalWrite (11, HIGH);
delay(t);
}
void forward(int t){
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
analogWrite (3, HIGH);
delay(600);
}
void backward(int t){
digitalWrite (9, LOW);
digitalWrite (12, LOW);
analogWrite (3, HIGH);
delay(t);
}
void stopmotors(){
digitalWrite (9, HIGH);
digitalWrite (8, HIGH);
digitalWrite (3, HIGH);
digitalWrite (11, HIGH);
delay(600);
}
char scan(){
int leftscanval, centerscanval, rightscanval;
char choice;
//Look left
scanservo.write(30);
delay(300);
leftscanval = ping();
//Look right
scanservo.write(150);
delay(1000);
rightscanval = ping();
//center scan servo
scanservo.write(88);
if (leftscanval>rightscanval && leftscanval>centerscanval){
choice = 'l';
}
else if (rightscanval>leftscanval && rightscanval>centerscanval){
choice = 'r';
}
else{
choice = 's';
}
Serial.print("Choice: ");
Serial.println(choice);
return choice;
}