MICROMOUSE ALGORITHMS
Navigation, Path finding, and Maze Encoding
FEEDBACK CONTROL
• Your mouse can never go perfectly straight in practice
• Mouse can drift to one side, traction may not be same on wheels, etc.
• Need feedback control to remain centered in the cell
• Periodically use IR sensor data (and perhaps gyro) to determine error
• Slightly speed up a motor and slow another to correct heading
PID FEEDBACK CONTROL
PROPORTIONAL FEEDBACK
•
Difference between where you are and where you want to be
•
The larger the error, the larger the correction to be applied (intuitive)
•
Scaled by the Kp constant
current_error:
return curPos - cellMidpoint
P_controller:
error = current_error()
return Kp * error
INTEGRAL FEEDBACK
•
The sum of all your previous errors in time
•
Attempts to prevent errors before they occur, based on how they occurred in past
•
E.g. mouse tends to lean right, it should tend to drive left to even out
•
Scaled by KI constant
error_integral = 0
I_controller:
error = current_error()
error_integral += (error * delay_time) // May need to reset to 0 sometimes
return Ki * error_integral
DERIVATIVE FEEDBACK
•
The change in error since the last sample
•
Prevents overshooting target based on how the error is changing by predicting when we
will hit the desired position
•
Scaled by K D constant
previous_error = 0
D_controller:
error = current_error()
derivative = (error - previous_error) / delay_time
previous_error = error
return Kd * derivative
COMBINING PID TOGETHER
error_integral = 0
previous_error = 0
last_sample = 0
pid:
curTime = time()
delay_time = curTime - last_sample
last_sample = curTime
error = curPos - cellMidpoint
error_integral += (error * delay_time)
error_derivative = (error - previous_error) / delay_time
previous_error = error
correction = (Kp * error) + (Ki * error_integral) + (Kd * error_derivative)
return correction
PID TUNING
•
The PID constants are very important and determine the accuracy of your system
•
Easy system is essentially unique, and can behave differently in different environments
•
Strategy:
1.
Set all gains to zero.
2.
Increase the P gain until the response to a disturbance is steady oscillation.
3.
Increase the D gain until the the oscillations go away (i.e. it's critically damped).
4.
Repeat steps 2 and 3 until increasing the D gain does not stop the oscillations.
5.
Set P and D to the last stable values.
6.
Increase the I gain until it brings you to the setpoint with the number of oscillations
desired (normally zero but a quicker response can be had if you don't mind a
couple oscillations of overshoot)
PATH FINDING
• Plethora of path finding algorithms exist
• Floodfill is one of the most common implementations
• Tries to find the shortest path
• Not necessarily fastest path
• Easy to implement
• Run-time variable
• Initialize goal to 0, all other cells to their Manhattan distance from the goal
• Move to neighbor with lowest distance
• Image water flowing down towards goal (sink) which we follow
FLOODFILL PSEUDOCODE
•
Slightly different than typical implementation: has modifications I found to be useful/effective
floodfill:
let stack = stack of points to be processed // can also use queue, same results
push current mouse position on stack
while stack not empty:
let cur = pop top element of stack
mark cur as processed
if dist(cur) == 0 skip to next // don't want to update end goal with a non-zero distance!
let shortest = +infinity // signify "not set"/invalid path
for all directions (N, S, E, W)
let neighbor = cur + direction
if no wall between cur and neighbor
if dist(neighbor) < shortest then set shortest = dist(neighbor)
if neighbor not processed before then push neighbor on stack
if shortest == +infinity then skip to next // cell has no openings, continue
if cur dist == shortest + 1 then skip to next // nothing was updated
set dist(cur) = shortest + 1
push all OPEN neighbors of cur on the stack, even if previously processed // can cause problems if you don’t
QUICK DEMO
FLOODFILL SIMULATOR
• You can find a decent Micromouse simulator at
https://code.google.com/p/maze-solver/
• Download the .jar file and run it on your computer
• On Linux/OS X: run `java –jar /path/to/maze-solver.jar`
• Helps in making sure your floodfill implementation is correct
ENCODING THE MAZE
• Maze is a 16x16 grid: 256 cells
• Goal is 4 cells in the center
• Various walls and no walls between the cells
• We need a data structure that we can use quickly and efficiently
• Any ideas?
ENCODING THE MAZE
• Naïve approach: store NSEW wall or no wall for each cell
• Kind of redundant: if you can go North from cell A to cell B, then you can go
South from cell B as well
• Better approach:
• store one array of wall positions
• Look up cell openings using a position and direction
• e.g. if at cell (0,0) can we go North (to (0,1))?
• Look if there is a wall above cell (0,0)
ENCODING THE MAZE
•
Since we are using boolean values, we can compress data even further, using a
bit per value instead of a whole byte
•
Use a bitvector for the walls in the maze!
•
Interface for bitvector data structure:
• Set a bit (set to 1)
• Clear a bit (set to 0)
• Get a bit
•
We can use an array of 16 16-bit integers (use the uint16_t type)
•
Row indexes in the array, column indexes the bit of the uint16_t
•
Note: to keep indexing values sane, it might be better to use one bitvector for
horizontal walls and one for vertical walls, so that we can disambiguate them.
BITWISE MATH REVIEW
OR ( | )
0 1
0
0 1
NEGATE
(~)
0
1
1 1
0
1
1
0
AND ( & ) 0 1
Left shift
0
0
0 0
0110 << 0
0110
1
0 1
0110 << 1
1100
0110 << 2
1000
BITWISE MATH REVIEW
00111001
& 00001000
00111001
& ~(00001000)
00001000
00110001
00111001
00111001
| 00000010
00111011
&
11110111
00110001
BITWISE MATH RECAP
• Set a bit by ORing variable with a number where only that bit is set
• Clear a bit by ANDing variable with a number where all bits BUT bit in
question is set
• Get a bit by ANDing a variable with a number where only that bit is set, and
check if the result == 0
• We can position a 1 anywhere we want by LEFT-SHIFTing by that amount
• We can position a 0 anywhere by positioning a 1 in its place and then
NEGATING that value
BITVECTOR IMPLEMENTATION
#include <stdint.h> // uint16_t
void set(unsigned x, unsigned y) {
#include <cstring> // memset
vector[x] |= 1<<y;
}
// WARNING: Code has no initialization/bounds
checking
void clear(unsigned x, unsigned y) {
vector[x] &= ~(1<<y);
// Don't use it directly or it will probably crash!!!
}
class BitVector256 {
bool get(unsigned x, unsigned y) {
protected:
return (vector[x] & 1<<y) != 0;
const unsigned VECTOR_SIZE = 16;
uint16_t vector[VECTOR_SIZE];
public:
}
}
CHECKIN CODE