Code to control a Traxster robot using a Wimote and Android app
Fork of BlueUSB by
Diff: WiiControl.cpp
- Revision:
- 1:accdaa84fe8d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WiiControl.cpp Mon Apr 28 16:06:32 2014 +0000 @@ -0,0 +1,154 @@ +#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(); +}