Hi,
As part of my project, I am using pololu 3 pi robot to find three lights…
It should rotate 180 degree first and then find the first light which is 1.2 m away from the starting point, then it turn 90 degree and find the light 1.2 m away from the first light & so on.
Below is my code for that…
for rotation I simply used the delay function, but I know it is not the way…how can I do it better?
#include <Pololu3pi.h>
#include <OrangutanLCD.h>
#include <PololuQTRSensors.h>
#include <OrangutanMotors.h>
#include <OrangutanLEDs.h>
#include <OrangutanAnalog.h>
#define SENSOR_LEFT 6 //pin where the left LDR is attached
#define SENSOR_RIGHT 7 //pin where the right LDR is attached
#define buzzer 10 //pin where the buzzor is attached
OrangutanMotors motors;
int LDR1=0, LDR2=0;
int valueDiff=0;
// initial speeds of left and right motors
int leftSpeed=40,rightSpeed=40, rSpeed=10;
long cmillis=0,pmillis=0;
int task=1;
boolean flag=true;
void setup()
{
pinMode(SENSOR_LEFT, INPUT); //initialize the input pin
pinMode(SENSOR_RIGHT, INPUT); //initialize the input pin
pinMode(buzzer, OUTPUT);
void motors_init(void);
delay(1000);
}
void loop()
{
switch (task)
{
case 1:
Rotation180();
task=task+1;
case 2:
ReadSensors();
light_finder();
task=task+1;
case 3:
Rotation90();
task=task+1;
case 4:
ReadSensors();
flag=true;
light_finder();
task=task+1;
case 5:
Rotation90();
task=task+1;
case 6:
ReadSensors();
flag=true;
light_finder();
task=task+1;
}
}
//function to find the light using sensor values
void light_finder()
{
if (flag == true)
{
motors.setSpeeds(80,80);
delay(600);
flag=false;
}
while (LDR1 > 0 || LDR2 > 0)
{
cmillis=millis();
//reading value from the sensors one second after the previous reading
if ((cmillis-pmillis)>250)
{
pmillis=cmillis;
//reading sensor values
ReadSensors();
}
if (LDR1 == 0 && LDR2==0)
{
motors.setSpeeds(0,0);
clear();
print("stop");
break;
}
//comparing sensor values, & go straight if both sensor's have same value
else if (0<=valueDiff && valueDiff<=5)
{
motors.setSpeeds(leftSpeed,rightSpeed);
clear();
print("straight");
delay(5);
}
else if (LDR1>(LDR2+5))
{
motors.setSpeeds(leftSpeed,rSpeed);
clear();
print("right");
delay(5);
}
else if ((LDR1+10)<LDR2)
{
motors.setSpeeds(rSpeed,rightSpeed);
clear();
print("left");
delay(5);
}
}
}
//function to rotate robot 180 degree
void Rotation180()
{
int Speed=-70, rotate=70;
motors.setSpeeds(rotate, Speed);
delay(400);
}
//function to rotate robot 90 degree
void Rotation90()
{
int Speed=-70, rotate=70;
motors.setSpeeds(rotate, Speed);
delay(200);
}
void LCD_display(int x,int y)
{
clear();
lcd_goto_xy(0,1);
print("L:");
lcd_goto_xy(2,1);
print_long(x);
lcd_goto_xy(4,1);
print("R:");
lcd_goto_xy(6,1);
print_long(y);
delay(50);
}
void ReadSensors()
{
//reading sensor values
LDR1=analogRead(SENSOR_LEFT);
LDR2=analogRead(SENSOR_RIGHT);
//rescaling the sensor values using map function
LDR1=map(LDR1,0,1023,0,18);
LDR2=map(LDR2,0,1023,0,18);
//calculating the absolute difference of the sensors
valueDiff=abs(LDR1-LDR2);
//displaying the rescaled values of the sensors using function call
LCD_display(LDR1,LDR2);
}