File content as of revision 2:b9297c2b2632:

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

MDC motor[4];
Omni omni(4, 45);
FEP fep(PA_0,PA_1,115200);
DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
PwmOut pwm(PA_6);

int main()
{
    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(i = 0;i<4;i++) {
        leds[i] = 0;
    }
    for(int j = 0; j < 4; j++) {
        motor[j].setAddr(7, j);
    }
    while(1) {
        fep_temp=fep.read_txt(data,6);
        if(fep_temp==FEP_RX_SUCCESS) {
//            pc.printf("Recieve succeeded,%s\r\n",data);
            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;
//                pc.printf("%d,",Button1[i]);
            }
            for(i = 0; i < 6; i++) {
                Button2[i] = tem[1] % 2;
                tem[1] /= 2;
//                pc.printf("%d,",Button2[i]);
            }
//            pc.printf("\r\n");
        } else if(fep_temp==FEP_NO_RESPONSE) {
//            pc.printf("No response\r\n");
            leds[0] = 1;
            continue;
        } else {
//            pc.printf("Recieve failed\r\n");
            leds[0] = 1;
            error_val++;
        }
        if(error_val < 4) {
            if (data[0]>100 && data[0]<150 && data[1]>100 && data[1]<150) {
                omni.stop();
                leds[2] = 1;
            } else {
                leds[2] = 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[0] = deg;
                polarVector[1] = 0.5;
                
                omni.computePolar(polarVector, 0);
            }
            
            for(int j = 0; j < 4; j++) {
                motor[j].write(omni.getOutput(j));
            }
            if(Button2[0] == 1 && Button2[1] == 0) {
                pwm = 0.2;
            } else if(Button2[0] == 0 && Button2[1] == 1) {
                pwm = 0.8;
            } else {
                pwm = 0.5;
            }
        } else {
            omni.stop();
            for(int j = 0; j < 4; j++) {
                motor[j].write(omni.getOutput(j));
            }
            pwm = 0.5;
        }
    }
}