Allen Wild / Mbed 2 deprecated 4180_Wiibot

Code to control a Traxster robot using a Wimote and Android app

Dependencies:   mbed

Fork of BlueUSB by Peter Barrett

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?

UserRevisionLine numberNew 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 }