Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.
Dependencies: IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos
HumanPosition.h@3:0a6e4d139b86, 2014-04-30 (annotated)
- Committer:
- Strikewolf
- Date:
- Wed Apr 30 12:25:47 2014 +0000
- Revision:
- 3:0a6e4d139b86
- Parent:
- 1:dacf7db790f6
Final product before demo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 1:dacf7db790f6 | 1 | #include <math.h> |
Strikewolf | 1:dacf7db790f6 | 2 | #include "USBHostMouse.h" |
Strikewolf | 1:dacf7db790f6 | 3 | |
Strikewolf | 1:dacf7db790f6 | 4 | #define PFLAG_ON 0 |
Strikewolf | 1:dacf7db790f6 | 5 | #define PFLAG_OFF 1 |
Strikewolf | 1:dacf7db790f6 | 6 | #define PFLAG_CALIB 2 |
Strikewolf | 1:dacf7db790f6 | 7 | |
Strikewolf | 1:dacf7db790f6 | 8 | #define SERIAL_BUFFER_SIZE 20 |
Strikewolf | 1:dacf7db790f6 | 9 | #define PACKET_SIZE 10 |
Strikewolf | 1:dacf7db790f6 | 10 | |
Strikewolf | 1:dacf7db790f6 | 11 | #define X_HUMAN_UPPER 1 |
Strikewolf | 1:dacf7db790f6 | 12 | #define X_HUMAN_LOWER 2 |
Strikewolf | 1:dacf7db790f6 | 13 | #define Y_HUMAN_UPPER 4 |
Strikewolf | 1:dacf7db790f6 | 14 | #define Y_HUMAN_LOWER 5 |
Strikewolf | 1:dacf7db790f6 | 15 | |
Strikewolf | 1:dacf7db790f6 | 16 | //Convert from radians to degrees. |
Strikewolf | 1:dacf7db790f6 | 17 | #define toDegrees(x) (x * 57.2957795) |
Strikewolf | 1:dacf7db790f6 | 18 | |
Strikewolf | 1:dacf7db790f6 | 19 | //Quadrant enum |
Strikewolf | 1:dacf7db790f6 | 20 | typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT; |
Strikewolf | 1:dacf7db790f6 | 21 | |
Strikewolf | 1:dacf7db790f6 | 22 | // updates xy position if on, does nothing if off |
Strikewolf | 1:dacf7db790f6 | 23 | extern char PFlag = PFLAG_ON; |
Strikewolf | 1:dacf7db790f6 | 24 | |
Strikewolf | 1:dacf7db790f6 | 25 | // variables to keep track of coordinate position |
Strikewolf | 1:dacf7db790f6 | 26 | double x_position = 0; |
Strikewolf | 1:dacf7db790f6 | 27 | double y_position = 0; |
Strikewolf | 1:dacf7db790f6 | 28 | |
Strikewolf | 1:dacf7db790f6 | 29 | // human position |
Strikewolf | 1:dacf7db790f6 | 30 | Serial xbee(p13, p14); |
Strikewolf | 1:dacf7db790f6 | 31 | DigitalOut rst1(p15); |
Strikewolf | 1:dacf7db790f6 | 32 | |
Strikewolf | 1:dacf7db790f6 | 33 | /* mouse event handler */ |
Strikewolf | 1:dacf7db790f6 | 34 | void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) |
Strikewolf | 1:dacf7db790f6 | 35 | { |
Strikewolf | 1:dacf7db790f6 | 36 | // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch |
Strikewolf | 1:dacf7db790f6 | 37 | double y_temp = y_mickey / 232.6 * 100; |
Strikewolf | 1:dacf7db790f6 | 38 | double g_temp = imuFilter.getYaw(); |
Strikewolf | 1:dacf7db790f6 | 39 | |
Strikewolf | 1:dacf7db790f6 | 40 | // determine direction we are facing and add to that direction |
Strikewolf | 1:dacf7db790f6 | 41 | //y_position += y_temp; |
Strikewolf | 1:dacf7db790f6 | 42 | //x_position += y_temp * tan(g_temp); |
Strikewolf | 1:dacf7db790f6 | 43 | double dx = 0; |
Strikewolf | 1:dacf7db790f6 | 44 | double dy = 0; |
Strikewolf | 1:dacf7db790f6 | 45 | QUADRANT quadrant; |
Strikewolf | 1:dacf7db790f6 | 46 | if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90) |
Strikewolf | 1:dacf7db790f6 | 47 | quadrant = FORWARD_LEFT; |
Strikewolf | 1:dacf7db790f6 | 48 | if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90) |
Strikewolf | 1:dacf7db790f6 | 49 | quadrant = FORWARD_RIGHT; |
Strikewolf | 1:dacf7db790f6 | 50 | if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180) |
Strikewolf | 1:dacf7db790f6 | 51 | quadrant = BACKWARD_LEFT; |
Strikewolf | 1:dacf7db790f6 | 52 | if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180) |
Strikewolf | 1:dacf7db790f6 | 53 | quadrant = BACKWARD_RIGHT; |
Strikewolf | 1:dacf7db790f6 | 54 | |
Strikewolf | 1:dacf7db790f6 | 55 | switch (quadrant) { |
Strikewolf | 1:dacf7db790f6 | 56 | case FORWARD_LEFT: |
Strikewolf | 1:dacf7db790f6 | 57 | dy = y_temp * cos(g_temp); |
Strikewolf | 1:dacf7db790f6 | 58 | dx = -y_temp * sin(g_temp); |
Strikewolf | 1:dacf7db790f6 | 59 | break; |
Strikewolf | 1:dacf7db790f6 | 60 | case FORWARD_RIGHT: |
Strikewolf | 1:dacf7db790f6 | 61 | dy = y_temp * cos(g_temp); |
Strikewolf | 1:dacf7db790f6 | 62 | dx = -y_temp * sin(g_temp); |
Strikewolf | 1:dacf7db790f6 | 63 | break; |
Strikewolf | 1:dacf7db790f6 | 64 | case BACKWARD_LEFT: |
Strikewolf | 1:dacf7db790f6 | 65 | dy = -y_temp * sin(g_temp); |
Strikewolf | 1:dacf7db790f6 | 66 | dx = y_temp * cos(g_temp); |
Strikewolf | 1:dacf7db790f6 | 67 | break; |
Strikewolf | 1:dacf7db790f6 | 68 | case BACKWARD_RIGHT: |
Strikewolf | 1:dacf7db790f6 | 69 | dy = y_temp * sin(g_temp); |
Strikewolf | 1:dacf7db790f6 | 70 | dx = -y_temp * cos(g_temp); |
Strikewolf | 1:dacf7db790f6 | 71 | break; |
Strikewolf | 1:dacf7db790f6 | 72 | } |
Strikewolf | 1:dacf7db790f6 | 73 | |
Strikewolf | 1:dacf7db790f6 | 74 | x_position += dx; |
Strikewolf | 1:dacf7db790f6 | 75 | y_position += dy; |
Strikewolf | 1:dacf7db790f6 | 76 | |
Strikewolf | 1:dacf7db790f6 | 77 | // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp)); |
Strikewolf | 1:dacf7db790f6 | 78 | // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant); |
Strikewolf | 1:dacf7db790f6 | 79 | // pc.printf("x: %f, y: %f, dx: %f, dy: %f, g: %f, q: %d\n\r", x_position, y_position, dx, dy, toDegrees(g_temp), quadrant); |
Strikewolf | 1:dacf7db790f6 | 80 | } |
Strikewolf | 1:dacf7db790f6 | 81 | |
Strikewolf | 1:dacf7db790f6 | 82 | /* positioning system thread function */ |
Strikewolf | 1:dacf7db790f6 | 83 | void PositionSystemMain(void const *) |
Strikewolf | 1:dacf7db790f6 | 84 | { |
Strikewolf | 1:dacf7db790f6 | 85 | |
Strikewolf | 1:dacf7db790f6 | 86 | USBHostMouse mouse; |
Strikewolf | 1:dacf7db790f6 | 87 | |
Strikewolf | 1:dacf7db790f6 | 88 | while(!gameOver) { |
Strikewolf | 1:dacf7db790f6 | 89 | // try to connect a USB mouse |
Strikewolf | 1:dacf7db790f6 | 90 | while(!mouse.connect() && !gameOver) |
Strikewolf | 1:dacf7db790f6 | 91 | Thread::wait(500); |
Strikewolf | 1:dacf7db790f6 | 92 | |
Strikewolf | 1:dacf7db790f6 | 93 | // when connected, attach handler called on mouse event |
Strikewolf | 1:dacf7db790f6 | 94 | mouse.attachEvent(onMouseEvent); |
Strikewolf | 1:dacf7db790f6 | 95 | |
Strikewolf | 1:dacf7db790f6 | 96 | // wait until the mouse is disconnected |
Strikewolf | 1:dacf7db790f6 | 97 | while(mouse.connected() && !gameOver) |
Strikewolf | 1:dacf7db790f6 | 98 | Thread::wait(500); |
Strikewolf | 1:dacf7db790f6 | 99 | } |
Strikewolf | 1:dacf7db790f6 | 100 | } |
Strikewolf | 1:dacf7db790f6 | 101 | |
Strikewolf | 1:dacf7db790f6 | 102 | void sendPosition(void const *) |
Strikewolf | 1:dacf7db790f6 | 103 | { |
Strikewolf | 3:0a6e4d139b86 | 104 | // temp variables |
Strikewolf | 1:dacf7db790f6 | 105 | int x = 0; |
Strikewolf | 1:dacf7db790f6 | 106 | int y = 0; |
Strikewolf | 1:dacf7db790f6 | 107 | char a = 0; |
Strikewolf | 1:dacf7db790f6 | 108 | char b = 0; |
Strikewolf | 1:dacf7db790f6 | 109 | char c = 0; |
Strikewolf | 1:dacf7db790f6 | 110 | char d = 0; |
Strikewolf | 1:dacf7db790f6 | 111 | |
Strikewolf | 1:dacf7db790f6 | 112 | // sample current xy position |
Strikewolf | 1:dacf7db790f6 | 113 | x = (int) x_position; |
Strikewolf | 1:dacf7db790f6 | 114 | y = (int) y_position; |
Strikewolf | 1:dacf7db790f6 | 115 | |
Strikewolf | 1:dacf7db790f6 | 116 | pc.printf("Sending: %d %d...\n\r", x, y); |
Strikewolf | 3:0a6e4d139b86 | 117 | |
Strikewolf | 3:0a6e4d139b86 | 118 | // break down x coordinate by byte |
Strikewolf | 1:dacf7db790f6 | 119 | a = x >> 24; |
Strikewolf | 1:dacf7db790f6 | 120 | b = x >> 16; |
Strikewolf | 1:dacf7db790f6 | 121 | c = x >> 8; |
Strikewolf | 1:dacf7db790f6 | 122 | d = x; |
Strikewolf | 3:0a6e4d139b86 | 123 | |
Strikewolf | 3:0a6e4d139b86 | 124 | // put each byte on the buffer |
Strikewolf | 1:dacf7db790f6 | 125 | xbee.putc('x'); |
Strikewolf | 3:0a6e4d139b86 | 126 | xbee.putc(a); |
Strikewolf | 3:0a6e4d139b86 | 127 | xbee.putc(b); |
Strikewolf | 1:dacf7db790f6 | 128 | xbee.putc(c); |
Strikewolf | 1:dacf7db790f6 | 129 | xbee.putc(d); |
Strikewolf | 1:dacf7db790f6 | 130 | |
Strikewolf | 3:0a6e4d139b86 | 131 | // break down y coordinate by byte |
Strikewolf | 1:dacf7db790f6 | 132 | a = y >> 24; |
Strikewolf | 1:dacf7db790f6 | 133 | b = y >> 16; |
Strikewolf | 1:dacf7db790f6 | 134 | c = y >> 8; |
Strikewolf | 1:dacf7db790f6 | 135 | d = y; |
Strikewolf | 3:0a6e4d139b86 | 136 | |
Strikewolf | 3:0a6e4d139b86 | 137 | // put each byte on the buffer |
Strikewolf | 1:dacf7db790f6 | 138 | xbee.putc('y'); |
Strikewolf | 3:0a6e4d139b86 | 139 | xbee.putc(a); |
Strikewolf | 3:0a6e4d139b86 | 140 | xbee.putc(b); |
Strikewolf | 1:dacf7db790f6 | 141 | xbee.putc(c); |
Strikewolf | 1:dacf7db790f6 | 142 | xbee.putc(d); |
Strikewolf | 1:dacf7db790f6 | 143 | |
Strikewolf | 1:dacf7db790f6 | 144 | pc.printf("Send complete.\n\r"); |
Strikewolf | 3:0a6e4d139b86 | 145 | |
Strikewolf | 3:0a6e4d139b86 | 146 | // wait for a possible game over response |
Strikewolf | 1:dacf7db790f6 | 147 | wait(0.25); |
Strikewolf | 1:dacf7db790f6 | 148 | if(xbee.readable() && xbee.getc() == 'd') { |
Strikewolf | 1:dacf7db790f6 | 149 | gameOver = true; |
Strikewolf | 1:dacf7db790f6 | 150 | pc.printf("gameOver Received!\n\r"); |
Strikewolf | 1:dacf7db790f6 | 151 | endGame(); |
Strikewolf | 1:dacf7db790f6 | 152 | } |
Strikewolf | 1:dacf7db790f6 | 153 | } |