Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.
Dependencies: IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos
Diff: HumanPosition.h
- Revision:
- 1:dacf7db790f6
- Child:
- 3:0a6e4d139b86
diff -r 84d5aa80fd77 -r dacf7db790f6 HumanPosition.h --- /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