I'm currently trying to make a robot go forward, stop over a color, and turn right or left depending on the light value range reflected from that color, then go straight again where the next color will be to repeat this process 4 times until the program stops and exits near a finish line.
I can't figure out where I'm going wrong.
The colors have enough difference in value/shade for me to see that they SHOULD be executing the if/else if statements... consistently.
**-Are my methods of storing my value range for the light reflected from the colors wrong and is that what is causing my if/if else statements not to execute? **
-How can I better improve my program so that it'll consistently execute the if/else if statements for the light ranges that the photoresistor sensor picks up from the colors reflecting light?
Currently it seems that the if/if else statements are NOT being executed, and that no matter where I point or test the sensor- its data is not useable the way I'm using it.
*Yes, sorry, my void functions will only work when they're up top, not on the bottom of the code for some reason.
here's my code:
#include <LiquidCrystal.h>
LiquidCrystal lcd(13, 4, 3, 5, 6, 2);
const int AIN1 = 12; //start motor driver pins
const int AIN2 = 11;
const int PWMA = 10;
const int BIN1 = 8;
const int BIN2 = 7;
const int PWMB = 9; //end motor driver pins
const int led = 52;
const int button = A0;
const int colorSensor = A5;
int color = analogRead(colorSensor);
int s = 600; // 's' for stop delay
int t = 210; // 't' for turn delay
int f = 2700; // 'f' for forward delay
void rightTurn()
{
digitalWrite(AIN1, 1); //turn RIGHT
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 1);
analogWrite(PWMA, 180);
analogWrite(PWMB, 180);
delay(t);
digitalWrite(AIN1, 0); //stop
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 0);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
delay(s);
}
void leftTurn()
{
digitalWrite(AIN1, 1); //turn LEFT
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 1);
analogWrite(PWMA, 180);
analogWrite(PWMB, 180);
delay(t);
digitalWrite(AIN1, 0); //stop
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 0);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
delay(s);
}
void Forward()
{
digitalWrite(AIN1, 0); //move forward
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 1);
digitalWrite(BIN2, 1);
analogWrite(PWMA, 80);
analogWrite(PWMB, 80);
delay(f);
digitalWrite(AIN1, 0); //stop
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 0);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
delay(s);
}
void fullStop()
{
digitalWrite(AIN1, 0); //stop
digitalWrite(BIN1, 0);
digitalWrite(AIN2, 0);
digitalWrite(BIN2, 0);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
delay(s);
}
void lcdMonitor()
{
lcd.setCursor (0, 0);//first row display, monitoring real-time color values
lcd.print("Color value:");
lcd.setCursor (13, 0);
lcd.print(analogRead(colorSensor));
delay(200);
}
void redDisplay()
{
lcd.setCursor (0, 1);//(col, row) second row display, monitoring real-time color values
lcd.print("RED color");
delay(1000);
}
void yellowDisplay()
{
lcd.setCursor (0, 1);
lcd.print("YELLOW color");
delay(1000);
}
void blueDisplay()
{
lcd.setCursor (0, 1);
lcd.print("BLUE color");
delay(1000);
}
void ledOn()
{
digitalWrite(led, HIGH);
}
void ledOff()
{
digitalWrite(led, LOW);
}
void setup()
{
Serial.begin(9600);
lcd.begin(16, 2); //LCD params
lcd.clear();
pinMode(AIN1, OUTPUT); //motor A reverse
pinMode(BIN1, OUTPUT); //motor B reverse
pinMode(PWMA, OUTPUT);
pinMode(AIN2, OUTPUT); //motor A forward
pinMode(BIN2, OUTPUT); //motor B forward
pinMode(PWMB, OUTPUT);
pinMode(led, OUTPUT); //led light ((in case needed for lack of light//better sensing))
pinMode(button, INPUT_PULLUP);
pinMode(colorSensor, INPUT);//todo: get REAL color sensor soon...
}
void loop()//main code below:
{
//Serial.print(analogRead(colorSensor));
//SET COLOR THRESHOLD BELOW:
//todo: find better methods for reponse to "color"/light sensor ranges:
// int RED = RED >= 690 && RED <= 729; //set color thresholds here
// int YELLOW = YELLOW >= 730 && YELLOW <= 880;
// int BLUE = BLUE >= 320 && BLUE <= 420;
//OR try this method:
int RED = constrain(RED, 690, 770);
int YELLOW = constrain(YELLOW, 800, 885);
int BLUE = constrain(BLUE, 200, 500);
lcdMonitor();
lcd.clear();
static int forwardLoop = 0;
//}
//comment out everything below JUST to test/calibrate color threshold via LCD screen.
///*
int buttonDown1 = digitalRead(button);
while (buttonDown1 == HIGH)
{
int buttonUp1 = digitalRead(button);
while (buttonUp1 == LOW && forwardLoop <= 4)
{
lcdMonitor();
Forward();
fullStop();
delay(200);
forwardLoop = forwardLoop + 1;
lcd.clear();
Serial.print(forwardLoop);
if (analogRead(colorSensor) >= 690 && analogRead(colorSensor) <= 729 && forwardLoop == 1)
{
lcdMonitor();
redDisplay();
leftTurn();
lcd.clear();
}
else if (analogRead(colorSensor) >= 730 && analogRead(colorSensor) <= 885 && forwardLoop == 2)
{
delay(t);
lcdMonitor();
yellowDisplay();
rightTurn();
lcd.clear();
}
else if (analogRead(colorSensor) >= 200 && analogRead(colorSensor) <= 420 && forwardLoop == 3)
{
delay(t);
lcdMonitor();
blueDisplay();
rightTurn();
lcd.clear();
}
while (forwardLoop == 4)
{
delay(t);
fullStop();
lcd.clear();
exit;
}
}
}
}//*/
My hardware setup:
elegoo mega 2560
1 x LCD to display current values on the photoresistor, and sensed "colors"
1 x photoresistor
1 x motor driver chip
2 x dc motors with wheels + 1 free spinning castor bolted on to the back.
1 x 7.4 volt lithium battery