File content as of revision 7:ef72ec7390c2:

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

#define DEBUG 
Omni omni(4);
Serial serial(PC_12,PD_2);
DigitalOut serialcontrol(D2);
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 
ikarashiMDC wheels[] {
    ikarashiMDC(&serialcontrol,0,0,SM,&serial),
    ikarashiMDC(&serialcontrol,1,0,SM,&serial),
    ikarashiMDC(&serialcontrol,1,4,SM,&serial),
    ikarashiMDC(&serialcontrol,0,4,SM,&serial)
};
ikarashiMDC lift[] {
    ikarashiMDC(&serialcontrol,1,2,SM,&serial)
};
void init()
{
    omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
    for(int i = 0;i<4;i++) {
        leds[i] = 0;
    }
}
void AllActuatorStop()
{
#ifdef DEBUG
    pc.printf("All actuators stop\n");
#endif
    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 stick[4] = { 0 };
    double pwm = 0.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 < 4; i++) stick[i] = -1*(data[i]-128)/128.0;
            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) {
            omni.computeXY(data[3],data[2],data[0]);           
            for(int i = 0; i < 4; i++) {
                wheels[i].setSpeed(omni.getOutput(i));
            }
            //昇降
            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;
            }
            lift[0].setSpeed(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();
        }
    }
}