File content as of revision 9:8f9607783d2d:

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

#define stickrange 0.25
#define DEBUG 
Omni omni(4);
Serial RS485(RS485_TX,RS485_RX,38400);
Serial pc(USBTX,USBRX,115200);
Controller con(FEP_TX,FEP_RX,200);
DigitalOut RS485control(D2);
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};

 
ikarashiMDC wheels[] {
    ikarashiMDC(&RS485control,0,0,SM,&RS485),
    ikarashiMDC(&RS485control,1,3,SM,&RS485),
    ikarashiMDC(&RS485control,1,0,SM,&RS485),
    ikarashiMDC(&RS485control,0,3,SM,&RS485)
};
ikarashiMDC lift[] {
    ikarashiMDC(&RS485control,1,1,SM,&RS485)
};
void init()
{
    int i;
    for(i = 0;i < 4;i++) {
        wheels[i].braking = true;
    }
    lift[0].braking = true;
    omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
    for(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;
    int error_val = 0;
    uint8_t fep_temp;
    double pwm = 0.0;
    init();
    while(1) {
        if(con.receiveState()==0) {
            error_val = 0;
            leds[0] = 0;
        } else if(fep_temp==FEP_NO_RESPONSE) {
            leds[0] = 1;
            continue;
        } else {
            leds[0] = 1;
            error_val++;
        }
        if(error_val < 4) {
            omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);           
            for(int i = 0; i < 4; i++) {
                pc.printf("%lf,",omni.getOutput(i));
                wheels[i].setSpeed(omni.getOutput(i));
            }
            //昇降
            if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
                pwm = -0.9;
            } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
                pwm = 0.9;
            } else {
                pwm = 0.0;
            }
            lift[0].setSpeed(pwm);
            //アーム攻撃(toggle)
            if((con.getButton2(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(con.getButton2(1)==1){
                airFlag1=0;
                attack[0]=0;
                attack[1]=0;    
            }
            //アーム角度(toggle)
            if((con.getButton2(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(con.getButton2(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();
        }
    }
}