File content as of revision 1:dacf7db790f6:

#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();
    }
}