Diff: WiiControl.cpp

Revision:

1:accdaa84fe8d

diff -r 606b230e5b4a -r accdaa84fe8d WiiControl.cpp
--- /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();
+}