Diff: AIPosition.h

Revision:

5:210cd333f770

Child:

6:3fb9f96765f6

diff -r b2a30c313297 -r 210cd333f770 AIPosition.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AIPosition.h	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,168 @@
+#include <math.h>
+#include "USBHostMouse.h"
+
+#define SONAR_STOP (2.0)
+
+#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;
+
+// sonar sensors
+AnalogIn sonar1(p16); // FL
+AnalogIn sonar2(p17); // FC
+AnalogIn sonar3(p18); // FR
+
+// 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;
+
+double x_hum = 123456;
+double y_hum = 123456;
+
+// 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);
+
+    // check if human car is close enough to end game
+    gameOver = isGameOver(x_hum, y_hum, x_position, y_position);
+    if(gameOver) {
+        // game is over at this point
+        xbee.putc('d');
+        pc.printf("Game over sent!\n\r");
+
+        // go to end game routine
+        endGame();
+    }
+}
+
+/* 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 receivePosition(void const *)
+{
+    char buffer[SERIAL_BUFFER_SIZE];
+    int index = 0;
+    int xt = 0;
+    int yt = 0;
+
+    // clear any garbage
+    // xbee.getc();
+    //xbee.getc();
+
+    // while(!gameOver) {
+
+    // wait for start character
+    while(xbee.readable() && xbee.getc() != 'x' && !gameOver);
+
+    // receive data packet of size PACKET_SIZE bytes
+    pc.printf("Receiving...\n\r");
+    index = 0;
+    while(index < PACKET_SIZE && !gameOver) {
+        if(xbee.readable())
+            buffer[index++] = xbee.getc();
+    }
+    buffer[index] = NULL;
+
+    // reassemble data
+    xt = buffer[1];
+    xt = xt << 8;
+    xt = xt | buffer[2];
+    xt = xt << 8;
+    xt = xt | buffer[3];
+    xt = xt << 8;
+    xt = xt | buffer[4];
+    x_hum = (double) xt;
+
+    yt = buffer[6];
+    yt = yt << 8;
+    yt = yt | buffer[7];
+    yt = yt << 8;
+    yt = yt | buffer[8];
+    yt = yt << 8;
+    yt = yt | buffer[9];
+    y_hum = (double) yt;
+
+    pc.printf("Recieve complete: %d, %d\n\r", (int) x_hum, (int) y_hum);
+}