I've built a robot that when it encounters an LED light source initiates a State, the way I've done this is to mount a photo resistor on the under chassis of the robot. When the light threshold passes 500 it's meant to initiate the state otherwise move forward and if it encounters an object ahead turns left (defined by the ultrasonic distance sensor). When testing the robot neither states are working, the photo resistor and distance sensor are both working and printing values to the monitor, I've also tested the robot on another code and the distance sensor works fine so I was just wondering if you guys can help me see what I'm missing?
Here's the code:
#include <Servo.h>
// create servo objects
Servo leftMotor;
Servo rightMotor;
const int serialPeriod = 250; // serial print to console every 1/4 second
unsigned long timeSerialDelay =0;
const int loopPeriod = 20; // ultrasonic period and frequency (50hz)
unsigned long timeLoopDelay =0;
// pins used for ultrasonic sensor,LDR and LED
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;
const int ldrPin = A0;
int led = 3;
int ultrasonic2Distance;
int ultrasonic2Duration;
int ldrValue;
#define DRIVE_FORWARD 0 // defined states
#define TURN_LEFT 1
#define MY_STATE 2
int state = DRIVE_FORWARD;
void setup()
{
Serial.begin(9600);
// ultrasonic sensor pin configurations
pinMode(ultrasonic2TrigPin, OUTPUT);
pinMode(ultrasonic2EchoPin, INPUT);
pinMode(led, OUTPUT);
leftMotor.attach(13);
rightMotor.attach(12);
}
void loop()
{
debugOutput(); // prints debugging messages to the serial console
if(millis() - timeLoopDelay >= loopPeriod)
{
readUltrasonicSensors(); // read and store the measured distances
readLdr(); // read and store the LDR value
stateMachine();
timeLoopDelay = millis();
digitalWrite(led, HIGH); // turn the LED on (HIGH is the voltage level)
delay(1000); // wait for a second
digitalWrite(led, LOW); // turn the LED off by making the voltage LOW
delay(1000); // wait for a second
}
}
void stateMachine()
{
if(state == DRIVE_FORWARD) // no obstacles detected
{
if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of us
{
// drive forward
rightMotor.write(180);
leftMotor.write(0);
}
else if(ldrValue > 500 ) // if an LED is detected
{
state = MY_STATE; // My state
rightMotor.write (90);
leftMotor.write (90);
}
else // there's an object in front
{
state = TURN_LEFT;
}
}
else if(state == TURN_LEFT) // obstacle detected -- turn left
{
unsigned long timeToTurnLeft = 500; // .5 seconds to turn 90 degrees
unsigned long turnStartTime = millis(); // save the time
while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until timeToTurnLeft (.5 seconds) has elapsed
{
// turn left
rightMotor.write(180);
leftMotor.write(180);
}
state = DRIVE_FORWARD; // revert back to the default state
}
}
void readUltrasonicSensors()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin, HIGH);
delayMicroseconds(10); // keep the trig pin high for at least 10us
digitalWrite(ultrasonic2TrigPin, LOW);
ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);
ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}
void readLdr()
{
ldrValue = analogRead(ldrPin);
}
void debugOutput()
{
if((millis() - timeSerialDelay) > serialPeriod)
{
Serial.print("ultrasonic2Distance: ");
Serial.print(ultrasonic2Distance);
Serial.print("cm");
Serial.println();
Serial.print("ldrValue: ");
Serial.print(ldrValue);
Serial.println();
timeSerialDelay = millis();
}
}