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();
}