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 6400
00012 #define GAP 15
00013 #define MTOU 300
00014 
00015 Serial serial(MDCTX,MDCRX,115200);
00016 ikarashiMDC motor[]={
00017     ikarashiMDC(1,0,SM,&serial),
00018     ikarashiMDC(1,1,SM,&serial),
00019     ikarashiMDC(1,2,SM,&serial),
00020     ikarashiMDC(1,3,SM,&serial)
00021 };
00022 PositionController position_control_1(1000,1000,0.3,0.01,0.3);
00023 
00024 OmniWheel omni(4);
00025 SerialMultiByte s(SERIALTX,SERIALRX);
00026 MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
00027 PID pid_spin(0,0,0,0.001);
00028 PID pid_x(0,0,0,0.001);
00029 PID pid_y(0,0,0,0.001);
00030 Serial pc(USBTX,USBRX,115200);
00031 
00032 DigitalIn an(USER_BUTTON);
00033 DigitalOut debug_led_0(LED_0);
00034 DigitalOut debug_led_1(LED_2);
00035 DigitalOut debug_led_2(LED_1);
00036 
00037 IRsensor IR0(IR_0);
00038 IRsensor IR1(IR_1);
00039 
00040 int mode;
00041 int instruction_mode;
00042 double omni_power[4];
00043 double X_power,Y_power;
00044 double spin_power;
00045 float y_point = YPOINT;
00046 int X_,Y_;
00047 double dai_x,dai_low_y;
00048 int gap = GAP;
00049 double ir_distance;
00050 int data_a;
00051 int m_to_u = MTOU;
00052 
00053 void set_up()
00054 {
00055   float theta = PIII / 4;
00056   omni.wheel[0].setRadian(PIII - theta);
00057   omni.wheel[1].setRadian(theta);
00058   omni.wheel[2].setRadian(-theta);
00059   omni.wheel[3].setRadian(PIII + theta);
00060 
00061   s.setHeaders('A','Z');
00062   s.startReceive(6);
00063 
00064   IR0.startAveraging(5);
00065   IR1.startAveraging(5);
00066 
00067 }
00068 
00069 void mode1()
00070 {
00071   pid_x.setProcessValue(m.getOutX());
00072   X_power += pid_x.compute();
00073 
00074   position_control_1.compute(1,m.getOutY());
00075   Y_power += position_control_1.getVelocityY();
00076 
00077   pid_y.setProcessValue(m.getOutY() - Y_ + 200);
00078   Y_power += pid_y.compute();
00079 
00080   if(Y_power <= 0.0)Y_power = 0.0;
00081 
00082   pid_spin.setProcessValue(m.getjyroAngle());
00083   spin_power = pid_spin.compute();
00084 
00085 }
00086 
00087 void mode2()
00088 {
00089   if(data_a)X_power -= 0.3;
00090   else X_power += 0.3;
00091 
00092   pid_x.setProcessValue(m.getOutX() - X_);
00093   X_power += pid_x.compute();
00094 
00095   pid_y.setProcessValue(m.getOutY());
00096   Y_power += pid_y.compute();
00097   
00098   if(y_point == 0){
00099       if(Y_power <= 0.0)Y_power = 0.0;
00100   }
00101 
00102   pid_spin.setProcessValue(m.getjyroAngle());
00103   spin_power = pid_spin.compute();
00104 
00105 }
00106 
00107 void mode3()
00108 {
00109   pid_x.setProcessValue(m.getOutX());
00110   X_power += pid_x.compute();
00111 
00112   pid_y.setProcessValue(Y_ - m.getOutY());
00113   Y_power -= pid_y.compute();
00114   
00115   
00116   if(Y_power <= 0.0)Y_power = 0.0;
00117 
00118   pid_spin.setProcessValue(m.getjyroAngle());
00119   spin_power = pid_spin.compute();
00120 
00121 }
00122 
00123 void mode4()
00124 {
00125   pid_x.setProcessValue(m.getOutX());
00126   X_power += pid_x.compute();
00127 
00128   pid_y.setProcessValue(Y_ - m.getOutY());
00129   Y_power -= pid_y.compute();
00130   
00131   if(Y_power <= 0.0)Y_power = 0.0;
00132 
00133 
00134   pid_spin.setProcessValue(m.getjyroAngle());
00135   spin_power = pid_spin.compute();
00136 
00137 }
00138 
00139 void mode5()
00140 {
00141   if(data_a)X_power -= 0.3;
00142   else X_power += 0.3;
00143 
00144 
00145   pid_x.setProcessValue(m.getOutX() - X_);
00146   X_power += pid_x.compute();
00147 
00148 
00149   pid_y.setProcessValue(Y_ - m.getOutY());
00150   Y_power -= pid_y.compute();
00151 
00152   pid_spin.setProcessValue(m.getjyroAngle());
00153   spin_power = pid_spin.compute();
00154 
00155 }
00156 
00157 void mode6()
00158 {
00159   if(data_a)X_power -= 0.3;
00160   else X_power += 0.3;
00161 
00162 
00163   pid_x.setProcessValue(m.getOutX() - X_);
00164   X_power += pid_x.compute();
00165 
00166 
00167   pid_y.setProcessValue(Y_ - m.getOutY());
00168   Y_power -= pid_y.compute();
00169 
00170   pid_spin.setProcessValue(m.getjyroAngle());
00171   spin_power = pid_spin.compute();
00172 
00173   if(m.getjyroAngle() <=-10.0)Y_power += 0.5;
00174 
00175 
00176 }
00177 
00178 void to_main()
00179 {
00180   unsigned char data[1];
00181   unsigned char getdata[6];
00182   data[0] = mode;
00183   s.sendData(data,1);
00184   s.getData(getdata);
00185   instruction_mode=getdata[0];
00186 
00187   if(getdata[2]>=(1<<7)){
00188     getdata[2]-=(1<<7);
00189     X_=-1*(getdata[1]+(getdata[2]<<8));
00190   }
00191   else X_= getdata[1]+(getdata[2]<<8);
00192 
00193   if(getdata[4]>=(1<<7)){
00194     getdata[4]-=(1<<7);
00195     Y_=-1*(getdata[3]+(getdata[4]<<8));
00196   }
00197   else Y_= getdata[3]+(getdata[4]<<8);
00198   data_a = getdata[5];
00199   if(instruction_mode!=0)debug_led_1 = !debug_led_1;
00200 }
00201 int main()
00202 {
00203   set_up();
00204     an.mode(PullUp);
00205   while(true){
00206     debug_led_0 = !debug_led_0;
00207     if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){
00208       debug_led_2 = 1;
00209     }
00210     else debug_led_2 = 0;
00211     X_power = 0.0;
00212     Y_power = 0.0;
00213     spin_power = 0.0;
00214     if(data_a)ir_distance = IR0.get_Averagingdistance();
00215     else ir_distance = IR1.get_Averagingdistance();
00216 
00217     to_main();
00218 
00219     if(instruction_mode == 1&&mode == 0){
00220 
00221       pid_x.reset();
00222       pid_x.setTunings(3.0,1.0,0.000001);
00223       pid_x.setInputLimits(-1000.0,1000.0);
00224       pid_x.setOutputLimits(-1.0,1.0);
00225       pid_x.setBias(0);
00226       pid_x.setMode(1);
00227       pid_x.setSetPoint(0.0);
00228 
00229       position_control_1.targetXY(1,int(y_point));
00230 
00231       pid_y.reset();
00232       pid_y.setTunings(7.0,1.0,0.000001);
00233       pid_y.setInputLimits(-1000.0,1000.0);
00234       pid_y.setOutputLimits(-1.0,1.0);
00235       pid_y.setBias(0);
00236       pid_y.setMode(1);
00237       pid_y.setSetPoint(0.0);
00238 
00239       pid_spin.reset();
00240       pid_spin.setTunings(5.0,1.0,0.000001);
00241       pid_spin.setInputLimits(-180.0,180.0);
00242       pid_spin.setOutputLimits(-0.5,0.5);
00243       pid_spin.setBias(0);
00244       pid_spin.setMode(1);
00245       pid_spin.setSetPoint(0.0);
00246 
00247       mode = 1;
00248     }
00249 
00250     if((m.getOutY() >= (y_point - 260)) && mode == 1){
00251 
00252       pid_x.reset();
00253       pid_x.setTunings(3.0,1.0,0.000001);
00254       pid_x.setInputLimits(-1000.0,1000.0);
00255       pid_x.setOutputLimits(-1.0,1.0);
00256       pid_x.setBias(0);
00257       pid_x.setMode(1);
00258       pid_x.setSetPoint(0.0);
00259 
00260       pid_y.reset();
00261       pid_y.setTunings(5.0,1.0,0.000001);
00262       pid_y.setInputLimits(y_point - 150 - 1000.0,y_point - 150 + 1000.0);
00263       pid_y.setOutputLimits(-1.0,1.0);
00264       pid_y.setBias(0);
00265       pid_y.setMode(1);
00266       pid_y.setSetPoint(y_point - 200);
00267 
00268       pid_spin.reset();
00269       pid_spin.setTunings(5.0,1.0,0.000001);
00270       pid_spin.setInputLimits(-180.0,180.0);
00271       pid_spin.setOutputLimits(-0.5,0.5);
00272       pid_spin.setBias(0);
00273       pid_spin.setMode(1);
00274       pid_spin.setSetPoint(0.0);
00275 
00276       mode = 2;
00277     }
00278 
00279     if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){
00280       dai_x = m.getOutX();
00281       mode = 0xff;
00282     }
00283 
00284     if(instruction_mode == 3&&mode == 0xff){
00285       pid_x.reset();
00286       pid_x.setTunings(3.0,1.0,0.000001);
00287       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00288       pid_x.setOutputLimits(-1.0,1.0);
00289       pid_x.setBias(0);
00290       pid_x.setMode(1);
00291       pid_x.setSetPoint(dai_x);
00292 
00293       pid_y.reset();
00294       pid_y.setTunings(10.0,1.0,0.000001);
00295       pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
00296       pid_y.setOutputLimits(-0.3,0.3);
00297       pid_y.setBias(0);
00298       pid_y.setMode(1);
00299       pid_y.setSetPoint(m_to_u);
00300 
00301       pid_spin.reset();
00302       pid_spin.setTunings(5.0,1.0,0.000001);
00303       pid_spin.setInputLimits(-180.0,180.0);
00304       pid_spin.setOutputLimits(-0.5,0.5);
00305       pid_spin.setBias(0);
00306       pid_spin.setMode(1);
00307       pid_spin.setSetPoint(0.0);
00308 
00309       mode = 3;
00310     }
00311 
00312     if(ir_distance>=20&&mode == 3){
00313       dai_low_y = m.getOutY();
00314 
00315       pid_x.reset();
00316       pid_x.setTunings(3.0,1.0,0.000001);
00317       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00318       pid_x.setOutputLimits(-1.0,1.0);
00319       pid_x.setBias(0);
00320       pid_x.setMode(1);
00321       pid_x.setSetPoint(dai_x);
00322 
00323  //     pid_y.reset();
00324 //      pid_y.setTunings(10.0,1.0,0.000001);
00325 //      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
00326 //      pid_y.setOutputLimits(-1.0,1.0);
00327 //      pid_y.setBias(0);
00328 //      pid_y.setMode(1);
00329 //      pid_y.setSetPoint(dai_low_y - gap);
00330 
00331       m_to_u = MTOU + 40 ;
00332       pid_y.reset();
00333       pid_y.setTunings(10.0,1.0,0.000001);
00334       pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
00335       pid_y.setOutputLimits(-0.5,0.5);
00336       pid_y.setBias(0);
00337       pid_y.setMode(1);
00338       pid_y.setSetPoint(m_to_u);
00339       
00340       pid_spin.reset();
00341       pid_spin.setTunings(5.0,1.0,0.000001);
00342       pid_spin.setInputLimits(-180.0,180.0);
00343       pid_spin.setOutputLimits(-0.5,0.5);
00344       pid_spin.setBias(0);
00345       pid_spin.setMode(1);
00346       pid_spin.setSetPoint(0.0);
00347 
00348       mode = 4;
00349     }
00350 
00351     if(instruction_mode == 5&&mode == 4){
00352       //gap = GAP - 30;
00353 
00354       pid_x.reset();
00355       pid_x.setTunings(5.0,1.0,0.000001);
00356       pid_x.setInputLimits(-1000.0,1000.0);
00357       pid_x.setOutputLimits(-1.0,1.0);
00358       pid_x.setBias(0);
00359       pid_x.setMode(1);
00360       pid_x.setSetPoint(0.0);
00361 
00362 //      pid_y.reset();
00363 //      pid_y.setTunings(5.0,1.0,0.000001);
00364 //      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
00365 //      pid_y.setOutputLimits(-1.0,1.0);
00366 //      pid_y.setBias(0);
00367 //      pid_y.setMode(1);
00368 //      pid_y.setSetPoint(dai_low_y - gap);
00369 
00370       pid_y.reset();
00371       pid_y.setTunings(5.0,1.0,0.000001);
00372       pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
00373       pid_y.setOutputLimits(-1.0,1.0);
00374       pid_y.setBias(0);
00375       pid_y.setMode(1);
00376       pid_y.setSetPoint(m_to_u);
00377       
00378       pid_spin.reset();
00379       pid_spin.setTunings(5.0,1.0,0.000001);
00380       pid_spin.setInputLimits(-180.0,180.0);
00381       pid_spin.setOutputLimits(-0.5,0.5);
00382       pid_spin.setBias(0);
00383       pid_spin.setMode(1);
00384       pid_spin.setSetPoint(0.0);
00385 
00386       mode = 5;
00387     }
00388 /*
00389     if(m.getOutX() <= -1600&&mode == 5){
00390 
00391       //gap = GAP - 50;
00392 
00393       pid_x.reset();
00394       pid_x.setTunings(1.0,1.0,0.000001);
00395       pid_x.setInputLimits(-1000.0,1000.0);
00396       pid_x.setOutputLimits(-1.0,1.0);
00397       pid_x.setBias(0);
00398       pid_x.setMode(1);
00399       pid_x.setSetPoint(0.0);
00400 
00401       pid_y.reset();
00402       pid_y.setTunings(8.0,1.0,0.000001);
00403       pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
00404       pid_y.setOutputLimits(-1.0,1.0);
00405       pid_y.setBias(0);
00406       pid_y.setMode(1);
00407       pid_y.setSetPoint(dai_low_y - gap);
00408 
00409       pid_spin.reset();
00410       pid_spin.setTunings(5.0,1.0,0.000001);
00411       pid_spin.setInputLimits(-180.0,180.0);
00412       pid_spin.setOutputLimits(-0.5,0.5);
00413       pid_spin.setBias(0);
00414       pid_spin.setMode(1);
00415       pid_spin.setSetPoint(0.0);
00416 
00417       mode = 6;
00418     }
00419     */
00420 
00421     if(an.read()==0)y_point = 0;
00422 
00423     if(mode == 1)mode1();
00424     if(mode == 2)mode2();
00425     if(mode == 3)mode3();
00426     if(mode == 4)mode4();
00427     if(mode == 5)mode5();
00428     if(mode == 6)mode6();
00429     if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;}
00430 
00431     omni.computeXY(Y_power,-X_power,-spin_power);
00432 
00433     for(int i = 0; i < 4; i++){
00434       omni_power[i] = 0.0;
00435       omni_power[i] = omni.wheel[i];
00436       motor[i].setSpeed(omni_power[i]);
00437     }
00438     //pc.printf("%.2f,%.2f,%d,%d,%.2f,%.2f\n\r",m.getOutX(),m.getOutY(),X_,Y_,m.getjyroAngle(),ir_distance);
00439 
00440 
00441   }
00442 }