HumanPosition.h

00001 #include <math.h>
00002 #include "USBHostMouse.h"
00003 
00004 #define PFLAG_ON 0
00005 #define PFLAG_OFF 1
00006 #define PFLAG_CALIB 2
00007 
00008 #define SERIAL_BUFFER_SIZE 20
00009 #define PACKET_SIZE 10
00010 
00011 #define X_HUMAN_UPPER 1
00012 #define X_HUMAN_LOWER 2
00013 #define Y_HUMAN_UPPER 4
00014 #define Y_HUMAN_LOWER 5
00015 
00016 //Convert from radians to degrees.
00017 #define toDegrees(x) (x * 57.2957795)
00018 
00019 //Quadrant enum
00020 typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT;
00021 
00022 // updates xy position if on, does nothing if off
00023 extern char PFlag = PFLAG_ON;
00024 
00025 // variables to keep track of coordinate position
00026 double x_position = 0;
00027 double y_position = 0;
00028 
00029 // human position
00030 Serial xbee(p13, p14);
00031 DigitalOut rst1(p15);
00032 
00033 /* mouse event handler */
00034 void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z)
00035 {
00036     // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
00037     double y_temp = y_mickey / 232.6 * 100;
00038     double g_temp = imuFilter.getYaw();
00039 
00040     // determine direction we are facing and add to that direction
00041     //y_position += y_temp;
00042     //x_position += y_temp * tan(g_temp);
00043     double dx = 0;
00044     double dy = 0;
00045     QUADRANT quadrant;
00046     if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90)
00047         quadrant = FORWARD_LEFT;
00048     if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90)
00049         quadrant = FORWARD_RIGHT;
00050     if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180)
00051         quadrant = BACKWARD_LEFT;
00052     if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180)
00053         quadrant = BACKWARD_RIGHT;
00054 
00055     switch (quadrant) {
00056         case FORWARD_LEFT:
00057             dy = y_temp * cos(g_temp);
00058             dx = -y_temp * sin(g_temp);
00059             break;
00060         case FORWARD_RIGHT:
00061             dy = y_temp * cos(g_temp);
00062             dx = -y_temp * sin(g_temp);
00063             break;
00064         case BACKWARD_LEFT:
00065             dy = -y_temp * sin(g_temp);
00066             dx = y_temp * cos(g_temp);
00067             break;
00068         case BACKWARD_RIGHT:
00069             dy = y_temp * sin(g_temp);
00070             dx = -y_temp * cos(g_temp);
00071             break;
00072     }
00073 
00074     x_position += dx;
00075     y_position += dy;
00076 
00077     // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp));
00078     // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant);
00079     // 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);
00080 }
00081 
00082 /* positioning system thread function */
00083 void PositionSystemMain(void const *)
00084 {
00085 
00086     USBHostMouse mouse;
00087 
00088     while(!gameOver) {
00089         // try to connect a USB mouse
00090         while(!mouse.connect() && !gameOver)
00091             Thread::wait(500);
00092 
00093         // when connected, attach handler called on mouse event
00094         mouse.attachEvent(onMouseEvent);
00095 
00096         // wait until the mouse is disconnected
00097         while(mouse.connected() && !gameOver)
00098             Thread::wait(500);
00099     }
00100 }
00101 
00102 void sendPosition(void const *)
00103 {
00104     // temp variables
00105     int x = 0;
00106     int y = 0;
00107     char a = 0;
00108     char b = 0;
00109     char c = 0;
00110     char d = 0;
00111 
00112     // sample current xy position
00113     x = (int) x_position;
00114     y = (int) y_position;
00115 
00116     pc.printf("Sending: %d %d...\n\r", x, y);
00117     
00118     // break down x coordinate by byte
00119     a = x >> 24;
00120     b = x >> 16;
00121     c = x >> 8;
00122     d = x;
00123     
00124     // put each byte on the buffer
00125     xbee.putc('x');
00126     xbee.putc(a);   
00127     xbee.putc(b); 
00128     xbee.putc(c);
00129     xbee.putc(d);
00130 
00131     // break down y coordinate by byte
00132     a = y >> 24;
00133     b = y >> 16;
00134     c = y >> 8;
00135     d = y;
00136   
00137     // put each byte on the buffer
00138     xbee.putc('y');
00139     xbee.putc(a); 
00140     xbee.putc(b); 
00141     xbee.putc(c);
00142     xbee.putc(d);
00143 
00144     pc.printf("Send complete.\n\r");
00145     
00146     // wait for a possible game over response
00147     wait(0.25);
00148     if(xbee.readable() && xbee.getc() == 'd') {
00149         gameOver = true;
00150         pc.printf("gameOver Received!\n\r");
00151         endGame();
00152     }
00153 }