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

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