Code to control a Traxster robot using a Wimote and Android app
Fork of BlueUSB by
WiiControl.cpp
- Committer:
- aswild
- Date:
- 2014-04-28
- Revision:
- 1:accdaa84fe8d
File content as of revision 1:accdaa84fe8d:
#include "mbed.h" #define ACC_LEFT 600 #define ACC_RIGHT 430 #define ACC_CENTERL 490 #define ACC_CENTERH 530 #define ACC_C_U_L 560 #define ACC_C_U_R 470 #define BASE_SPEED 30 #define X_MAX 610 #define X_MIN 410 #define X_UPPER_MID 530 #define X_LOWER_MID 490 #define SPEED_BASE 30 #define SPEED_BOOST 20 #define SPEED_TURBO 100 extern Serial rob, pc; extern time_t lastReportTime; DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); AnalogIn ir(p20); void MoveRobot(); void ControlRobot(int, int); int GetMotorSpeeds(int y, int x, bool turbo); typedef struct { bool a, b, up, down, left, right, one, two; int x, y, z; } WiimoteState; WiimoteState state; void ProcessHID(const unsigned char *data) { lastReportTime = time(NULL); led3 = 0; char b1 = data[2]; char b2 = data[3]; state.a = (bool)(b2 & 8); state.b = (bool)(b2 & 4); state.up = (bool)(b1 & 8); state.down = (bool)(b1 & 4); state.left = (bool)(b1 & 1); state.right = (bool)(b1 & 2); state.one = (bool)(b2 & 2); state.two = (bool)(b2 & 1); state.x = (data[4] << 2) | ((b1 & 0x60) >> 5); state.y = (data[5] << 2) | ((b2 & 0x20) >> 4); state.z = (data[6] << 2) | ((b2 & 0x40) >> 5); MoveRobot(); } int GetMotorSpeeds(int y, int x, bool turbo) { int speed = turbo ? SPEED_TURBO : SPEED_BASE; int m1, m2; float yscale, xscale; if (x > X_UPPER_MID) { if (x > X_MAX) x = X_MAX; xscale = (1.0*x - X_UPPER_MID) / (X_MAX - X_UPPER_MID); speed += (int)(xscale * SPEED_BOOST); } else if (x < X_LOWER_MID) { if (x < X_MIN) x = X_MIN; xscale = (1.0*x - X_LOWER_MID) / (X_MIN - X_LOWER_MID); speed -= (int)(xscale * SPEED_BOOST); } if (y >= ACC_CENTERL && y <= ACC_CENTERH) { m1 = m2 = speed; } else if (y > ACC_CENTERH) // turning left { if (y > ACC_LEFT) y = ACC_LEFT; yscale = (1.0*y - ACC_CENTERH) / (ACC_LEFT - ACC_CENTERH); yscale *= speed; m1 = speed; m2 = (int)(speed - yscale); } else // turning right { if (y < ACC_RIGHT) y = ACC_RIGHT; yscale = (1.0*y - ACC_CENTERL) / (ACC_RIGHT - ACC_CENTERL); yscale *= speed; m1 = (int)(speed - yscale); m2 = speed; } return ((m1&0xFF)<<8) | (m2 & 0xFF); } void MoveRobot() { int turnspeed = state.b ? SPEED_TURBO : SPEED_BASE; bool wall = ir.read() > (2.2 / 3.3); led4 = wall; if (state.up) ControlRobot(turnspeed, -1*turnspeed); else if (state.down) ControlRobot(-1*turnspeed, turnspeed); else if (state.one || state.two) { int dir = state.two ? 1 : -1; int speeds = GetMotorSpeeds(state.y, state.x, state.b); int m1 = speeds >> 8; int m2 = speeds & 0xFF; if (wall && dir == 1) ControlRobot(0,0); else ControlRobot(m1*dir, m2*dir); } else ControlRobot(0,0); } void ControlRobot(int m1, int m2) { if (m1 == 0 && m2 == 0) { led1 = 0; rob.puts("stop\r"); } else { led1 = 1; rob.printf("mogo 1:%d 2:%d\r", m1, m2); } while (rob.readable()) rob.getc(); }