File content as of revision 12:c7e91c4c2ffa:

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

#define DEBUG 

#define KP 5
#define KI 0 
#define KD 0
#define RATE 0.01
Omni omni(4);
Serial RS485(RS485_TX,RS485_RX,38400);
Serial pc(USBTX,USBRX,115200);
Controller con(FEP_TX,FEP_RX,200);
DigitalOut RS485control(PA_4);
//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};
DigitalOut powerSw(powerSW);
R1370 R1370(R1370_TX,R1370_RX);
PID pid(KP,KI,KD,RATE); 
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()
{
    pc.printf("Hello\n");
    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 pidInit()
{
    pid.reset();
    pid.setInputLimits(-180.0,180.0);
    pid.setOutputLimits(-1.0,1.0);
    pid.setMode(1);
    pid.setSetPoint(0.0);
    pid.setBias(0.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,i = 0;
    double pwm = 0.0;
    init();
    pidInit();
    while(1) 
    {
        if((con.receiveState()==0)&&(R1370.update()==0)) 
        {
            if(con.getButton1(6)==0) {
                powerSw = 0;
            } else {
                powerSw = 1;
            }
            error_val = 0;
            pid.setProcessValue(R1370.getAngle());
            pc.printf("%lf    %lf     ",R1370.getAngle(),-1*pid.compute());
            omni.computeXY(0,0,-1*pid.compute());
            omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());           
            for(int i = 0; i < 4; i++) {
                pc.printf("%lf,",omni.getOutput(i));
                wheels[i].setSpeed(omni.getOutput(i));
            }
            pc.printf("\n");
            //昇降
            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;    
            }
        }else {
            error_val++;
            if(error_val > 10) powerSw = 0;
        }
    }
}