File content as of revision 4:f1d2000776c6:

#include "mbed.h"
#include "omni.h"
#include "MotorDriverController.h"
#include "pin_config.h"
#include "FEP.h"

//#define DEBUG 

//I2C i2c(i2c_sda,i2c_scl);
MDC motor(i2c_sda,i2c_scl);
Omni omni(4, 135);
FEP fep(PB_10,PB_11);
DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
PwmOut pwm(PA_6);

void init()
{
    for(int i = 0;i<4;i++) {
        leds[i] = 0;
    }
//    for(int j = 0; j < 4; j++) {
//        motor[j].setAddr(7, j);
//    }
}

int main()
{
    pwm.period(0.00005);
    char data[10] = { 0 };
    int  i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
    uint8_t fep_temp;
    double polarVector[2];
    double Xstick,Ystick;
    double radian;
    double deg = 360;
    pwm = 0.5;
    for(int j = 0; j < 4; j++) {
        motor.write(7,j,0);
    }
    
    while(1) {
        fep_temp=fep.read_txt(data,6);
        if(fep_temp==FEP_RX_SUCCESS) {

#ifdef DEBUG
            pc.printf("Recieve succeeded,%s\r\n",data);
#endif

            error_val = 0;
            leds[0] = 0;
            tem[0] = data[4];
            tem[1] = data[5];
            for(i = 0; i < 7; i++) {
                Button1[i] = tem[0] % 2;
                tem[0] /= 2;

#ifdef DEBUG
                pc.printf("%d,",Button1[i]);
#endif

            }
            for(i = 0; i < 6; i++) {
                Button2[i] = tem[1] % 2;
                tem[1] /= 2;

#ifdef DEBUG
                pc.printf("%d,",Button2[i]);
#endif
            }

#ifdef DEBUG
            pc.printf("\r\n");
#endif 
        } else if(fep_temp==FEP_NO_RESPONSE) {

#ifdef DEBUG
            pc.printf("No response\r\n");
#endif
            leds[0] = 1;
            continue;
        } else {

#ifdef DEBUG
            pc.printf("Recieve failed\r\n");
#endif
            leds[0] = 1;
            error_val++;
        }
        if(error_val < 4) {
            /*
            if (data[0]>100 && data[0]<150 && data[1]>100 && data[1]<150) {
                omni.stop();
                polarVector[0] = 360;
                polarVector[1] = 0;
                leds[1] = 1;
            } else {
                leds[1] = 0;
                Xstick = (double)(data[0]/255.0)*2.0-1.0;
                Ystick = (double)(data[1]/255.0)*2.0-1.0;
                radian = atan2(-(Ystick),-(Xstick))+PI/2.0;
                deg = (radian*180/PI-20);
            }
            */
            polarVector[1] = 0.3;
            if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) {
                deg = 270;
                leds[1] = 0;
            }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) {
                deg = 180;
                leds[1] = 0;
            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) {
                deg = 90;
                leds[1] = 0;
            }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) {
                deg = 0;
                leds[1] = 0;
            }else {
                deg = 360;
                polarVector[1] = 0;
                leds[1] = 1;
            }
            polarVector[0] = deg;
            if(data[2] < 50) {
                omni.computePolar(polarVector, 0.15);
                leds[2] = 1;
                leds[3] = 0;
            } else if(data[2] > 200) {
                omni.computePolar(polarVector, -0.15);
                leds[2] = 0;
                leds[3] = 1;
            } else {
                omni.computePolar(polarVector, 0);
                leds[2] = 0;
                leds[3] = 0;
            }
            
            for(int j = 0; j < 4; j++) {
                motor.write(7,j,omni.getOutput(j));
            }
            if(Button2[0] == 1 && Button2[1] == 0) {
                pwm = 0.3;
            } else if(Button2[0] == 0 && Button2[1] == 1) {
                pwm = 0.7;
            } else {
                pwm = 0.5;
            }
        } else {
            omni.stop();
            for(int j = 0; j < 4; j++) {
                motor.write(7,j,omni.getOutput(j));
            }
            pwm = 0.5;
        }
    }
}