Navigation, Path finding, and Maze Encoding

• 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

• 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 K p constant current_error: return curPos - cellMidpoint
P_controller: error = current_error() return Kp * error

• 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 K
I 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

• 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

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

• 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)

• 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

•
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

• 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

• 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?

• 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)

• 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.

OR ( | ) 0 1
0 0 1
1 1 1
NEGATE
(~)
0
1
0
1
0
AND ( & ) 0 1
0 0 0
1 0 1
Left shift 0
0110 << 0 0110
0110 << 1 1100
0110 << 2 1000

0011 1 001
& 0000 1 000
0000 1 000
0011 1 001
& ~(0000 1 000)
0011 0 001
001110 0 1
| 000000 1 0
001110 1 1
0011 1 001
& 1111 0 111
0011 0 001

• 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

#include <stdint.h> // uint16_t
#include <cstring> // memset
// WARNING: Code has no initialization/bounds checking
// Don't use it directly or it will probably crash!!!
class BitVector256 { protected : const unsigned VECTOR_SIZE = 16 ; uint16_t vector[VECTOR_SIZE];
} void set( unsigned x, unsigned y) { vector[x] |= 1 <<y;
} void clear( unsigned x, unsigned y) { vector[x] &= ~( 1 <<y);
}
} bool get( unsigned x, unsigned y) { return (vector[x] & 1 <<y) != 0; public :