File content as of revision 16:7eaf3343f45e:

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

#define DEBUG

#define KP 0.0//1.5 //2.5
#define KI 0.0 //0s
#define KD 0.0 //0.05
#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 attack[2] = {air1_0,air1_1};
DigitalOut angle[2] = {air2_0,air2_1};
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;
    }
    omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
    lift[0].braking = true;
}
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 gyroInit()
{
    R1370.update();
    R1370.setZeroPoint(R1370.getAngle());
}
int main()
{
    bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
    int error_val = 0,i = 0;
    float nowAngle = 0,rote = 0.0;//,oldAngle = 0;
    init();
    pidInit();
    gyroInit();
    while(1)
    {
        if(con.receiveState()==0)
        {
            if(R1370.update()==0)
            {
                if(con.getButton1(6)==0) {
                    powerSw = 0;
                } else {
                    powerSw = 1;
                }
                for(i=0;i<6;i++){
                    pc.printf("%d,%d\n",i,con.getButton2(i));
                }
                error_val = 0;
                nowAngle = R1370.getAngle();
                /*if((con.getStick(0)==0)&&(pidflag==1)) {
                    pc.printf("zeroPointSet\n");
                    pidflag = 0;
                    R1370.setZeroPoint(nowAngle);
                }else if(con.getStick(0)!=0) {
                    pidflag = 1;
                    if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
                        rote = 0.15;
                    }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
                        rote = -0.15;
                    }else {
                        rote = 0.0;
                    }
                    omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);//(-1*con.getStick(0))/3.0);
                }else if((con.getStick(0)==0)&&(pidflag==0)) {
                    pid.setProcessValue(R1370.getDeviation(nowAngle));
                    omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
                }*/
                if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
                    rote = 0.15;
                }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
                    rote = -0.15;
                }else {
                    rote = 0.0;
                }
                omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);
                pc.printf("Now%lf:D%lf:set%lf:PID%lf   ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
                for(i = 0; i < 4; i++) {
                    pc.printf("%lf,",omni.getOutput(i));
                    wheels[i].setSpeed(omni.getOutput(i));
                }
                pc.printf("\n");
                //昇降
                lift[0].setSpeed(con.getStick(1));
                //アーム攻撃(toggle)
                pc.printf("%d\n",con.getButton2(1));
                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 {
                //pc.printf("gyro_error\n");
            }
        }else {
            //pc.printf("FEP_error\n");
            error_val++;
            if(error_val > 5) powerSw = 0;
            else powerSw = 1;
        }
    }
}