Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.
Dependencies: ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed
Fork of Project by
AIPosition.h@6:3fb9f96765f6, 2014-04-30 (annotated)
- Committer:
- Strikewolf
- Date:
- Wed Apr 30 12:23:04 2014 +0000
- Revision:
- 6:3fb9f96765f6
- Parent:
- 5:210cd333f770
Final product before demo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Strikewolf | 5:210cd333f770 | 1 | #include <math.h> |
Strikewolf | 5:210cd333f770 | 2 | #include "USBHostMouse.h" |
Strikewolf | 5:210cd333f770 | 3 | |
Strikewolf | 5:210cd333f770 | 4 | #define SONAR_STOP (2.0) |
Strikewolf | 5:210cd333f770 | 5 | |
Strikewolf | 5:210cd333f770 | 6 | #define PFLAG_ON 0 |
Strikewolf | 5:210cd333f770 | 7 | #define PFLAG_OFF 1 |
Strikewolf | 5:210cd333f770 | 8 | #define PFLAG_CALIB 2 |
Strikewolf | 5:210cd333f770 | 9 | |
Strikewolf | 5:210cd333f770 | 10 | #define SERIAL_BUFFER_SIZE 20 |
Strikewolf | 5:210cd333f770 | 11 | #define PACKET_SIZE 10 |
Strikewolf | 5:210cd333f770 | 12 | |
Strikewolf | 5:210cd333f770 | 13 | #define X_HUMAN_UPPER 1 |
Strikewolf | 5:210cd333f770 | 14 | #define X_HUMAN_LOWER 2 |
Strikewolf | 5:210cd333f770 | 15 | #define Y_HUMAN_UPPER 4 |
Strikewolf | 5:210cd333f770 | 16 | #define Y_HUMAN_LOWER 5 |
Strikewolf | 5:210cd333f770 | 17 | |
Strikewolf | 5:210cd333f770 | 18 | //Convert from radians to degrees. |
Strikewolf | 5:210cd333f770 | 19 | #define toDegrees(x) (x * 57.2957795) |
Strikewolf | 5:210cd333f770 | 20 | |
Strikewolf | 5:210cd333f770 | 21 | //Quadrant enum |
Strikewolf | 5:210cd333f770 | 22 | typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT; |
Strikewolf | 5:210cd333f770 | 23 | |
Strikewolf | 5:210cd333f770 | 24 | // sonar sensors |
Strikewolf | 5:210cd333f770 | 25 | AnalogIn sonar1(p16); // FL |
Strikewolf | 5:210cd333f770 | 26 | AnalogIn sonar2(p17); // FC |
Strikewolf | 5:210cd333f770 | 27 | AnalogIn sonar3(p18); // FR |
Strikewolf | 5:210cd333f770 | 28 | |
Strikewolf | 5:210cd333f770 | 29 | // updates xy position if on, does nothing if off |
Strikewolf | 5:210cd333f770 | 30 | extern char PFlag = PFLAG_ON; |
Strikewolf | 5:210cd333f770 | 31 | |
Strikewolf | 5:210cd333f770 | 32 | // variables to keep track of coordinate position |
Strikewolf | 5:210cd333f770 | 33 | double x_position = 0; |
Strikewolf | 5:210cd333f770 | 34 | double y_position = 0; |
Strikewolf | 5:210cd333f770 | 35 | |
Strikewolf | 5:210cd333f770 | 36 | double x_hum = 123456; |
Strikewolf | 5:210cd333f770 | 37 | double y_hum = 123456; |
Strikewolf | 5:210cd333f770 | 38 | |
Strikewolf | 5:210cd333f770 | 39 | // human position |
Strikewolf | 5:210cd333f770 | 40 | Serial xbee(p13, p14); |
Strikewolf | 5:210cd333f770 | 41 | DigitalOut rst1(p15); |
Strikewolf | 5:210cd333f770 | 42 | |
Strikewolf | 5:210cd333f770 | 43 | /* mouse event handler */ |
Strikewolf | 5:210cd333f770 | 44 | void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) |
Strikewolf | 5:210cd333f770 | 45 | { |
Strikewolf | 5:210cd333f770 | 46 | // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch |
Strikewolf | 5:210cd333f770 | 47 | double y_temp = y_mickey / 232.6 * 100; |
Strikewolf | 5:210cd333f770 | 48 | double g_temp = imuFilter.getYaw(); |
Strikewolf | 5:210cd333f770 | 49 | |
Strikewolf | 5:210cd333f770 | 50 | // determine direction we are facing and add to that direction |
Strikewolf | 5:210cd333f770 | 51 | double dx = 0; |
Strikewolf | 5:210cd333f770 | 52 | double dy = 0; |
Strikewolf | 5:210cd333f770 | 53 | QUADRANT quadrant; |
Strikewolf | 5:210cd333f770 | 54 | if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90) |
Strikewolf | 5:210cd333f770 | 55 | quadrant = FORWARD_LEFT; |
Strikewolf | 5:210cd333f770 | 56 | if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90) |
Strikewolf | 5:210cd333f770 | 57 | quadrant = FORWARD_RIGHT; |
Strikewolf | 5:210cd333f770 | 58 | if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180) |
Strikewolf | 5:210cd333f770 | 59 | quadrant = BACKWARD_LEFT; |
Strikewolf | 5:210cd333f770 | 60 | if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180) |
Strikewolf | 5:210cd333f770 | 61 | quadrant = BACKWARD_RIGHT; |
Strikewolf | 5:210cd333f770 | 62 | |
Strikewolf | 5:210cd333f770 | 63 | switch (quadrant) { |
Strikewolf | 5:210cd333f770 | 64 | case FORWARD_LEFT: |
Strikewolf | 5:210cd333f770 | 65 | dy = y_temp * cos(g_temp); |
Strikewolf | 5:210cd333f770 | 66 | dx = -y_temp * sin(g_temp); |
Strikewolf | 5:210cd333f770 | 67 | break; |
Strikewolf | 5:210cd333f770 | 68 | case FORWARD_RIGHT: |
Strikewolf | 5:210cd333f770 | 69 | dy = y_temp * cos(g_temp); |
Strikewolf | 5:210cd333f770 | 70 | dx = -y_temp * sin(g_temp); |
Strikewolf | 5:210cd333f770 | 71 | break; |
Strikewolf | 5:210cd333f770 | 72 | case BACKWARD_LEFT: |
Strikewolf | 5:210cd333f770 | 73 | dy = -y_temp * sin(g_temp); |
Strikewolf | 5:210cd333f770 | 74 | dx = y_temp * cos(g_temp); |
Strikewolf | 5:210cd333f770 | 75 | break; |
Strikewolf | 5:210cd333f770 | 76 | case BACKWARD_RIGHT: |
Strikewolf | 5:210cd333f770 | 77 | dy = y_temp * sin(g_temp); |
Strikewolf | 5:210cd333f770 | 78 | dx = -y_temp * cos(g_temp); |
Strikewolf | 5:210cd333f770 | 79 | break; |
Strikewolf | 5:210cd333f770 | 80 | } |
Strikewolf | 5:210cd333f770 | 81 | |
Strikewolf | 6:3fb9f96765f6 | 82 | // add to total position |
Strikewolf | 5:210cd333f770 | 83 | x_position += dx; |
Strikewolf | 5:210cd333f770 | 84 | y_position += dy; |
Strikewolf | 5:210cd333f770 | 85 | |
Strikewolf | 5:210cd333f770 | 86 | // check if human car is close enough to end game |
Strikewolf | 5:210cd333f770 | 87 | gameOver = isGameOver(x_hum, y_hum, x_position, y_position); |
Strikewolf | 5:210cd333f770 | 88 | if(gameOver) { |
Strikewolf | 5:210cd333f770 | 89 | // game is over at this point |
Strikewolf | 5:210cd333f770 | 90 | xbee.putc('d'); |
Strikewolf | 5:210cd333f770 | 91 | pc.printf("Game over sent!\n\r"); |
Strikewolf | 5:210cd333f770 | 92 | |
Strikewolf | 6:3fb9f96765f6 | 93 | // sanity |
Strikewolf | 6:3fb9f96765f6 | 94 | for(int i = 0; i < 500; i++) |
Strikewolf | 6:3fb9f96765f6 | 95 | xbee.putc('d'); |
Strikewolf | 6:3fb9f96765f6 | 96 | |
Strikewolf | 5:210cd333f770 | 97 | // go to end game routine |
Strikewolf | 5:210cd333f770 | 98 | endGame(); |
Strikewolf | 5:210cd333f770 | 99 | } |
Strikewolf | 5:210cd333f770 | 100 | } |
Strikewolf | 5:210cd333f770 | 101 | |
Strikewolf | 5:210cd333f770 | 102 | /* positioning system thread function */ |
Strikewolf | 5:210cd333f770 | 103 | void PositionSystemMain(void const *) |
Strikewolf | 5:210cd333f770 | 104 | { |
Strikewolf | 5:210cd333f770 | 105 | |
Strikewolf | 5:210cd333f770 | 106 | USBHostMouse mouse; |
Strikewolf | 5:210cd333f770 | 107 | |
Strikewolf | 5:210cd333f770 | 108 | while(!gameOver) { |
Strikewolf | 5:210cd333f770 | 109 | // try to connect a USB mouse |
Strikewolf | 5:210cd333f770 | 110 | while(!mouse.connect() && !gameOver) |
Strikewolf | 5:210cd333f770 | 111 | Thread::wait(500); |
Strikewolf | 5:210cd333f770 | 112 | |
Strikewolf | 5:210cd333f770 | 113 | // when connected, attach handler called on mouse event |
Strikewolf | 5:210cd333f770 | 114 | mouse.attachEvent(onMouseEvent); |
Strikewolf | 5:210cd333f770 | 115 | |
Strikewolf | 5:210cd333f770 | 116 | // wait until the mouse is disconnected |
Strikewolf | 5:210cd333f770 | 117 | while(mouse.connected() && !gameOver) |
Strikewolf | 5:210cd333f770 | 118 | Thread::wait(500); |
Strikewolf | 5:210cd333f770 | 119 | } |
Strikewolf | 5:210cd333f770 | 120 | } |
Strikewolf | 5:210cd333f770 | 121 | |
Strikewolf | 5:210cd333f770 | 122 | void receivePosition(void const *) |
Strikewolf | 5:210cd333f770 | 123 | { |
Strikewolf | 6:3fb9f96765f6 | 124 | // temp variables |
Strikewolf | 5:210cd333f770 | 125 | char buffer[SERIAL_BUFFER_SIZE]; |
Strikewolf | 5:210cd333f770 | 126 | int index = 0; |
Strikewolf | 5:210cd333f770 | 127 | int xt = 0; |
Strikewolf | 5:210cd333f770 | 128 | int yt = 0; |
Strikewolf | 5:210cd333f770 | 129 | |
Strikewolf | 5:210cd333f770 | 130 | // wait for start character |
Strikewolf | 5:210cd333f770 | 131 | while(xbee.readable() && xbee.getc() != 'x' && !gameOver); |
Strikewolf | 5:210cd333f770 | 132 | |
Strikewolf | 5:210cd333f770 | 133 | // receive data packet of size PACKET_SIZE bytes |
Strikewolf | 5:210cd333f770 | 134 | pc.printf("Receiving...\n\r"); |
Strikewolf | 5:210cd333f770 | 135 | index = 0; |
Strikewolf | 5:210cd333f770 | 136 | while(index < PACKET_SIZE && !gameOver) { |
Strikewolf | 5:210cd333f770 | 137 | if(xbee.readable()) |
Strikewolf | 5:210cd333f770 | 138 | buffer[index++] = xbee.getc(); |
Strikewolf | 5:210cd333f770 | 139 | } |
Strikewolf | 5:210cd333f770 | 140 | buffer[index] = NULL; |
Strikewolf | 5:210cd333f770 | 141 | |
Strikewolf | 5:210cd333f770 | 142 | // reassemble data |
Strikewolf | 5:210cd333f770 | 143 | xt = buffer[1]; |
Strikewolf | 5:210cd333f770 | 144 | xt = xt << 8; |
Strikewolf | 5:210cd333f770 | 145 | xt = xt | buffer[2]; |
Strikewolf | 5:210cd333f770 | 146 | xt = xt << 8; |
Strikewolf | 5:210cd333f770 | 147 | xt = xt | buffer[3]; |
Strikewolf | 5:210cd333f770 | 148 | xt = xt << 8; |
Strikewolf | 5:210cd333f770 | 149 | xt = xt | buffer[4]; |
Strikewolf | 5:210cd333f770 | 150 | x_hum = (double) xt; |
Strikewolf | 5:210cd333f770 | 151 | |
Strikewolf | 5:210cd333f770 | 152 | yt = buffer[6]; |
Strikewolf | 5:210cd333f770 | 153 | yt = yt << 8; |
Strikewolf | 5:210cd333f770 | 154 | yt = yt | buffer[7]; |
Strikewolf | 5:210cd333f770 | 155 | yt = yt << 8; |
Strikewolf | 5:210cd333f770 | 156 | yt = yt | buffer[8]; |
Strikewolf | 5:210cd333f770 | 157 | yt = yt << 8; |
Strikewolf | 5:210cd333f770 | 158 | yt = yt | buffer[9]; |
Strikewolf | 5:210cd333f770 | 159 | y_hum = (double) yt; |
Strikewolf | 5:210cd333f770 | 160 | |
Strikewolf | 5:210cd333f770 | 161 | pc.printf("Recieve complete: %d, %d\n\r", (int) x_hum, (int) y_hum); |
Strikewolf | 5:210cd333f770 | 162 | } |