NHK2019_Team_B_Automatic_machine_maegawa
Dependencies: SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen
main.cpp
- Committer:
- skouki
- Date:
- 2019-10-02
- Revision:
- 1:acb42f35c7c2
- Parent:
- 0:76663617eca3
File content as of revision 1:acb42f35c7c2:
#include"mbed.h" #include"measuring_wheel.h" #include"ikarashiMDC.h" #include"position_controller.h" #include"omni_wheel.h" #include"pin_config.h" #include"SerialMultiByte.h" #include"PID.h" #include"IRsensor.h" #define YPOINT 6400 #define GAP 20 Serial serial(MDCTX,MDCRX,115200); ikarashiMDC motor[]={ ikarashiMDC(1,0,SM,&serial), ikarashiMDC(1,1,SM,&serial), ikarashiMDC(1,2,SM,&serial), ikarashiMDC(1,3,SM,&serial) }; PositionController position_control_1(1000,1000,0.1,0.1,0.3); OmniWheel omni(4); SerialMultiByte s(SERIALTX,SERIALRX); MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX); PID pid_spin(0,0,0,0.001); PID pid_x(0,0,0,0.001); PID pid_y(0,0,0,0.001); Serial pc(USBTX,USBRX,115200); DigitalIn an(USER_BUTTON); DigitalOut debug_led_0(LED_0); DigitalOut debug_led_1(LED_2); DigitalOut debug_led_2(LED_1); DigitalOut emergency_stop(EMERGENCY_STOP); IRsensor IR0(IR_0); IRsensor IR1(IR_1); int mode; int instruction_mode; double omni_power[4]; double X_power,Y_power; double spin_power; float y_point = YPOINT; double dai_x,dai_high_y; int gap = GAP; double ir_distance; int data_a; void set_up() { float theta = PIII / 4; omni.wheel[0].setRadian(PIII - theta); omni.wheel[1].setRadian(theta); omni.wheel[2].setRadian(-theta); omni.wheel[3].setRadian(PIII + theta); s.setHeaders('A','Z'); s.startReceive(2); IR0.startAveraging(5); IR1.startAveraging(5); } void mode1() { pid_x.setProcessValue(m.getOutX()); X_power += pid_x.compute(); position_control_1.compute(1,m.getOutY()); Y_power += position_control_1.getVelocityY(); pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); } void mode2() { if(data_a)X_power -= 0.3; else X_power += 0.3; pid_y.setProcessValue(m.getOutY()); Y_power += pid_y.compute(); pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); } void mode3() { pid_x.setProcessValue(m.getOutX()); X_power += pid_x.compute(); Y_power += 0.15; pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); } void mode4() { pid_x.setProcessValue(m.getOutX()); X_power += pid_x.compute(); pid_y.setProcessValue(m.getOutY()); Y_power += pid_y.compute(); pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); } void mode5() { if(data_a)X_power -= 0.3; else X_power += 0.3; pid_y.setProcessValue(m.getOutY()); Y_power += pid_y.compute(); pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); } void mode6() { if(data_a)X_power -= 0.3; else X_power += 0.3; pid_y.setProcessValue(m.getOutY()); Y_power += pid_y.compute(); pid_spin.setProcessValue(m.getjyroAngle()); spin_power = pid_spin.compute(); if(m.getjyroAngle() >=10.0)Y_power -= 0.5; } void to_main() { unsigned char data[5]; unsigned char getdata[2]; int X_ = m.getOutX(); int Y_ = m.getOutY(); data[0] = mode; if(X_>0){ data[1] = X_&0x00ff; data[2] = (X_>>8)&0x00ff; } else{ X_*=-1; data[1] = X_&0x00ff; data[2] = ((X_>>8)&0x00ff )+ (1<<7); } if(Y_>0){ data[3] = Y_&0x00ff; data[4] = (Y_>>8)&0x00ff; } else{ Y_*=-1; data[3] = Y_&0x00ff; data[4] = ((Y_>>8)&0x00ff )+ (1<<7); } s.sendData(data,5); s.getData(getdata); instruction_mode = getdata[0]; data_a = getdata[1]; if(instruction_mode)debug_led_1 = !debug_led_1; } int main() { set_up(); emergency_stop = 1; an.mode(PullUp); while(true){ debug_led_0 = !debug_led_0; if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){ debug_led_2 = 1; } else debug_led_2 = 0; X_power = 0.0; Y_power = 0.0; spin_power = 0.0; if(data_a)ir_distance = IR0.get_Averagingdistance(); else ir_distance = IR1.get_Averagingdistance(); to_main(); if(instruction_mode == 1&&mode == 0){ pid_x.reset(); pid_x.setTunings(10.0,1.0,0.000001); pid_x.setInputLimits(-1000.0,1000.0); pid_x.setOutputLimits(-1.0,1.0); pid_x.setBias(0); pid_x.setMode(1); pid_x.setSetPoint(0.0); position_control_1.targetXY(1,int(y_point)); pid_spin.reset(); pid_spin.setTunings(10.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 1; } if(instruction_mode == 2&&mode == 1){ pid_y.reset(); pid_y.setTunings(3.0,1.0,0.000001); pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0); pid_y.setOutputLimits(-1.0,1.0); pid_y.setBias(0); pid_y.setMode(1); pid_y.setSetPoint(y_point); pid_spin.reset(); pid_spin.setTunings(5.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 2; } if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){ dai_x = m.getOutX(); mode = 0xff; } if(instruction_mode == 3&&mode == 0xff){ pid_x.reset(); pid_x.setTunings(3.0,1.0,0.000001); pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0); pid_x.setOutputLimits(-1.0,1.0); pid_x.setBias(0); pid_x.setMode(1); pid_x.setSetPoint(dai_x); pid_spin.reset(); pid_spin.setTunings(5.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 3; } if(ir_distance>=20&&mode == 3){ dai_high_y = m.getOutY(); pid_x.reset(); pid_x.setTunings(3.0,1.0,0.000001); pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0); pid_x.setOutputLimits(-1.0,1.0); pid_x.setBias(0); pid_x.setMode(1); pid_x.setSetPoint(dai_x); pid_y.reset(); pid_y.setTunings(10.0,1.0,0.000001); pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0); pid_y.setOutputLimits(-1.0,1.0); pid_y.setBias(0); pid_y.setMode(1); pid_y.setSetPoint(dai_high_y + gap); pid_spin.reset(); pid_spin.setTunings(5.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 4; } if(instruction_mode == 5&&mode == 4){ //gap = GAP - 30; pid_y.reset(); pid_y.setTunings(5.0,1.0,0.000001); pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0); pid_y.setOutputLimits(-1.0,1.0); pid_y.setBias(0); pid_y.setMode(1); pid_y.setSetPoint(dai_high_y + gap); pid_spin.reset(); pid_spin.setTunings(5.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 5; } /* if(m.getOutX() <= -1600 && mode==5){ //gap = GAP - 50; pid_y.reset(); pid_y.setTunings(5.0,1.0,0.000001); pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0); pid_y.setOutputLimits(-1.0,1.0); pid_y.setBias(0); pid_y.setMode(1); pid_y.setSetPoint(dai_high_y + gap); pid_spin.reset(); pid_spin.setTunings(5.0,1.0,0.000001); pid_spin.setInputLimits(-180.0,180.0); pid_spin.setOutputLimits(-0.5,0.5); pid_spin.setBias(0); pid_spin.setMode(1); pid_spin.setSetPoint(0.0); mode = 6; } */ if(an.read()==0)y_point = 0; if(mode == 1)mode1(); if(mode == 2)mode2(); if(mode == 3)mode3(); if(mode == 4)mode4(); if(mode == 5)mode5(); if(mode == 6)mode6(); if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;} omni.computeXY(Y_power,-X_power,-spin_power); for(int i = 0; i < 4; i++){ omni_power[i] = 0.0; omni_power[i] = omni.wheel[i]; motor[i].setSpeed(omni_power[i]); } //pc.printf("%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n\r",m.getOutX(),m.getOutY(),m.getjyroAngle(),X_power,Y_power,spin_power,ir_distance); } }