use for experiment before the demonstration at open-campus
Dependencies: FEP ikarashiMDC omni PID R1370
Fork of omni_sample by
main.cpp
- Committer:
- tkNara
- Date:
- 2017-09-15
- Revision:
- 16:7eaf3343f45e
- Parent:
- 15:d4ff132a616d
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; } } }