Code to control a Traxster robot using a Wimote and Android app
Fork of BlueUSB by
WiiControl.cpp@1:accdaa84fe8d, 2014-04-28 (annotated)
- Committer:
- aswild
- Date:
- Mon Apr 28 16:06:32 2014 +0000
- Revision:
- 1:accdaa84fe8d
Wiimote controlled Traxster robot using BlueUSB
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aswild | 1:accdaa84fe8d | 1 | #include "mbed.h" |
aswild | 1:accdaa84fe8d | 2 | |
aswild | 1:accdaa84fe8d | 3 | #define ACC_LEFT 600 |
aswild | 1:accdaa84fe8d | 4 | #define ACC_RIGHT 430 |
aswild | 1:accdaa84fe8d | 5 | #define ACC_CENTERL 490 |
aswild | 1:accdaa84fe8d | 6 | #define ACC_CENTERH 530 |
aswild | 1:accdaa84fe8d | 7 | #define ACC_C_U_L 560 |
aswild | 1:accdaa84fe8d | 8 | #define ACC_C_U_R 470 |
aswild | 1:accdaa84fe8d | 9 | #define BASE_SPEED 30 |
aswild | 1:accdaa84fe8d | 10 | |
aswild | 1:accdaa84fe8d | 11 | #define X_MAX 610 |
aswild | 1:accdaa84fe8d | 12 | #define X_MIN 410 |
aswild | 1:accdaa84fe8d | 13 | #define X_UPPER_MID 530 |
aswild | 1:accdaa84fe8d | 14 | #define X_LOWER_MID 490 |
aswild | 1:accdaa84fe8d | 15 | |
aswild | 1:accdaa84fe8d | 16 | #define SPEED_BASE 30 |
aswild | 1:accdaa84fe8d | 17 | #define SPEED_BOOST 20 |
aswild | 1:accdaa84fe8d | 18 | #define SPEED_TURBO 100 |
aswild | 1:accdaa84fe8d | 19 | |
aswild | 1:accdaa84fe8d | 20 | extern Serial rob, pc; |
aswild | 1:accdaa84fe8d | 21 | extern time_t lastReportTime; |
aswild | 1:accdaa84fe8d | 22 | |
aswild | 1:accdaa84fe8d | 23 | DigitalOut led1(LED1); |
aswild | 1:accdaa84fe8d | 24 | DigitalOut led2(LED2); |
aswild | 1:accdaa84fe8d | 25 | DigitalOut led3(LED3); |
aswild | 1:accdaa84fe8d | 26 | DigitalOut led4(LED4); |
aswild | 1:accdaa84fe8d | 27 | |
aswild | 1:accdaa84fe8d | 28 | AnalogIn ir(p20); |
aswild | 1:accdaa84fe8d | 29 | |
aswild | 1:accdaa84fe8d | 30 | void MoveRobot(); |
aswild | 1:accdaa84fe8d | 31 | void ControlRobot(int, int); |
aswild | 1:accdaa84fe8d | 32 | int GetMotorSpeeds(int y, int x, bool turbo); |
aswild | 1:accdaa84fe8d | 33 | |
aswild | 1:accdaa84fe8d | 34 | typedef struct |
aswild | 1:accdaa84fe8d | 35 | { |
aswild | 1:accdaa84fe8d | 36 | bool a, b, up, down, left, right, one, two; |
aswild | 1:accdaa84fe8d | 37 | int x, y, z; |
aswild | 1:accdaa84fe8d | 38 | } WiimoteState; |
aswild | 1:accdaa84fe8d | 39 | |
aswild | 1:accdaa84fe8d | 40 | WiimoteState state; |
aswild | 1:accdaa84fe8d | 41 | |
aswild | 1:accdaa84fe8d | 42 | void ProcessHID(const unsigned char *data) |
aswild | 1:accdaa84fe8d | 43 | { |
aswild | 1:accdaa84fe8d | 44 | lastReportTime = time(NULL); |
aswild | 1:accdaa84fe8d | 45 | led3 = 0; |
aswild | 1:accdaa84fe8d | 46 | |
aswild | 1:accdaa84fe8d | 47 | char b1 = data[2]; |
aswild | 1:accdaa84fe8d | 48 | char b2 = data[3]; |
aswild | 1:accdaa84fe8d | 49 | |
aswild | 1:accdaa84fe8d | 50 | state.a = (bool)(b2 & 8); |
aswild | 1:accdaa84fe8d | 51 | state.b = (bool)(b2 & 4); |
aswild | 1:accdaa84fe8d | 52 | state.up = (bool)(b1 & 8); |
aswild | 1:accdaa84fe8d | 53 | state.down = (bool)(b1 & 4); |
aswild | 1:accdaa84fe8d | 54 | state.left = (bool)(b1 & 1); |
aswild | 1:accdaa84fe8d | 55 | state.right = (bool)(b1 & 2); |
aswild | 1:accdaa84fe8d | 56 | state.one = (bool)(b2 & 2); |
aswild | 1:accdaa84fe8d | 57 | state.two = (bool)(b2 & 1); |
aswild | 1:accdaa84fe8d | 58 | |
aswild | 1:accdaa84fe8d | 59 | state.x = (data[4] << 2) | ((b1 & 0x60) >> 5); |
aswild | 1:accdaa84fe8d | 60 | state.y = (data[5] << 2) | ((b2 & 0x20) >> 4); |
aswild | 1:accdaa84fe8d | 61 | state.z = (data[6] << 2) | ((b2 & 0x40) >> 5); |
aswild | 1:accdaa84fe8d | 62 | |
aswild | 1:accdaa84fe8d | 63 | MoveRobot(); |
aswild | 1:accdaa84fe8d | 64 | } |
aswild | 1:accdaa84fe8d | 65 | |
aswild | 1:accdaa84fe8d | 66 | int GetMotorSpeeds(int y, int x, bool turbo) |
aswild | 1:accdaa84fe8d | 67 | { |
aswild | 1:accdaa84fe8d | 68 | int speed = turbo ? SPEED_TURBO : SPEED_BASE; |
aswild | 1:accdaa84fe8d | 69 | int m1, m2; |
aswild | 1:accdaa84fe8d | 70 | float yscale, xscale; |
aswild | 1:accdaa84fe8d | 71 | |
aswild | 1:accdaa84fe8d | 72 | if (x > X_UPPER_MID) |
aswild | 1:accdaa84fe8d | 73 | { |
aswild | 1:accdaa84fe8d | 74 | if (x > X_MAX) |
aswild | 1:accdaa84fe8d | 75 | x = X_MAX; |
aswild | 1:accdaa84fe8d | 76 | xscale = (1.0*x - X_UPPER_MID) / (X_MAX - X_UPPER_MID); |
aswild | 1:accdaa84fe8d | 77 | speed += (int)(xscale * SPEED_BOOST); |
aswild | 1:accdaa84fe8d | 78 | } |
aswild | 1:accdaa84fe8d | 79 | else if (x < X_LOWER_MID) |
aswild | 1:accdaa84fe8d | 80 | { |
aswild | 1:accdaa84fe8d | 81 | if (x < X_MIN) |
aswild | 1:accdaa84fe8d | 82 | x = X_MIN; |
aswild | 1:accdaa84fe8d | 83 | xscale = (1.0*x - X_LOWER_MID) / (X_MIN - X_LOWER_MID); |
aswild | 1:accdaa84fe8d | 84 | speed -= (int)(xscale * SPEED_BOOST); |
aswild | 1:accdaa84fe8d | 85 | } |
aswild | 1:accdaa84fe8d | 86 | |
aswild | 1:accdaa84fe8d | 87 | if (y >= ACC_CENTERL && y <= ACC_CENTERH) |
aswild | 1:accdaa84fe8d | 88 | { |
aswild | 1:accdaa84fe8d | 89 | m1 = m2 = speed; |
aswild | 1:accdaa84fe8d | 90 | } |
aswild | 1:accdaa84fe8d | 91 | else if (y > ACC_CENTERH) // turning left |
aswild | 1:accdaa84fe8d | 92 | { |
aswild | 1:accdaa84fe8d | 93 | if (y > ACC_LEFT) |
aswild | 1:accdaa84fe8d | 94 | y = ACC_LEFT; |
aswild | 1:accdaa84fe8d | 95 | yscale = (1.0*y - ACC_CENTERH) / (ACC_LEFT - ACC_CENTERH); |
aswild | 1:accdaa84fe8d | 96 | yscale *= speed; |
aswild | 1:accdaa84fe8d | 97 | m1 = speed; |
aswild | 1:accdaa84fe8d | 98 | m2 = (int)(speed - yscale); |
aswild | 1:accdaa84fe8d | 99 | } |
aswild | 1:accdaa84fe8d | 100 | else // turning right |
aswild | 1:accdaa84fe8d | 101 | { |
aswild | 1:accdaa84fe8d | 102 | if (y < ACC_RIGHT) |
aswild | 1:accdaa84fe8d | 103 | y = ACC_RIGHT; |
aswild | 1:accdaa84fe8d | 104 | yscale = (1.0*y - ACC_CENTERL) / (ACC_RIGHT - ACC_CENTERL); |
aswild | 1:accdaa84fe8d | 105 | yscale *= speed; |
aswild | 1:accdaa84fe8d | 106 | m1 = (int)(speed - yscale); |
aswild | 1:accdaa84fe8d | 107 | m2 = speed; |
aswild | 1:accdaa84fe8d | 108 | } |
aswild | 1:accdaa84fe8d | 109 | |
aswild | 1:accdaa84fe8d | 110 | return ((m1&0xFF)<<8) | (m2 & 0xFF); |
aswild | 1:accdaa84fe8d | 111 | } |
aswild | 1:accdaa84fe8d | 112 | |
aswild | 1:accdaa84fe8d | 113 | void MoveRobot() |
aswild | 1:accdaa84fe8d | 114 | { |
aswild | 1:accdaa84fe8d | 115 | int turnspeed = state.b ? SPEED_TURBO : SPEED_BASE; |
aswild | 1:accdaa84fe8d | 116 | |
aswild | 1:accdaa84fe8d | 117 | bool wall = ir.read() > (2.2 / 3.3); |
aswild | 1:accdaa84fe8d | 118 | led4 = wall; |
aswild | 1:accdaa84fe8d | 119 | |
aswild | 1:accdaa84fe8d | 120 | if (state.up) |
aswild | 1:accdaa84fe8d | 121 | ControlRobot(turnspeed, -1*turnspeed); |
aswild | 1:accdaa84fe8d | 122 | else if (state.down) |
aswild | 1:accdaa84fe8d | 123 | ControlRobot(-1*turnspeed, turnspeed); |
aswild | 1:accdaa84fe8d | 124 | else if (state.one || state.two) |
aswild | 1:accdaa84fe8d | 125 | { |
aswild | 1:accdaa84fe8d | 126 | int dir = state.two ? 1 : -1; |
aswild | 1:accdaa84fe8d | 127 | int speeds = GetMotorSpeeds(state.y, state.x, state.b); |
aswild | 1:accdaa84fe8d | 128 | int m1 = speeds >> 8; |
aswild | 1:accdaa84fe8d | 129 | int m2 = speeds & 0xFF; |
aswild | 1:accdaa84fe8d | 130 | |
aswild | 1:accdaa84fe8d | 131 | if (wall && dir == 1) |
aswild | 1:accdaa84fe8d | 132 | ControlRobot(0,0); |
aswild | 1:accdaa84fe8d | 133 | else |
aswild | 1:accdaa84fe8d | 134 | ControlRobot(m1*dir, m2*dir); |
aswild | 1:accdaa84fe8d | 135 | } |
aswild | 1:accdaa84fe8d | 136 | else |
aswild | 1:accdaa84fe8d | 137 | ControlRobot(0,0); |
aswild | 1:accdaa84fe8d | 138 | } |
aswild | 1:accdaa84fe8d | 139 | |
aswild | 1:accdaa84fe8d | 140 | void ControlRobot(int m1, int m2) |
aswild | 1:accdaa84fe8d | 141 | { |
aswild | 1:accdaa84fe8d | 142 | if (m1 == 0 && m2 == 0) |
aswild | 1:accdaa84fe8d | 143 | { |
aswild | 1:accdaa84fe8d | 144 | led1 = 0; |
aswild | 1:accdaa84fe8d | 145 | rob.puts("stop\r"); |
aswild | 1:accdaa84fe8d | 146 | } |
aswild | 1:accdaa84fe8d | 147 | else |
aswild | 1:accdaa84fe8d | 148 | { |
aswild | 1:accdaa84fe8d | 149 | led1 = 1; |
aswild | 1:accdaa84fe8d | 150 | rob.printf("mogo 1:%d 2:%d\r", m1, m2); |
aswild | 1:accdaa84fe8d | 151 | } |
aswild | 1:accdaa84fe8d | 152 | while (rob.readable()) |
aswild | 1:accdaa84fe8d | 153 | rob.getc(); |
aswild | 1:accdaa84fe8d | 154 | } |