Improving the 3pi's sample maze solving code

Hello everyone,

I have been tinkering around with the sample maze solving code included in the Pololu library, and I am having difficulties increasing the turning speed. My main goal is to make my 3pi run as efficiently as the prototype 3pi in the “Pololu 3pi Maze Solving Robot” video in the 3pi User’s Guide. Every time I increase the turning speed, the robot tends to either record a non-existent turn (such as recording a straight or ‘S’ turn when there is no intersection present) or veer off the track. I have tried tuning the PID constants, modifying the turning delays and speeds, and increasing the base speed, but to no avail. Reading the “Improving the Maze-Solving Code” in the 3pi User’s Guide did not provide me with much insight on this problem either.

I am not a master programmer, so I apologize if I am making some simple mistake. Here is my modified version of the file “turn.c”:

void turn(char dir)
{
	switch(dir)
	{
	case 'L':
		// Turn left.
		red_led(HIGH); // Some simple light effects
		play("L16 c"); // Some simple sound effects
		set_motors(-150,150);
		delay_ms(100);
		red_led(LOW);
		break;
	case 'R':
		// Turn right.
		green_led(HIGH); // Simple light effects
		play("L16 e");	 // Simple sound effects
		set_motors(150,-150);
		delay_ms(100);
		green_led(LOW);
		break;
	case 'B':
		// Turn around.
		set_motors(0,0); // Stop the motors while performing sound & light effects 
		play("L16 O7 a#>a#a#>a# !");
		int i = 0;
		while(i<2)
		{
			red_led(HIGH);
			delay_ms(15);
			red_led(LOW);
			delay_ms(15);
			green_led(HIGH);
			delay_ms(15);
			green_led(LOW);
			delay_ms(15);
			i++;
		}
		set_motors(200,-200);
		delay_ms(150);
		break;
	case 'S':
		// Don't do anything!
		break;
	}
}

And here is my modified version of the PID constants in the file “follow-segment.c”:

//int power_difference = proportional/20 + integral/10000 + derivative*3/2; //ORIGINAL
int power_difference = proportional/5 + integral/4000 + derivative*3/2; //MODIFIED

Please note that I am asking for advice only; I am not requesting that someone write the code for me. Any help is greatly appreciated.

Bump

Hello.

The PID constants I used in that video were Kp = 1/7 and Kd = 4. I did not use the integral term. I think I might have also done something a little fancy like relaxing the PID after a short period of time so that the next intersection wouldn’t cause as big of a deflection, then strengthening it again right after the intersection so that it would to a better job of getting back onto the line.

The code in that video is pretty complicated, with a lot of timing-related code designed to make sure the robot can accurately detect and identify intersections while traveling at higher speeds. It’s not necessarily easy to do, and it takes a lot of trial and error with plenty of tweaking to get the timing right. To start with, if you want to turn faster, you should set the robot down and characterize exactly how much of delay you need to reliably turn 90° and 180°. If you are veering off the course, you probably are not well aligned after the turn, or your PID constants aren’t well tuned. If you are recording a non-existent turn, you probably should be disregarding what the sensors are saying for a little while after each intersection.

- Ben

Thank you very much! Those are all great suggestions - I’ll give them a try.

Good luck! Please let me know how it goes.

- Ben

Hello Ben,

Thanks to your suggestions I have made a substantial amount of improvement, particularly on increasing the turning speed. I implemented your suggestion of ignoring what the sensors are saying for some time after each turn, and now the robot can solve the maze correctly the majority of the time. I still need to do some tweaking, however.

That’s great to hear! Thanks for following up. I’d be happy to try to help if you run into any other problems.

- Ben

So I kept increasing the turning speed, and now I’m at 190 (which is the maximum speed I intend to turn). The robot turns reliably and records the correct turns, but for some reason it is not simplifying the solution correctly. This behavior was not present until I hit that 190 mark. The following characters are the turns the 3pi will make, unsimplified:

L, L, R, B, L, L, L, B, R, L, L, L, L, L, B, S, B, L, L, B, S, R, S

And this is what the program simplifies it into:

L, L, L, L, L, R, R, L, L, R, R, S

The correct solution should be:

L, R, L

I have been monitoring the robot as it makes turns to see if it is recording any false turns, but I did not see any discrepancies there. I have not made any modifications to the simplification algorithm.

Mostly because I don’t want to manually try simplifying that path, I am going to suggest a few ways you can try to debug this:

  1. If the 3pi is working properly at slower turn speeds, run it at those slower speeds and print out the unsimplified path. Next, run it with the higher turn speeds and print out the unsimplified path. If the two are reducing to different things, they should be different, and the way they differ should give you a clue about what is going wrong. If the two unsimplified paths are the same and yet they are reducing to different things, then the part of your program that is simplifying the path must have changed and that’s where you should be looking.

or

  1. Run your 3pi on a much smaller/simpler maze so that it’s easier to mentally reduce the path and see what might be going wrong. If you only have one maze, you can always start the 3pi in the middle somewhere so that it only has to run through a few turns before finding the end. If you want my help, please post a picture of this simplified maze and tell me what the unsimplified path is along with the incorrectly simplified path.

Please let me know what you find out.

- Ben

Well, by printing out the unsimplified path, I discovered what the problem is… Every time a U-turn is made, an extra ‘S’ gets recorded for some reason or another. So the unsimplified solution was filled with a bunch of ‘BS’ sequences. This is the same problem that keeps reappearing, except instead of just recording a false ‘S’ on all turns except for a U-turn, it is now only recording a false straight on a U-turn and none of the other turns. I have fine tuned the turning speed delays, and I am also ignoring what the sensors are saying for some time after each turn, so I am not sure what is causing this error. At slower speeds the false recordings do not occur.

Edit: I am also using your PID constants (or PI since the integral term is not being used), and I have tried other tuned versions of the PID constants, so I do not believe it is a line following issue.

Do you start ignoring sensors before or after the turn? One immediate suspicion I have is that with the higher turn speed, you are overshooting your rotation and causing the sensors to interpret that as a new intersection; maybe this happens before you start ignoring them? If you post your program, I can take a look and see if I have any ideas. In general, if changing the rotation speed changes the behavior, then I’d be on the look out for some timing-related problem or an overshoot problem.

- Ben

I start ignoring the sensors after the turn. However, I did manage to get everything running perfectly! I just started messing around with a base speed of 155, and tuned the delays to accommodate the increased speed. Here’s what I changed:

I added this delay to ignore the sensors right after a turn is executed:

set_motors(155,155); delay_ms(70);

And I tuned the intersection delays:

set_motors(145,145); delay_ms(10);

set_motors(135,135); delay_ms(30);

Now the only thing missing is increasing the speed on long segments of track when running the solution. I’m planning on measuring the times of each intersection and speeding up on the longer segments of tape, just as you suggested in the User’s Guide.

Anyway, I greatly appreciate all your help and suggestions! Without your advice I would still be at square one. Thanks again! :smiley:

That’s great! Good luck with your final modification!

- Ben

Hello,
I was looking at the improvements mr.Robot has done and they were quite nice, but I thought I could share some of the improvements I did (its possible to see a video of the improvements in the video page under improved maze solver from any location), I changed the turning speed up to 120, and didn’t try anymore, I don’t know why!
Anyhow the modifications; I didn’t just adjust the delay for each type of turn, I made the delay smaller (so the Robot will not record any faulty turn, then I used the sensors to specify that the Robot is back at a straight line, here is the code for the Turn class:

#include <pololu/3pi.h>

// Turns according to the parameter dir, which should be 'L', 'R', 'S'
// (straight), or 'B' (back).
void turn(char dir)
{
	// Get the position of the line.
	unsigned int sensors[5];
	//unsigned int position = read_line(sensors,IR_EMITTERS_ON);
	int tl;
	switch(dir)
	{
	case 'L':
		// Turn left.
		set_motors(-120,120);
		delay_ms(100);
		tl=0;
		while (tl<100)
		{
			set_motors(-120,120);
			read_line(sensors,IR_EMITTERS_ON);
			if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
			{
				tl=100;
			}
		}
		
		break;
	case 'R':
		// Turn right.
		set_motors(120,-120);
		delay_ms(100);
		tl=0;
		while (tl<100)
		{
			set_motors(120,-120);
			read_line(sensors,IR_EMITTERS_ON);
			if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
			{
				tl=100;
			}
		}
		
		break;
	case 'B':
		// Turn around.
		set_motors(120,-120);
		delay_ms(230);
		tl=0;
		while (tl<100)
		{
			set_motors(120,-120);
			read_line(sensors,IR_EMITTERS_ON);
			if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
			{
				tl=100;
			}
		}
		
		break;
	case 'S':
		// Don't do anything!
		break;
	}
}

Another improvement can be to adjust how the Robot recognize the intersections, for example I have changed the sensitivity of detecting the line ( it can be also used if you will not use a white background), here is the modifications I have done:

		// We use the inner three sensors (1, 2, and 3) for
		// determining whether there is a line straight ahead, and the
		// sensors 0 and 4 for detecting lines going to the left and
		// right.

		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.
			return;
		}
		else if(sensors[0] > 900 || sensors[4] > 900)
		{
			// Found an intersection.
			return;
		}

I hope this can help to improve the performance of the magnificent 3pi :slight_smile:.
Moat.

I really like the idea of detecting when the robot is back on the line. I may have to implement that in my code :mrgreen: I have also increased the threshold for detecting intersections, but I don’t think mine is as high as yours (900; I think mine is ~600). Thanks for sharing your thoughts!