File content as of revision 6:259deb365510:

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

#define DEBUG 
MDC motor(i2c_sda,i2c_scl);
Omni omni(4, 135);
FEP fep(PC_10,PC_11,200);
DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
DigitalOut attack[2] = {PA_10,PC_4};
DigitalOut angle[2] = {PB_3,PB_5};
#ifdef DEBUG
    Serial pc(USBTX,USBRX,115200);
#endif 
void init()
{
    for(int i = 0;i<4;i++) {
        leds[i] = 0;
    }
}
void AllActuatorStop()
{
#ifdef DEBUG
    pc.printf("All actuators are stop\n");
#endif
    omni.stop();
    for(int j = 0; j < 4; j++) {
        motor.write(7,j,0);
        motor.write(6,j,0);
    }
    for(int i=0;i<1;i++)
    {
        attack[i]=0;
        angle[i]=0;
    }
}
int main()
{   
    bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
    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 deg = 360;
    double pwm = 0.0;
    for(int j = 0; j < 4; j++) {
        motor.write(7,j,0);
    }
    
    while(1) {
        fep_temp=fep.read(data,6);
        if(fep_temp==0) {

#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

            }
            pc.printf("\n");
            for(i = 0; i < 6; i++) {
                Button2[i] = tem[1] % 2;
                tem[1] /= 2;

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

#ifdef DEBUG
            for(i=0;i < 4; i++) {
                pc.printf("%d,",data[i]);
            }
            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) {
            polarVector[1] = 0.6;
            //移動方向
            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(Button1[0] == 1 && Button1[1] == 0) {
                pwm = 0.9;
            } else if(Button1[0] == 0 && Button1[1] == 1) {
                pwm = -0.9;
            } else {
                pwm = -0.0;
            }
            motor.write(6,0,pwm);
            //アーム攻撃(toggle)
            if((Button2[1]==0)&&(airFlag1 == 0))
            {
                if(airStatus1==1) {
                    attack[0]=0;
                    attack[1]=1;
                    airFlag1=1;
                    airStatus1=0;
                }else if(airStatus1==0) {
                    attack[0]=1;
                    attack[1]=0;
                    airFlag1=1;
                    airStatus1=1;
                }                   
            }else if(Button2[1]==1){
                airFlag1=0;
                attack[0]=0;
                attack[1]=0;    
            }
            //アーム角度(toggle)
            if((Button2[3]==0)&&(airFlag2 == 0))
            {
                if(airStatus2==1) {
                    angle[0]=0;
                    angle[1]=1;
                    airFlag2=1;
                    airStatus2=0;
                }else if(airStatus2==0) {
                    angle[0]=1;
                    angle[1]=0;
                    airFlag2=1;
                    airStatus2=1;
                }                   
            }else if(Button2[3]==1){
                airFlag2=0;
                angle[0]=0;
                angle[1]=0;    
            }
#ifdef DEBUG
            printf("Status of airFlags %d,%d\r\n",airStatus1,airStatus2);
#endif
        } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){
            AllActuatorStop();
        } else {
            AllActuatorStop();
        }
    }
}