File content as of revision 5:210cd333f770:

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