Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_HumanDriver

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 Ganesh Subramaniam

AIPosition.h@5:210cd333f770, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 05:53:51 2014 +0000
Revision:
5:210cd333f770
Child:
6:3fb9f96765f6
works without any motors

Who changed what in which revision?

UserRevisionLine numberNew 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 //y_position += y_temp;
Strikewolf 5:210cd333f770 52 //x_position += y_temp * tan(g_temp);
Strikewolf 5:210cd333f770 53 double dx = 0;
Strikewolf 5:210cd333f770 54 double dy = 0;
Strikewolf 5:210cd333f770 55 QUADRANT quadrant;
Strikewolf 5:210cd333f770 56 if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90)
Strikewolf 5:210cd333f770 57 quadrant = FORWARD_LEFT;
Strikewolf 5:210cd333f770 58 if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90)
Strikewolf 5:210cd333f770 59 quadrant = FORWARD_RIGHT;
Strikewolf 5:210cd333f770 60 if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180)
Strikewolf 5:210cd333f770 61 quadrant = BACKWARD_LEFT;
Strikewolf 5:210cd333f770 62 if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180)
Strikewolf 5:210cd333f770 63 quadrant = BACKWARD_RIGHT;
Strikewolf 5:210cd333f770 64
Strikewolf 5:210cd333f770 65 switch (quadrant) {
Strikewolf 5:210cd333f770 66 case FORWARD_LEFT:
Strikewolf 5:210cd333f770 67 dy = y_temp * cos(g_temp);
Strikewolf 5:210cd333f770 68 dx = -y_temp * sin(g_temp);
Strikewolf 5:210cd333f770 69 break;
Strikewolf 5:210cd333f770 70 case FORWARD_RIGHT:
Strikewolf 5:210cd333f770 71 dy = y_temp * cos(g_temp);
Strikewolf 5:210cd333f770 72 dx = -y_temp * sin(g_temp);
Strikewolf 5:210cd333f770 73 break;
Strikewolf 5:210cd333f770 74 case BACKWARD_LEFT:
Strikewolf 5:210cd333f770 75 dy = -y_temp * sin(g_temp);
Strikewolf 5:210cd333f770 76 dx = y_temp * cos(g_temp);
Strikewolf 5:210cd333f770 77 break;
Strikewolf 5:210cd333f770 78 case BACKWARD_RIGHT:
Strikewolf 5:210cd333f770 79 dy = y_temp * sin(g_temp);
Strikewolf 5:210cd333f770 80 dx = -y_temp * cos(g_temp);
Strikewolf 5:210cd333f770 81 break;
Strikewolf 5:210cd333f770 82 }
Strikewolf 5:210cd333f770 83
Strikewolf 5:210cd333f770 84 x_position += dx;
Strikewolf 5:210cd333f770 85 y_position += dy;
Strikewolf 5:210cd333f770 86
Strikewolf 5:210cd333f770 87 // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp));
Strikewolf 5:210cd333f770 88 // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant);
Strikewolf 5:210cd333f770 89 // 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 5:210cd333f770 90
Strikewolf 5:210cd333f770 91 // check if human car is close enough to end game
Strikewolf 5:210cd333f770 92 gameOver = isGameOver(x_hum, y_hum, x_position, y_position);
Strikewolf 5:210cd333f770 93 if(gameOver) {
Strikewolf 5:210cd333f770 94 // game is over at this point
Strikewolf 5:210cd333f770 95 xbee.putc('d');
Strikewolf 5:210cd333f770 96 pc.printf("Game over sent!\n\r");
Strikewolf 5:210cd333f770 97
Strikewolf 5:210cd333f770 98 // go to end game routine
Strikewolf 5:210cd333f770 99 endGame();
Strikewolf 5:210cd333f770 100 }
Strikewolf 5:210cd333f770 101 }
Strikewolf 5:210cd333f770 102
Strikewolf 5:210cd333f770 103 /* positioning system thread function */
Strikewolf 5:210cd333f770 104 void PositionSystemMain(void const *)
Strikewolf 5:210cd333f770 105 {
Strikewolf 5:210cd333f770 106
Strikewolf 5:210cd333f770 107 USBHostMouse mouse;
Strikewolf 5:210cd333f770 108
Strikewolf 5:210cd333f770 109 while(!gameOver) {
Strikewolf 5:210cd333f770 110 // try to connect a USB mouse
Strikewolf 5:210cd333f770 111 while(!mouse.connect() && !gameOver)
Strikewolf 5:210cd333f770 112 Thread::wait(500);
Strikewolf 5:210cd333f770 113
Strikewolf 5:210cd333f770 114 // when connected, attach handler called on mouse event
Strikewolf 5:210cd333f770 115 mouse.attachEvent(onMouseEvent);
Strikewolf 5:210cd333f770 116
Strikewolf 5:210cd333f770 117 // wait until the mouse is disconnected
Strikewolf 5:210cd333f770 118 while(mouse.connected() && !gameOver)
Strikewolf 5:210cd333f770 119 Thread::wait(500);
Strikewolf 5:210cd333f770 120 }
Strikewolf 5:210cd333f770 121 }
Strikewolf 5:210cd333f770 122
Strikewolf 5:210cd333f770 123 void receivePosition(void const *)
Strikewolf 5:210cd333f770 124 {
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 // clear any garbage
Strikewolf 5:210cd333f770 131 // xbee.getc();
Strikewolf 5:210cd333f770 132 //xbee.getc();
Strikewolf 5:210cd333f770 133
Strikewolf 5:210cd333f770 134 // while(!gameOver) {
Strikewolf 5:210cd333f770 135
Strikewolf 5:210cd333f770 136 // wait for start character
Strikewolf 5:210cd333f770 137 while(xbee.readable() && xbee.getc() != 'x' && !gameOver);
Strikewolf 5:210cd333f770 138
Strikewolf 5:210cd333f770 139 // receive data packet of size PACKET_SIZE bytes
Strikewolf 5:210cd333f770 140 pc.printf("Receiving...\n\r");
Strikewolf 5:210cd333f770 141 index = 0;
Strikewolf 5:210cd333f770 142 while(index < PACKET_SIZE && !gameOver) {
Strikewolf 5:210cd333f770 143 if(xbee.readable())
Strikewolf 5:210cd333f770 144 buffer[index++] = xbee.getc();
Strikewolf 5:210cd333f770 145 }
Strikewolf 5:210cd333f770 146 buffer[index] = NULL;
Strikewolf 5:210cd333f770 147
Strikewolf 5:210cd333f770 148 // reassemble data
Strikewolf 5:210cd333f770 149 xt = buffer[1];
Strikewolf 5:210cd333f770 150 xt = xt << 8;
Strikewolf 5:210cd333f770 151 xt = xt | buffer[2];
Strikewolf 5:210cd333f770 152 xt = xt << 8;
Strikewolf 5:210cd333f770 153 xt = xt | buffer[3];
Strikewolf 5:210cd333f770 154 xt = xt << 8;
Strikewolf 5:210cd333f770 155 xt = xt | buffer[4];
Strikewolf 5:210cd333f770 156 x_hum = (double) xt;
Strikewolf 5:210cd333f770 157
Strikewolf 5:210cd333f770 158 yt = buffer[6];
Strikewolf 5:210cd333f770 159 yt = yt << 8;
Strikewolf 5:210cd333f770 160 yt = yt | buffer[7];
Strikewolf 5:210cd333f770 161 yt = yt << 8;
Strikewolf 5:210cd333f770 162 yt = yt | buffer[8];
Strikewolf 5:210cd333f770 163 yt = yt << 8;
Strikewolf 5:210cd333f770 164 yt = yt | buffer[9];
Strikewolf 5:210cd333f770 165 y_hum = (double) yt;
Strikewolf 5:210cd333f770 166
Strikewolf 5:210cd333f770 167 pc.printf("Recieve complete: %d, %d\n\r", (int) x_hum, (int) y_hum);
Strikewolf 5:210cd333f770 168 }