Hi,
I'm trying to improve my R2D2 robot, by adding 2 ir sensors on the legs, the code worked fine before i added the legs sensors code, only when i added a second "if" for the legs sensors i got this error, the most strange for me is the error appears on the first "if" of the code that worked fine before i added the second "if", I cant figure out what is wrong
Thanks in advance
Here the part of the code where the error appears
if (distance <= 20){
moveStop();
delay(300);
moveBackward();
delay(500);
moveStop();
delay(300);
distanceRight = lookRight();
delay(300);
distanceLeft = lookLeft();
delay(300);
if (distance >= distanceLeft){
turnRight();
moveStop();
}
else{
turnLeft();
moveStop();
}
}
else{
moveForward();
}
distance = readPing(); << the program points to this line where the error is
}
And here the full code
#include <Servo.h>
// L298N motor control pins
const int LeftMotorForward = 3;
const int LeftMotorBackward = 11;
const int RightMotorForward = 5;
const int RightMotorBackward = 6;
#define sensor A0
boolean goesForward = false;
int distance = 100;
Servo servo_motor;
int sensorPin = 5;
int lightPin = 8;
const int beepPin = 12;// the number of the beep pin
int beepState = LOW;
unsigned long previousMillis = 0;
// constants won't change:
const long interval = 10000;
int IRleftpin = A4;
int IRleftemitter = 2;
int ambientIRleft;
int obstacleIRleft;
int valueleft[10];
int distanceleftleg;
int IRrightpin = A1;
int IRrightemitter = 9;
int ambientIRright;
int obstacleIRright;
int valueright[10];
int distancerightleg;
void setup(){
pinMode(beepPin, OUTPUT);
pinMode(RightMotorForward, OUTPUT);
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
servo_motor.attach(13);
servo_motor.write(115);
delay(2000);
distance = readPing();
delay(100);
pinMode(IRleftemitter,OUTPUT);
digitalWrite(IRleftemitter,LOW);
pinMode(IRrightemitter,OUTPUT);
digitalWrite(IRrightemitter,LOW);
}
void loop(){
beeps();
frontlights();
distanceleftleg = readIRleft(5); // calling the function that will read the distance and passing the "accuracy" to it
distancerightleg = readIRright(5); // calling the function that will read the distance and passing the "accuracy" to it
int distanceRight = 0;
int distanceLeft = 0;
delay(50);
if (distance <= 20){
moveStop();
delay(300);
moveBackward();
delay(500);
moveStop();
delay(300);
distanceRight = lookRight();
delay(300);
distanceLeft = lookLeft();
delay(300);
if (distance >= distanceLeft){
turnRight();
moveStop();
}
else{
turnLeft();
moveStop();
}
}
else{
moveForward();
}
distance = readPing(); [color=red]<< the part where is the error[/color]
}
if ((distanceleftleg > 100) || (distancerightleg > 100)){
moveStop();
delay(300);
moveBackward();
delay(500);
moveStop();
delay(300);
distanceRight = readIRleft();
delay(300);
distanceLeft = readIRleft();
delay(300);
if (distancerightleg > distanceLeftleg){
turnRight();
moveStop();
}
else{
turnLeft();
moveStop();
}
}
else{
moveForward();
}
distanceleftleg = readIRleft(5);
distancerightleg = readIRright(5);
}
int readPing(){
delay(70);
float volts = analogRead(sensor)*0.0048828125; // value from sensor * (5/1024)
int cm = 13*pow(volts, -1); // worked out from datasheet graph
if (cm==0){
cm=250;
}
return cm;
}
int lookRight(){
for (int i = 115; i > 50; i--)
{
servo_motor.write(i);
delay(10);
}
delay(500);
int distance = readPing();
delay(100);
for (int i = 50; i < 115; i++)
{
servo_motor.write(i);
delay(10);
}
return distance;
}
int lookLeft(){
for (int i = 115; i < 170; i++)
{
servo_motor.write(i);
delay(10);
}
delay(500);
int distance = readPing();
delay(100);
for (int i = 170; i > 115; i--)
{
servo_motor.write(i);
delay(10);
}
return distance;
delay(100);
}
void moveStop(){
digitalWrite(RightMotorForward, LOW);
digitalWrite(LeftMotorForward, LOW);
digitalWrite(RightMotorBackward, LOW);
digitalWrite(LeftMotorBackward, LOW);
}
void moveForward(){
if(!goesForward){
goesForward=true;
analogWrite(LeftMotorForward, 80);
analogWrite(RightMotorForward, 90);
analogWrite(LeftMotorBackward, 0);
analogWrite(RightMotorBackward, 0);
}
}
void moveBackward(){
goesForward=false;
analogWrite(LeftMotorBackward, 80);
analogWrite(RightMotorBackward, 90);
analogWrite(LeftMotorForward, 0);
analogWrite(RightMotorForward, 0);
}
void turnRight(){
analogWrite(LeftMotorForward, 80);
analogWrite(RightMotorBackward, 90);
analogWrite(LeftMotorBackward, 0);
analogWrite(RightMotorForward, 0);
delay(850);
analogWrite(LeftMotorForward, 80);
analogWrite(RightMotorForward, 90);
analogWrite(LeftMotorBackward, 0);
analogWrite(RightMotorBackward, 0);
}
void turnLeft(){
analogWrite(LeftMotorBackward, 80);
analogWrite(RightMotorForward, 90);
analogWrite(LeftMotorForward, 0);
analogWrite(RightMotorBackward, 0);
delay(850);
digitalWrite(LeftMotorForward, 80);
digitalWrite(RightMotorForward, 90);
digitalWrite(LeftMotorBackward, 0);
digitalWrite(RightMotorBackward, 0);
}
void beeps(){
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval) {
// save the last time you blinked the LED
previousMillis = currentMillis;
// if the LED is off turn it on and vice-versa:
if (beepState == LOW) {
beepState = HIGH;
} else {
beepState = LOW;
}
// set the LED with the ledState of the variable:
digitalWrite(beepPin, beepState);
}
}
void frontlights(){
// Read the sensor pin
int sensorValue = analogRead(sensorPin);
// If light level is low is detected, switch light on
if (sensorValue < 35){
digitalWrite(lightPin, HIGH);
}
// If light level goes up again, switch the lights off
if (sensorValue > 150){
digitalWrite(lightPin, LOW);
}
}
int readIRleft(int times){
for(int x=0;x<times;x++){
digitalWrite(IRleftemitter,LOW); // turning the IR LEDs off to read the IR coming from the ambient
delay(1); // minimum delay necessary to read values
ambientIRleft = analogRead(IRleftpin); // storing IR coming from the ambient
digitalWrite(IRleftemitter,HIGH); // turning the IR LEDs on to read the IR coming from the obstacle
delay(1); // minimum delay necessary to read values
obstacleIRleft = analogRead(IRleftpin); // storing IR coming from the obstacle
valueleft[x] = ambientIRleft-obstacleIRleft; // calculating changes in IR values and storing it for future average
}
for(int x=0;x<times;x++){ // calculating the average based on the "accuracy"
distanceleftleg+=valueleft[x];
}
return(distanceleftleg/times); // return the final value
}
int readIRright(int times){
for(int x=0;x<times;x++){
digitalWrite(IRrightemitter,LOW); // turning the IR LEDs off to read the IR coming from the ambient
delay(1); // minimum delay necessary to read values
ambientIRright = analogRead(IRrightpin); // storing IR coming from the ambient
digitalWrite(IRrightemitter,HIGH); // turning the IR LEDs on to read the IR coming from the obstacle
delay(1); // minimum delay necessary to read values
obstacleIRright = analogRead(IRrightpin); // storing IR coming from the obstacle
valueright[x] = ambientIRright-obstacleIRright; // calculating changes in IR values and storing it for future average
}
for(int x=0;x<times;x++){ // calculating the average based on the "accuracy"
distancerightleg+=valueright[x];
}
return(distancerightleg/times); // return the final value
}