Hi guys,
i need to build a obstacles avoider m3pi by adding an analog sharp sensor and ultrasonic digital sensor.
i am new to m3pi, i am trying to run my m3pi around the maze using the AVR library converted to mbed. Everytime it turns left is perfect, the problem is that it’s ignoring the right turn even though is stopping at every intersection.
If i run the m3pi on a straight line including a right(turn) intersection between the line, it lookslike is taking the turn as it moves a little bit when it sees the right turn, but it doesn’t turn and keeps going straight.
i have included my code, can someone help me and give me some hints please!!!
Once i solved this problem
When i finish my project i will insert the code on the Pololu website
#include "mbed.h"
#include "m3pimaze.h"
BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);
#define MAX 0.5
#define MIN 0
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20
// Global variables
// The path array stores the route info. Each element shows what we did at an intersection
// 'L' for left
// 'R' for right
// 'F' for forward
// 'B' for back
//
char path[1000] = "";
unsigned char path_length = 0; // the length of the path so far
void follow_line()
{
float right;
float left;
float position_of_line = 0.0;
float prev_pos_of_line = 0.0;
float derivative,proportional;
float integral = 0;
float power;
float speed = MAX;
int foundjunction=0;
int countdown=150; //make sure we don't stop for a junction too soon after starting
int sensors[5];
while (foundjunction==0) {
// Get the position of the line.
position_of_line = m3pi.line_position();
proportional = position_of_line;
// Compute the derivative
derivative = position_of_line - prev_pos_of_line;
// Compute the integral
integral += proportional;
// Remember the last position.
prev_pos_of_line = position_of_line;
// Compute
power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
// Compute new speeds
right = speed+power;
left = speed-power;
// limit checks
if (right < MIN)
right = MIN;
else if (right > MAX)
right = MAX;
if (left < MIN)
left = MIN;
else if (left > MAX)
left = MAX;
// set speed
m3pi.left_motor(left);
m3pi.right_motor(right);
if (countdown>0) countdown--; else {
// Next, we are going to use the sensors to look for whether there is still a line ahead
// and try to detect dead ends and possible left or right turns.
m3pi.readsensor(sensors);
if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
{
// There is no line visible ahead, and we didn't see any
// intersection. Must be a dead end.
foundjunction=1;
}
else if(sensors[0] > 200 || sensors[4] > 200)
{
// Found an intersection.
foundjunction=1;
}
} //else countdown
} //while
// straighten up a bit, by steering opposite direction
// not sure if this is needed
// m3pi.left_motor(right);
// m3pi.right_motor(left);
// wait(0.02);
}
// This function decides which way to turn during the learning phase of
// maze solving. It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.
char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
{
// The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
if(found_left)
return 'L';
else if(found_forward)
return 'F';
else if(found_right)
return 'R';
else
return 'B';
}
void doturn(unsigned char dir)
{
if (dir=='R')
{m3pi.right(0.25);wait(0.28);}
else if(dir=='L')
{m3pi.left(0.25);wait(0.28);}
else if(dir=='F')
{m3pi.forward(0.3);wait(0.15);}
else if(dir=='B')
{m3pi.right(0.25);wait(0.6);}
m3pi.forward(0.1);wait(0.1);m3pi.forward(0);
return;
}
// change LBL to S (etc), to bypass dead ends
void simplify()
{
// 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] = 'F';
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;
}
// This function is called once, from main.c.
void mazesolve()
{
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, while examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_forward=0;
unsigned char found_right=0;
int sensors[5];
// Loop until we have solved the maze.
while(1)
{
// Follow the line until an intersection is detected
follow_line();
// Inch forward a bit. This helps us in case we entered the
// intersection at an angle.
found_left=0;found_forward=0;found_right=0;
// m3pi.forward(0.1);
// wait(0.05);
// Now read the sensors and check the intersection type.
m3pi.forward(0.0);
// Check for a forward exit.
m3pi.readsensor(sensors);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_forward = 1;
m3pi.readsensor(sensors);
// Check for left and right exits.
if(sensors[0] > 200)
found_left = 1;
if(sensors[4] > 200)
found_right = 1;
//debug code
m3pi.cls();
if (found_left==1)
m3pi.printf("L");
if (found_right==1)
m3pi.printf("R");
if (found_forward==1)
m3pi.printf("F");
wait (2);
// Check for the ending spot.
// If all five sensors are on dark black, we have
// solved the maze.
if(sensors[0]>600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4]>600)
break;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
m3pi.forward(0.15);
wait(0.02);
unsigned char dir = turn(found_left, found_forward, found_right);
// Make the turn indicated by the path.
doturn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// Need to insert check to make sure that the path_length does not
// exceed the bounds of the array.
// Simplify the learned path.
simplify();
}
// Solved the maze!
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
m3pi.forward(0.0);
m3pi.printf("Finished");
// wait 15s to give time to turn off, or put the robot back to the start
wait(15);
// ideally we would use a button press here
// but I don't think it can easily be read
// Re-run the maze. It's not necessary to identify the
// intersections, so this loop is really simple.
int i;
for(i=0;i<path_length;i++)
{
follow_line();
// Drive straight while slowing down
//m3pi.forward(0.5);
//wait(0.05);
m3pi.forward(0.3);
wait(0.1);
// Make a turn according to the instruction stored in
// path[i].
doturn(path[i]);
}
// Follow the last segment up to the finish.
follow_line();
// Now we should be at the finish! Restart the loop.
}
}
void checksensors()
{
int sensors[5];
while (1) {
m3pi.readsensor(sensors);
m3pi.cls();
if (sensors[0]>200)
m3pi.printf("D");
else m3pi.printf("L");
if (sensors[1]>200)
m3pi.printf("D");
else m3pi.printf("L");
if (sensors[2]>200)
m3pi.printf("D");
else m3pi.printf("L");
if (sensors[3]>200)
m3pi.printf("D");
else m3pi.printf("L");
if (sensors[4]>200)
m3pi.printf("D");
else m3pi.printf("L");
}
}
int main() {
// int sensors[5];
m3pi.locate(0,1);
wait(2.0);
m3pi.sensor_auto_calibrate();
m3pi.printf("MazeSolve");
mazesolve();
m3pi.forward(0.0);
}