Report

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