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-04
- Revision:
- 11:a2e3d11f5750
- Parent:
- 10:73148221684e
- Child:
- 12:c7e91c4c2ffa
File content as of revision 11:a2e3d11f5750:
#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}; 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; uint8_t fep_temp; double pwm = 0.0; init(); pidInit(); while(1) { if(1)//con.receiveState()==0) { error_val = 0; //leds[0] = 0; if(R1370.reciveState() == 0) { //omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.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; } } } }