MICROMOUSE ALGORITHMS. Navigation, Path finding, and Maze Encoding

Transcription

1 MICROMOUSE ALGORITHMS Navigation, Path finding, and Maze Encoding

2 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

3 PID FEEDBACK CONTROL

4 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 K p constant current_error: return curpos - cellmidpoint P_controller: error = current_error() return Kp * error

5 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 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

6 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

7 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

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

9 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

10

11 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

12 QUICK DEMO

13 FLOODFILL SIMULATOR You can find a decent Micromouse simulator at 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

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

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

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

17 BITWISE MATH REVIEW OR ( ) NEGATE (~) AND ( & ) Left shift << << <<

18 BITWISE MATH REVIEW & & ~( ) &

19 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

20 !!!! BITVECTOR IMPLEMENTATION #include <stdint.h> // uint16_t! #include <cstring> // memset! public:! void set(unsigned x, unsigned y) {! // 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];! }! 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;! }!

21 CHECKIN CODE