Justin Eng / Mbed 2 deprecated RoboticsCatAndMouse_AutonomousDriver

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

HumanPosition.h@3:0a6e4d139b86, 2014-04-30 (annotated)

Committer:
Strikewolf
Date:
Wed Apr 30 12:25:47 2014 +0000
Revision:
3:0a6e4d139b86
Parent:
1:dacf7db790f6 
Final product before demo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Strikewolf 1:dacf7db790f6 1 #include <math.h>
Strikewolf 1:dacf7db790f6 2 #include "USBHostMouse.h"
Strikewolf 1:dacf7db790f6 3
Strikewolf 1:dacf7db790f6 4 #define PFLAG_ON 0
Strikewolf 1:dacf7db790f6 5 #define PFLAG_OFF 1
Strikewolf 1:dacf7db790f6 6 #define PFLAG_CALIB 2
Strikewolf 1:dacf7db790f6 7
Strikewolf 1:dacf7db790f6 8 #define SERIAL_BUFFER_SIZE 20
Strikewolf 1:dacf7db790f6 9 #define PACKET_SIZE 10
Strikewolf 1:dacf7db790f6 10
Strikewolf 1:dacf7db790f6 11 #define X_HUMAN_UPPER 1
Strikewolf 1:dacf7db790f6 12 #define X_HUMAN_LOWER 2
Strikewolf 1:dacf7db790f6 13 #define Y_HUMAN_UPPER 4
Strikewolf 1:dacf7db790f6 14 #define Y_HUMAN_LOWER 5
Strikewolf 1:dacf7db790f6 15
Strikewolf 1:dacf7db790f6 16 //Convert from radians to degrees.
Strikewolf 1:dacf7db790f6 17 #define toDegrees(x) (x * 57.2957795)
Strikewolf 1:dacf7db790f6 18
Strikewolf 1:dacf7db790f6 19 //Quadrant enum
Strikewolf 1:dacf7db790f6 20 typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT;
Strikewolf 1:dacf7db790f6 21
Strikewolf 1:dacf7db790f6 22 // updates xy position if on, does nothing if off
Strikewolf 1:dacf7db790f6 23 extern char PFlag = PFLAG_ON;
Strikewolf 1:dacf7db790f6 24
Strikewolf 1:dacf7db790f6 25 // variables to keep track of coordinate position
Strikewolf 1:dacf7db790f6 26 double x_position = 0;
Strikewolf 1:dacf7db790f6 27 double y_position = 0;
Strikewolf 1:dacf7db790f6 28
Strikewolf 1:dacf7db790f6 29 // human position
Strikewolf 1:dacf7db790f6 30 Serial xbee(p13, p14);
Strikewolf 1:dacf7db790f6 31 DigitalOut rst1(p15);
Strikewolf 1:dacf7db790f6 32
Strikewolf 1:dacf7db790f6 33 /* mouse event handler */
Strikewolf 1:dacf7db790f6 34 void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z)
Strikewolf 1:dacf7db790f6 35 {
Strikewolf 1:dacf7db790f6 36 // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
Strikewolf 1:dacf7db790f6 37 double y_temp = y_mickey / 232.6 * 100;
Strikewolf 1:dacf7db790f6 38 double g_temp = imuFilter.getYaw();
Strikewolf 1:dacf7db790f6 39
Strikewolf 1:dacf7db790f6 40 // determine direction we are facing and add to that direction
Strikewolf 1:dacf7db790f6 41 //y_position += y_temp;
Strikewolf 1:dacf7db790f6 42 //x_position += y_temp * tan(g_temp);
Strikewolf 1:dacf7db790f6 43 double dx = 0;
Strikewolf 1:dacf7db790f6 44 double dy = 0;
Strikewolf 1:dacf7db790f6 45 QUADRANT quadrant;
Strikewolf 1:dacf7db790f6 46 if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90)
Strikewolf 1:dacf7db790f6 47 quadrant = FORWARD_LEFT;
Strikewolf 1:dacf7db790f6 48 if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90)
Strikewolf 1:dacf7db790f6 49 quadrant = FORWARD_RIGHT;
Strikewolf 1:dacf7db790f6 50 if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180)
Strikewolf 1:dacf7db790f6 51 quadrant = BACKWARD_LEFT;
Strikewolf 1:dacf7db790f6 52 if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180)
Strikewolf 1:dacf7db790f6 53 quadrant = BACKWARD_RIGHT;
Strikewolf 1:dacf7db790f6 54
Strikewolf 1:dacf7db790f6 55 switch (quadrant) {
Strikewolf 1:dacf7db790f6 56 case FORWARD_LEFT:
Strikewolf 1:dacf7db790f6 57 dy = y_temp * cos(g_temp);
Strikewolf 1:dacf7db790f6 58 dx = -y_temp * sin(g_temp);
Strikewolf 1:dacf7db790f6 59 break;
Strikewolf 1:dacf7db790f6 60 case FORWARD_RIGHT:
Strikewolf 1:dacf7db790f6 61 dy = y_temp * cos(g_temp);
Strikewolf 1:dacf7db790f6 62 dx = -y_temp * sin(g_temp);
Strikewolf 1:dacf7db790f6 63 break;
Strikewolf 1:dacf7db790f6 64 case BACKWARD_LEFT:
Strikewolf 1:dacf7db790f6 65 dy = -y_temp * sin(g_temp);
Strikewolf 1:dacf7db790f6 66 dx = y_temp * cos(g_temp);
Strikewolf 1:dacf7db790f6 67 break;
Strikewolf 1:dacf7db790f6 68 case BACKWARD_RIGHT:
Strikewolf 1:dacf7db790f6 69 dy = y_temp * sin(g_temp);
Strikewolf 1:dacf7db790f6 70 dx = -y_temp * cos(g_temp);
Strikewolf 1:dacf7db790f6 71 break;
Strikewolf 1:dacf7db790f6 72 }
Strikewolf 1:dacf7db790f6 73
Strikewolf 1:dacf7db790f6 74 x_position += dx;
Strikewolf 1:dacf7db790f6 75 y_position += dy;
Strikewolf 1:dacf7db790f6 76
Strikewolf 1:dacf7db790f6 77 // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp));
Strikewolf 1:dacf7db790f6 78 // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant);
Strikewolf 1:dacf7db790f6 79 // 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);
Strikewolf 1:dacf7db790f6 80 }
Strikewolf 1:dacf7db790f6 81
Strikewolf 1:dacf7db790f6 82 /* positioning system thread function */
Strikewolf 1:dacf7db790f6 83 void PositionSystemMain(void const *)
Strikewolf 1:dacf7db790f6 84 {
Strikewolf 1:dacf7db790f6 85
Strikewolf 1:dacf7db790f6 86 USBHostMouse mouse;
Strikewolf 1:dacf7db790f6 87
Strikewolf 1:dacf7db790f6 88 while(!gameOver) {
Strikewolf 1:dacf7db790f6 89 // try to connect a USB mouse
Strikewolf 1:dacf7db790f6 90 while(!mouse.connect() && !gameOver)
Strikewolf 1:dacf7db790f6 91 Thread::wait(500);
Strikewolf 1:dacf7db790f6 92
Strikewolf 1:dacf7db790f6 93 // when connected, attach handler called on mouse event
Strikewolf 1:dacf7db790f6 94 mouse.attachEvent(onMouseEvent);
Strikewolf 1:dacf7db790f6 95
Strikewolf 1:dacf7db790f6 96 // wait until the mouse is disconnected
Strikewolf 1:dacf7db790f6 97 while(mouse.connected() && !gameOver)
Strikewolf 1:dacf7db790f6 98 Thread::wait(500);
Strikewolf 1:dacf7db790f6 99 }
Strikewolf 1:dacf7db790f6 100 }
Strikewolf 1:dacf7db790f6 101
Strikewolf 1:dacf7db790f6 102 void sendPosition(void const *)
Strikewolf 1:dacf7db790f6 103 {
Strikewolf 3:0a6e4d139b86 104 // temp variables
Strikewolf 1:dacf7db790f6 105 int x = 0;
Strikewolf 1:dacf7db790f6 106 int y = 0;
Strikewolf 1:dacf7db790f6 107 char a = 0;
Strikewolf 1:dacf7db790f6 108 char b = 0;
Strikewolf 1:dacf7db790f6 109 char c = 0;
Strikewolf 1:dacf7db790f6 110 char d = 0;
Strikewolf 1:dacf7db790f6 111
Strikewolf 1:dacf7db790f6 112 // sample current xy position
Strikewolf 1:dacf7db790f6 113 x = (int) x_position;
Strikewolf 1:dacf7db790f6 114 y = (int) y_position;
Strikewolf 1:dacf7db790f6 115
Strikewolf 1:dacf7db790f6 116 pc.printf("Sending: %d %d...\n\r", x, y);
Strikewolf 3:0a6e4d139b86 117
Strikewolf 3:0a6e4d139b86 118 // break down x coordinate by byte
Strikewolf 1:dacf7db790f6 119 a = x >> 24;
Strikewolf 1:dacf7db790f6 120 b = x >> 16;
Strikewolf 1:dacf7db790f6 121 c = x >> 8;
Strikewolf 1:dacf7db790f6 122 d = x;
Strikewolf 3:0a6e4d139b86 123
Strikewolf 3:0a6e4d139b86 124 // put each byte on the buffer
Strikewolf 1:dacf7db790f6 125 xbee.putc('x');
Strikewolf 3:0a6e4d139b86 126 xbee.putc(a);
Strikewolf 3:0a6e4d139b86 127 xbee.putc(b);
Strikewolf 1:dacf7db790f6 128 xbee.putc(c);
Strikewolf 1:dacf7db790f6 129 xbee.putc(d);
Strikewolf 1:dacf7db790f6 130
Strikewolf 3:0a6e4d139b86 131 // break down y coordinate by byte
Strikewolf 1:dacf7db790f6 132 a = y >> 24;
Strikewolf 1:dacf7db790f6 133 b = y >> 16;
Strikewolf 1:dacf7db790f6 134 c = y >> 8;
Strikewolf 1:dacf7db790f6 135 d = y;
Strikewolf 3:0a6e4d139b86 136
Strikewolf 3:0a6e4d139b86 137 // put each byte on the buffer
Strikewolf 1:dacf7db790f6 138 xbee.putc('y');
Strikewolf 3:0a6e4d139b86 139 xbee.putc(a);
Strikewolf 3:0a6e4d139b86 140 xbee.putc(b);
Strikewolf 1:dacf7db790f6 141 xbee.putc(c);
Strikewolf 1:dacf7db790f6 142 xbee.putc(d);
Strikewolf 1:dacf7db790f6 143
Strikewolf 1:dacf7db790f6 144 pc.printf("Send complete.\n\r");
Strikewolf 3:0a6e4d139b86 145
Strikewolf 3:0a6e4d139b86 146 // wait for a possible game over response
Strikewolf 1:dacf7db790f6 147 wait(0.25);
Strikewolf 1:dacf7db790f6 148 if(xbee.readable() && xbee.getc() == 'd') {
Strikewolf 1:dacf7db790f6 149 gameOver = true;
Strikewolf 1:dacf7db790f6 150 pc.printf("gameOver Received!\n\r");
Strikewolf 1:dacf7db790f6 151 endGame();
Strikewolf 1:dacf7db790f6 152 }
Strikewolf 1:dacf7db790f6 153 }