main.cpp

00001 #include"mbed.h"
00002 #include"measuring_wheel.h"
00003 #include"ikarashiMDC.h"
00004 #include"position_controller.h"
00005 #include"omni_wheel.h"
00006 #include"pin_config.h"
00007 #include"SerialMultiByte.h"
00008 #include"PID.h"
00009 #include"IRsensor.h"
00010 
00011 #define YPOINT 4000
00012 #define GAP 0
00013 
00014 Serial serial(MDCTX,MDCRX,115200);
00015 ikarashiMDC motor[]={
00016     ikarashiMDC(1,0,SM,&serial),
00017     ikarashiMDC(1,1,SM,&serial),
00018     ikarashiMDC(1,2,SM,&serial),
00019     ikarashiMDC(1,3,SM,&serial)
00020 };
00021 PositionController position_control_1(500,500,0.1,0.01,0.8);
00022 
00023 OmniWheel omni(4);
00024 SerialMultiByte s(SERIALTX,SERIALRX);
00025 MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
00026 PID pid_spin(0,0,0,0.001);
00027 PID pid_x(0,0,0,0.001);
00028 PID pid_y(0,0,0,0.001);
00029 Serial pc(USBTX,USBRX,115200);
00030 
00031 DigitalIn debug_button(USER_BUTTON);
00032 DigitalOut debug_led_0(LED_0);
00033 DigitalOut debug_led_1(LED_2);
00034 DigitalOut debug_led_2(LED_1);
00035 
00036 IRsensor IR0(IR_0);
00037 
00038 int mode;
00039 int instruction_mode;
00040 double omni_power[4];
00041 double X_power,Y_power;
00042 double spin_power;
00043 float y_point = YPOINT;
00044 int X_,Y_;
00045 double dai_x,dai_low_y;
00046 int gap = GAP;
00047 
00048 void set_up()
00049 {
00050   float theta = PIII / 4;
00051   omni.wheel[0].setRadian(PIII - theta);
00052   omni.wheel[1].setRadian(theta);
00053   omni.wheel[2].setRadian(-theta);
00054   omni.wheel[3].setRadian(PIII + theta);
00055 
00056   s.setHeaders('A','Z');
00057   s.startReceive(5);
00058 
00059   IR0.startAveraging(5);
00060 
00061 }
00062 
00063 void mode1()
00064 {
00065   pid_x.setProcessValue(m.getOutX());
00066   X_power += pid_x.compute();
00067 
00068   position_control_1.compute(0.0,m.getOutY());
00069   Y_power += position_control_1.getVelocityY();
00070 
00071   pid_y.setProcessValue(m.getOutY() - Y_);
00072   Y_power += pid_y.compute();
00073 
00074   pid_spin.setProcessValue(m.getjyroAngle());
00075   spin_power = pid_spin.compute();
00076 
00077 }
00078 
00079 void mode2()
00080 {
00081   X_power -= 0.3;
00082 
00083   pid_x.setProcessValue(m.getOutX() - X_);
00084   X_power += pid_x.compute();
00085 
00086   pid_y.setProcessValue(m.getOutY());
00087   Y_power += pid_y.compute();
00088 
00089   pid_spin.setProcessValue(m.getjyroAngle());
00090   spin_power = pid_spin.compute();
00091 
00092 }
00093 
00094 void mode3()
00095 {
00096   pid_x.setProcessValue(m.getOutX());
00097   X_power += pid_x.compute();
00098 
00099   Y_power -= 0.3;
00100 
00101   pid_spin.setProcessValue(m.getjyroAngle());
00102   spin_power = pid_spin.compute();
00103 
00104 }
00105 
00106 void mode4()
00107 {
00108   pid_x.setProcessValue(m.getOutX());
00109   X_power += pid_x.compute();
00110 
00111   pid_y.setProcessValue(m.getOutY());
00112   Y_power += pid_y.compute();
00113 
00114   pid_spin.setProcessValue(m.getjyroAngle());
00115   spin_power = pid_spin.compute();
00116 
00117 }
00118 
00119 void to_main()
00120 {
00121   unsigned char data[1];
00122   unsigned char getdata[5];
00123   data[0] = mode;
00124   s.sendData(data,1);
00125   s.getData(getdata);
00126   instruction_mode=getdata[0];
00127 
00128   if(getdata[2]>=(1<<7)){
00129     getdata[2]-=(1<<7);
00130     X_=-1*(getdata[1]+(getdata[2]<<8));
00131   }
00132   else X_= getdata[1]+(getdata[2]<<8);
00133 
00134   if(getdata[4]>=(1<<7)){
00135     getdata[4]-=(1<<7);
00136     Y_=-1*(getdata[3]+(getdata[4]<<8));
00137   }
00138   else Y_= getdata[3]+(getdata[4]<<8);
00139   if(instruction_mode!=0)debug_led_1 = !debug_led_1;
00140 }
00141 int main()
00142 {
00143   set_up();
00144   while(true){
00145     debug_led_0 = !debug_led_0;
00146     X_power = 0.0;
00147     Y_power = 0.0;
00148     spin_power = 0.0;
00149 
00150     to_main();
00151 
00152     if(instruction_mode == 1&&mode == 0){
00153 
00154       pid_x.reset();
00155       pid_x.setTunings(3.0,1.0,0.000001);
00156       pid_x.setInputLimits(-1000.0,1000.0);
00157       pid_x.setOutputLimits(-1.0,1.0);
00158       pid_x.setBias(0);
00159       pid_x.setMode(1);
00160       pid_x.setSetPoint(0.0);
00161 
00162       position_control_1.targetXY(1,int(y_point));
00163 
00164       pid_y.reset();
00165       pid_y.setTunings(1.0,1.0,0.000001);
00166       pid_y.setInputLimits(-1000.0,1000.0);
00167       pid_y.setOutputLimits(-1.0,1.0);
00168       pid_y.setBias(0);
00169       pid_y.setMode(1);
00170       pid_y.setSetPoint(0.0);
00171 
00172       pid_spin.reset();
00173       pid_spin.setTunings(5.0,1.0,0.000001);
00174       pid_spin.setInputLimits(-180.0,180.0);
00175       pid_spin.setOutputLimits(-0.5,0.5);
00176       pid_spin.setBias(0);
00177       pid_spin.setMode(1);
00178       pid_spin.setSetPoint(0.0);
00179 
00180       mode = 1;
00181     }
00182 
00183     if((m.getOutY() >= (y_point - 50)) && mode == 1){
00184 
00185       pid_x.reset();
00186       pid_x.setTunings(1.0,1.0,0.000001);
00187       pid_x.setInputLimits(-1000.0,1000.0);
00188       pid_x.setOutputLimits(-1.0,1.0);
00189       pid_x.setBias(0);
00190       pid_x.setMode(1);
00191       pid_x.setSetPoint(0.0);
00192 
00193       pid_y.reset();
00194       pid_y.setTunings(1.0,1.0,0.000001);
00195       pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
00196       pid_y.setOutputLimits(-1.0,1.0);
00197       pid_y.setBias(0);
00198       pid_y.setMode(1);
00199       pid_y.setSetPoint(y_point);
00200 
00201       pid_spin.reset();
00202       pid_spin.setTunings(5.0,1.0,0.000001);
00203       pid_spin.setInputLimits(-180.0,180.0);
00204       pid_spin.setOutputLimits(-0.5,0.5);
00205       pid_spin.setBias(0);
00206       pid_spin.setMode(1);
00207       pid_spin.setSetPoint(0.0);
00208 
00209       mode = 2;
00210     }
00211 
00212     if(IR0.get_Averagingdistance()<=20&&mode == 2){
00213       dai_x = m.getOutX();
00214       mode = 0xff;
00215     }
00216 
00217     if(instruction_mode == 3&&mode == 0xff){
00218       pid_x.reset();
00219       pid_x.setTunings(3.0,1.0,0.000001);
00220       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00221       pid_x.setOutputLimits(-1.0,1.0);
00222       pid_x.setBias(0);
00223       pid_x.setMode(1);
00224       pid_x.setSetPoint(dai_x);
00225 
00226       pid_spin.reset();
00227       pid_spin.setTunings(5.0,1.0,0.000001);
00228       pid_spin.setInputLimits(-180.0,180.0);
00229       pid_spin.setOutputLimits(-0.5,0.5);
00230       pid_spin.setBias(0);
00231       pid_spin.setMode(1);
00232       pid_spin.setSetPoint(0.0);
00233 
00234       mode = 3;
00235     }
00236 
00237     if(IR0.get_Averagingdistance()>=50&&mode == 3){
00238       dai_low_y = m.getOutY();
00239 
00240       pid_x.reset();
00241       pid_x.setTunings(3.0,1.0,0.000001);
00242       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00243       pid_x.setOutputLimits(-1.0,1.0);
00244       pid_x.setBias(0);
00245       pid_x.setMode(1);
00246       pid_x.setSetPoint(dai_x);
00247 
00248       pid_y.reset();
00249       pid_y.setTunings(1.0,1.0,0.000001);
00250       pid_y.setInputLimits(dai_low_y + gap - 1000.0 ,dai_low_y + gap + 1000.0);
00251       pid_y.setOutputLimits(-1.0,1.0);
00252       pid_y.setBias(0);
00253       pid_y.setMode(1);
00254       pid_y.setSetPoint(dai_low_y + gap);
00255 
00256       pid_spin.reset();
00257       pid_spin.setTunings(5.0,1.0,0.000001);
00258       pid_spin.setInputLimits(-180.0,180.0);
00259       pid_spin.setOutputLimits(-0.5,0.5);
00260       pid_spin.setBias(0);
00261       pid_spin.setMode(1);
00262       pid_spin.setSetPoint(0.0);
00263 
00264       mode = 4;
00265     }
00266 
00267     if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
00268       mode = 0xff;
00269     }
00270 
00271     if(mode == 1)mode1();
00272     if(mode == 2)mode2();
00273     if(mode == 3)mode3();
00274     if(mode == 4)mode4();
00275 
00276     omni.computeXY(Y_power,-X_power,-spin_power);
00277 
00278     for(int i = 0; i < 4; i++){
00279       omni_power[i] = 0.0;
00280       omni_power[i] = omni.wheel[i];
00281       motor[i].setSpeed(omni_power[i]);
00282     }
00283 
00284   }
00285 }