Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.
Dependencies: ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed
Fork of Project by
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); +}