Hello,
I have a project at hand which is to hack a toy car. I removed the circuit, and replaced it with L298N motor driver, so as to be able to control the speed of the toy car with PWM signals.
I also want it to avoid obstacles, so I got an idea of using two Ultrasonic sensors [Parallax PING)))], one on the left and the other on the right.
My toy isn't responding to the go_forward command and the go_reverse command (these two are PWM commands). Please help me out and see if there are wrong coding in the codes I have. The code is given below.
#include <NewPing.h>
/*
* This code is to avoid obstacles encountered on the left or right.
* If there is an object on the left (Robot goes to the right),
* and if there is an object on the right (Robot goes to the left)
*/
// PING))) on the right
const int pingPin1 = 12; // this pin serves for triggering and receiving
//sonic pulse for the PING))) sensor on the right
#define MAX_DISTANCE 400
NewPing DistanceSensor1 (pingPin1, pingPin1, MAX_DISTANCE);
long duration1; // how long it takes for the sound to rebound
unsigned int cm1;
// PING))) on the left
const int pingPin2 = 4;
#define MAX_DISTANCE 400
NewPing DistanceSensor2 (pingPin2, pingPin2, MAX_DISTANCE);
long duration2;
unsigned int cm2;
// sensing or not
boolean right_wall_detected = false;
boolean left_wall_detected = false;
// which way to turn
boolean Right = true;
int enA = 11;
int forward_in1 = 10;
int reverse_in2 = 9;
int enB = 6;
int right_in3 = 8;
int left_in4 = 7;
void go_forward();
void go_reverse();
void forward_brake();
void reverse_brake();
void turn_right();
void turn_left();
void stop_device();
void detect_right_wall();
void detect_left_wall();
void setup() {
Serial.begin(9600);
Serial.println ("Starting");
// initializing the digital pins as outputs:
pinMode (enA, OUTPUT);
pinMode (enB, OUTPUT);
pinMode (forward_in1, OUTPUT);
pinMode (reverse_in2, OUTPUT);
pinMode (left_in4, OUTPUT);
pinMode (right_in3, OUTPUT);
}
void go_forward()
{ //Increase speed from 0 -> 153
for(int i=0; i<=153; i++)
{
analogWrite(forward_in1, i);
analogWrite(reverse_in2,0);
analogWrite(enA,153);
delay(20);
}
//Hold at this speed
delay(500);
}
void go_reverse()
{ //Increase speed from 0 -> 102
for(int i=0; i<=102; i++)
{
analogWrite(forward_in1,0);
analogWrite(reverse_in2,i);
analogWrite(enA,102);
delay(20);
}
//Hold at this speed
delay(500);
}
void forward_brake()
{ //Decrease speed from 153 -> 0
for(int i=153; i>=0; i--)
{
analogWrite(forward_in1,i);
analogWrite(reverse_in2,0);
analogWrite(enA,0);
delay(10);
}
//Hold at zero
delay(500);
}
void reverse_brake()
{ //Decrease speed from 102 -> 0
for(int i=102; i>=0; i--)
{
analogWrite(reverse_in2,i);
analogWrite(forward_in1,0);
analogWrite(enA,0);
delay(10);
}
//Hold at zero
delay(500);
}
void turn_right()
{
digitalWrite (right_in3, HIGH);
digitalWrite (left_in4, LOW);
analogWrite (enB, 255);
}
void turn_left()
{
digitalWrite (left_in4, HIGH);
digitalWrite (right_in3, LOW);
analogWrite(enB, 255);
}
void stop_device()
{
analogWrite (forward_in1, 0);
analogWrite (reverse_in2, 0);
analogWrite(enA,0);
digitalWrite (left_in4, LOW);
digitalWrite (right_in3, LOW);
analogWrite(enB,0);
}
void detect_right_wall ()
{
// PING))) 1
cm1 = DistanceSensor1.ping_cm();
pinMode(pingPin1, OUTPUT); // initialize the pingPin1 as output (Trigger):
digitalWrite(pingPin1, LOW);
delayMicroseconds(2);
digitalWrite(pingPin1, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin1, LOW);
pinMode(pingPin1, INPUT); // initialize the pingPin1 as an input to receive echo:
duration1 = pulseIn (pingPin1, 10);
Serial.print ("Duration 1:");
Serial.println (duration1, DEC);
Serial.print("cm1 = ");
Serial.println(cm1);
if ((duration1 > 4000 || duration1 == 0))
{
right_wall_detected = false;
Serial.println("There is no wall on the right");
}
else
{
right_wall_detected = true;
Serial.println("There is a wall on the right");
}
}
void detect_left_wall()
{
// PING))) 2
cm2 = DistanceSensor2.ping_cm();
pinMode(pingPin2, OUTPUT);
digitalWrite (pingPin2, LOW);
delayMicroseconds (2);
digitalWrite (pingPin2, HIGH);
delayMicroseconds (5);
digitalWrite (pingPin2, LOW);
pinMode(pingPin2, INPUT);
duration2 = pulseIn (pingPin2, 10);
Serial.print ("Duration2:");
Serial.println (duration2, DEC);
Serial.print("cm2 = ");
Serial.println(cm2);
if ((duration2 > 4000 || duration2 == 0)) {
left_wall_detected = false;
Serial.println("There is no wall on the left");
}
else {
left_wall_detected = true;
Serial.println("There is is a wall on the left");
}
}
void loop()
{
go_forward();
delay (5000);
detect_right_wall();
delay(1000);
detect_left_wall();
if (right_wall_detected == false && left_wall_detected == false)
{
go_forward();
}
else
{
if (right_wall_detected == true && left_wall_detected == true)
{
forward_brake();
delay(1000);
stop_device();
delay(500);
do
{
go_forward();
detect_right_wall();
detect_left_wall();
delay(3000);
}while(cm1 > 40.00 && cm2 > 40.00);
delay(1000);
forward_brake();
delay(2000);
stop_device();
delay(500);
}
else
{
if (right_wall_detected == false && left_wall_detected == true)
{
forward_brake();
delay(1000);
stop_device();
detect_left_wall();
delay(100);
do
{
go_forward();
detect_left_wall();
delay(1000);
}while(cm2 > 40);
forward_brake();
delay(100);
stop_device();
delay(1000);
turn_right(), go_forward();
delay(1000);
forward_brake();
delay(1000);
stop_device();
delay(250);
}
else
{
if (right_wall_detected == true && left_wall_detected == false)
{
forward_brake();
delay(1000);
stop_device();
detect_right_wall();
delay(200);
do
{
go_forward();
detect_right_wall();
delay(1000);
}while(cm1 > 40);
forward_brake();
delay(1000);
stop_device();
delay(1000);
turn_left(), go_forward();
delay(1000);
forward_brake();
delay(500);
stop_device();
delay(100);
}
}
}
}
}