I am trying to make a line follower and maze solver robot using Arduino and QTR8RC sensors. In that case I can’t rotate my robot on sharp 90 degree or turn around…Here is my code…please help me to improve this code…
#include <Button.h>
#include <QTRSensors.h>
#include <Wire.h>
#define NUMREADINGS 10
Button button = Button(1,PULLUP);
QTRSensorsRC qtr((unsigned char[]) {13, 12, 9, 8 ,7, 4, 3, 2}, 8,3500, QTR_NO_EMITTER_PIN);
unsigned int sensors[8];
//left Motor
int inA1 = 6;
int inA2 = 5;
// Right motor
int inB1 = 11; // Pin Number of Dual-H
int inB2 = 10;
int pinBattery = 19;
int pinAudio = 18;
int voltage;
int readings[NUMREADINGS];
float volts = 0;
int total = 0;
float average = 0;
int index = 0;
void setup(){
Serial.begin(9600); // Initializes the serial communication
pinMode(pinAudio, OUTPUT); // Sets pin as audio output
set_motors(0,0); // While waiting, engines remain standing
voltage = read_battery(); // Reads the battery voltage
Serial.print("Battery");
Serial.print(voltage);
Serial.print(" mV");
delay(2000);
while(!button.isPressed()){
}
delay(500); // Delay to allow time to take your finger off the button
// Always wait for a button to be pressed before
// Your robot might start moving
unsigned int counter; // used as a simple counter
for(counter=0; counter<80; counter++){
if(counter < 20 || counter >= 60){
set_motors(80,-80); // Turn right
}
else{
set_motors(-80,80); // Turns to the left
}
// This function stores a set of sensor readings , and maintains
// Information about the maximum and minimum values found
qtr.calibrate();
// Since we count up to 80 , the total calibration time
// Will be 80 * 10 = 800 ms
delay(10);
}
set_motors(0,0); // Ensure engines stopped after the process
// Calibration
}
// Drive engine
void set_motors(int left_speed, int right_speed){
if(right_speed >= 0 && left_speed >= 0){
analogWrite(inA1, 0);
analogWrite(inA2, right_speed);
analogWrite(inB1, 0);
analogWrite(inB2, left_speed);
}
if(right_speed >= 0 && left_speed < 0){
left_speed = -left_speed;
analogWrite(inA1, 0);
analogWrite(inA2, right_speed);
analogWrite(inB1, left_speed);
analogWrite(inB2, 0);
}
if(right_speed < 0 && left_speed >= 0){
right_speed = -right_speed;
analogWrite(inA1, right_speed);
analogWrite(inA2, 0);
analogWrite(inB1, 0);
analogWrite(inB2, left_speed);
}
}
// Check battery voltage
unsigned int read_battery(){
for (int k = 0; k < NUMREADINGS; k++){ // Resetting the matrix
readings[k] = 0;
}
total -= readings[index];// Initializes Total
readings[index] = analogRead(pinBattery); // Reads the sensor
total += readings[index]; // Sum of matrix values
index = (index + 1); // next value
if (index >= NUMREADINGS){ // Check if reached the end of the readings
index = 0; // If yes resets the index
}
average = total / NUMREADINGS; // simple average
volts = average * 5000 * 3 / 2 / 1023 * 10;
return (volts);
}
// Play Music
void playMusic(){
// The frequency setting of banknotes
#define NOTE_B0 31
#define NOTE_C1 33
#define NOTE_CS1 35
#define NOTE_D1 37
#define NOTE_DS1 39
#define NOTE_E1 41
#define NOTE_F1 44
#define NOTE_FS1 46
#define NOTE_G1 49
#define NOTE_GS1 52
#define NOTE_A1 55
#define NOTE_AS1 58
#define NOTE_B1 62
#define NOTE_C2 65
#define NOTE_CS2 69
#define NOTE_D2 73
#define NOTE_DS2 78
#define NOTE_E2 82
#define NOTE_F2 87
#define NOTE_FS2 93
#define NOTE_G2 98
#define NOTE_GS2 104
#define NOTE_A2 110
#define NOTE_AS2 117
#define NOTE_B2 123
#define NOTE_C3 131
#define NOTE_CS3 139
#define NOTE_D3 147
#define NOTE_DS3 156
#define NOTE_E3 165
#define NOTE_F3 175
#define NOTE_FS3 185
#define NOTE_G3 196
#define NOTE_GS3 208
#define NOTE_A3 220
#define NOTE_AS3 233
#define NOTE_B3 247
#define NOTE_C4 262
#define NOTE_CS4 277
#define NOTE_D4 294
#define NOTE_DS4 311
#define NOTE_E4 330
#define NOTE_F4 349
#define NOTE_FS4 370
#define NOTE_G4 392
#define NOTE_GS4 415
#define NOTE_A4 440
#define NOTE_AS4 466
#define NOTE_B4 494
#define NOTE_C5 523
#define NOTE_CS5 554
#define NOTE_D5 587
#define NOTE_DS5 622
#define NOTE_E5 659
#define NOTE_F5 698
#define NOTE_FS5 740
#define NOTE_G5 784
#define NOTE_GS5 831
#define NOTE_A5 880
#define NOTE_AS5 932
#define NOTE_B5 988
#define NOTE_C6 1047
#define NOTE_CS6 1109
#define NOTE_D6 1175
#define NOTE_DS6 1245
#define NOTE_E6 1319
#define NOTE_F6 1397
#define NOTE_FS6 1480
#define NOTE_G6 1568
#define NOTE_GS6 1661
#define NOTE_A6 1760
#define NOTE_AS6 1865
#define NOTE_B6 1976
#define NOTE_C7 2093
#define NOTE_CS7 2217
#define NOTE_D7 2349
#define NOTE_DS7 2489
#define NOTE_E7 2637
#define NOTE_F7 2794
#define NOTE_FS7 2960
#define NOTE_G7 3136
#define NOTE_GS7 3322
#define NOTE_A7 3520
#define NOTE_AS7 3729
#define NOTE_B7 3951
#define NOTE_C8 4186
#define NOTE_CS8 4435
#define NOTE_D8 4699
#define NOTE_DS8 4978
// Music
int note[] = {NOTE_C4, NOTE_C4, NOTE_G4, NOTE_C5, NOTE_G4, NOTE_C5};
int duration[] = {100, 100, 100, 300, 100, 300};
int starttune[] = {NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_C4, NOTE_F4, NOTE_G4, NOTE_F4, NOTE_E4, NOTE_F4, NOTE_G4};
int duration2[] = {100, 200, 100, 200, 100, 400, 100, 100, 100, 100, 200, 100, 500};
int error[] = {NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3, NOTE_G3, NOTE_C3};
int duration3[] = {100, 200, 100, 200, 100, 200, 100, 200};
// Loop with quandiade of notes to be played
//If necessary, change the internal value of the "for" loop
for(int i=0;i<6;i++){
tone(pinAudio, note[i], duration[i]); // Emits the sound note / duration
delay(duration[i]); // Pausa
noTone(pinAudio); // Finalize
}
}
void follow_segment(){
int last_proportional=0;
long integral;
// This is the main loop - will run forever
while(1){
// Gets the line position
// Here we are not interested in the individual values of each sensor
unsigned int position = qtr.readLine(sensors);
// The proportional term must be 0 when we are on line
int proportional = ((int)position) - 3500;
// Calculate the derivative term ( change) and the integral term (sum )
// Position
int derivative = proportional - last_proportional;
integral += proportional;
last_proportional = proportional;
// Calculate the difference between the power of the two engines
// M1 - M2 . If a positive number , the robot will turn to
// Right. If it is a negative number, the robot will turn left
// And magnetude of numbers determine the sharpness with which will make the curves / turns
int power_difference = proportional/2.5 + integral/2500 + derivative*3/8;
// Remembering the last position
// Calculate the current configuration of the engines. We will never set
// A motor with a negative value
const int max = 255;
const int max_diffrence = 20;
const int factor_diffrence = 2;
if(power_difference > max)
power_difference = max;
if(power_difference < -max)
power_difference = -max;
// if diffrence is too much robot skids
int left_speed = max;
int right_speed = max-power_difference;
if(power_difference < 0)
{
left_speed = max+power_difference;
right_speed = max;
}
if(left_speed - right_speed > max_diffrence)
{
left_speed -= (left_speed - right_speed)/factor_diffrence;
}
else if(right_speed - left_speed > max_diffrence)
{
right_speed -= (right_speed - left_speed)/factor_diffrence;
}
set_motors(left_speed,right_speed);
}
qtr.read(sensors);
if(sensors[0] < 100 && sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100 && sensors[4] < 100 && sensors[5] < 100 && sensors[6] < 100 && sensors[7] < 100)
{
// There is no line visible ahead, and we didn't see any
// intersection. Must be a dead end.
return;
}
else if((sensors[0] > 200 && sensors[1]>200) || (sensors[6] > 200 && sensors[7]>200 ))
{
// Found an intersection.
return;
}
}
void turn(unsigned char dir){
switch(dir){
case 'L':
// Turn left.
set_motors(-100,100);
delay(200);
break;
case 'R':
// Turn right.
set_motors(100,-100);
delay(200);
break;
case 'B':
// Turn around.
set_motors(100,-100);
delay(400);
break;
case 'S':
// Don't do anything!
break;
}
}
char path[100] = "";
unsigned char path_length = 0;
unsigned char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right)
{
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if(found_left)
return 'L';
else if(found_straight)
return 'S';
else if(found_right)
return 'R';
else
return 'B';
}
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if (path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
int i;
for (i = 1; i <= 3; i++)
{
switch (path[path_length - i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch (total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
void loop() {
while(!button.isPressed()){
}
delay(200);
playMusic();
delay(250);
while(1){
follow_segment();
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[8];
qtr.read(sensors);
if(sensors[7]> 100 && sensors[6]> 100 && sensors[6]> 100)
found_left=1;
if(sensors[0] > 100 && sensors[1] > 100 )
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
set_motors(100,100);
delay(200);
// Check for a straight exit.
qtr.read(sensors);
if(sensors[3] > 500 || sensors[4] > 500)
found_straight = 1;
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
path[path_length] = dir;
path_length++;
simplify_path();
}
while(1){
set_motors(0,0);
while(!button.isPressed()){
}
delay(30);
int i;
for (i = 0; i < path_length; i++)
{
follow_segment();
// Drive straight while slowing down, as before.
set_motors(50, 50);
delay(50);
set_motors(40, 40);
delay(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
follow_segment();
}
}