Diff: HumanPosition.h

Revision:

1:dacf7db790f6

Child:

3:0a6e4d139b86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HumanPosition.h	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,146 @@
+#include <math.h>
+#include "USBHostMouse.h"
+
+#define PFLAG_ON 0
+#define PFLAG_OFF 1
+#define PFLAG_CALIB 2
+
+#define SERIAL_BUFFER_SIZE 20
+#define PACKET_SIZE 10
+
+#define X_HUMAN_UPPER 1
+#define X_HUMAN_LOWER 2
+#define Y_HUMAN_UPPER 4
+#define Y_HUMAN_LOWER 5
+
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+
+//Quadrant enum
+typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT;
+
+// updates xy position if on, does nothing if off
+extern char PFlag = PFLAG_ON;
+
+// variables to keep track of coordinate position
+double x_position = 0;
+double y_position = 0;
+
+// human position
+Serial xbee(p13, p14);
+DigitalOut rst1(p15);
+
+/* mouse event handler */
+void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z)
+{
+    // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
+    double y_temp = y_mickey / 232.6 * 100;
+    double g_temp = imuFilter.getYaw();
+
+    // determine direction we are facing and add to that direction
+    //y_position += y_temp;
+    //x_position += y_temp * tan(g_temp);
+    double dx = 0;
+    double dy = 0;
+    QUADRANT quadrant;
+    if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90)
+        quadrant = FORWARD_LEFT;
+    if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90)
+        quadrant = FORWARD_RIGHT;
+    if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180)
+        quadrant = BACKWARD_LEFT;
+    if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180)
+        quadrant = BACKWARD_RIGHT;
+
+    switch (quadrant) {
+        case FORWARD_LEFT:
+            dy = y_temp * cos(g_temp);
+            dx = -y_temp * sin(g_temp);
+            break;
+        case FORWARD_RIGHT:
+            dy = y_temp * cos(g_temp);
+            dx = -y_temp * sin(g_temp);
+            break;
+        case BACKWARD_LEFT:
+            dy = -y_temp * sin(g_temp);
+            dx = y_temp * cos(g_temp);
+            break;
+        case BACKWARD_RIGHT:
+            dy = y_temp * sin(g_temp);
+            dx = -y_temp * cos(g_temp);
+            break;
+    }
+
+    x_position += dx;
+    y_position += dy;
+
+    // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp));
+    // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant);
+    // 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);
+}
+
+/* positioning system thread function */
+void PositionSystemMain(void const *)
+{
+
+    USBHostMouse mouse;
+
+    while(!gameOver) {
+        // try to connect a USB mouse
+        while(!mouse.connect() && !gameOver)
+            Thread::wait(500);
+
+        // when connected, attach handler called on mouse event
+        mouse.attachEvent(onMouseEvent);
+
+        // wait until the mouse is disconnected
+        while(mouse.connected() && !gameOver)
+            Thread::wait(500);
+    }
+}
+
+void sendPosition(void const *)
+{
+    int x = 0;
+    int y = 0;
+    char a = 0;
+    char b = 0;
+    char c = 0;
+    char d = 0;
+
+    // sample current xy position
+    x = (int) x_position;
+    y = (int) y_position;
+
+    // send current xy position to cpu bot
+    pc.printf("Sending: %d %d...\n\r", x, y);
+    a = x >> 24;
+    b = x >> 16;
+    c = x >> 8;
+    d = x;
+    // pc.printf("\ta: %x b: %x\n\r", a, b);
+    xbee.putc('x');
+    xbee.putc(a);   // send upper bits
+    xbee.putc(b);   // send lower bits
+    xbee.putc(c);
+    xbee.putc(d);
+
+    a = y >> 24;
+    b = y >> 16;
+    c = y >> 8;
+    d = y;
+    //pc.printf("\ta: %x b: %x\n\r", a, b);
+    xbee.putc('y');
+    xbee.putc(a);   // send upper bits
+    xbee.putc(b);   // send lower bits
+    xbee.putc(c);
+    xbee.putc(d);
+
+    pc.printf("Send complete.\n\r");
+    wait(0.25);
+    if(xbee.readable() && xbee.getc() == 'd') {
+        gameOver = true;
+        pc.printf("gameOver Received!\n\r");
+        endGame();
+    }
+}
\ No newline at end of file