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 20
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(1000,1000,0.1,0.1,0.3);
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 an(USER_BUTTON);
00032 DigitalOut debug_led_0(LED_0);
00033 DigitalOut debug_led_1(LED_2);
00034 DigitalOut debug_led_2(LED_1);
00035 DigitalOut emergency_stop(EMERGENCY_STOP);
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 
00047 double dai_x,dai_high_y;
00048 int gap = GAP;
00049 double ir_distance;
00050 int data_a;
00051 
00052 void set_up()
00053 {
00054   float theta = PIII / 4;
00055   omni.wheel[0].setRadian(PIII - theta);
00056   omni.wheel[1].setRadian(theta);
00057   omni.wheel[2].setRadian(-theta);
00058   omni.wheel[3].setRadian(PIII + theta);
00059 
00060   s.setHeaders('A','Z');
00061   s.startReceive(2);
00062 
00063   IR0.startAveraging(5);
00064   IR1.startAveraging(5);
00065 
00066 }
00067 
00068 void mode1()
00069 {
00070   pid_x.setProcessValue(m.getOutX());
00071   X_power += pid_x.compute();
00072 
00073   position_control_1.compute(1,m.getOutY());
00074   Y_power += position_control_1.getVelocityY();
00075 
00076   pid_spin.setProcessValue(m.getjyroAngle());
00077   spin_power = pid_spin.compute();
00078 
00079 }
00080 
00081 void mode2()
00082 {
00083   if(data_a)X_power -= 0.3;
00084   else X_power += 0.3;
00085 
00086 
00087   pid_y.setProcessValue(m.getOutY());
00088   Y_power += pid_y.compute();
00089 
00090   pid_spin.setProcessValue(m.getjyroAngle());
00091   spin_power = pid_spin.compute();
00092 
00093 }
00094 
00095 void mode3()
00096 {
00097   pid_x.setProcessValue(m.getOutX());
00098   X_power += pid_x.compute();
00099 
00100   Y_power += 0.15;
00101 
00102   pid_spin.setProcessValue(m.getjyroAngle());
00103   spin_power = pid_spin.compute();
00104 
00105 }
00106 
00107 void mode4()
00108 {
00109   pid_x.setProcessValue(m.getOutX());
00110   X_power += pid_x.compute();
00111 
00112   pid_y.setProcessValue(m.getOutY());
00113   Y_power += pid_y.compute();
00114 
00115   pid_spin.setProcessValue(m.getjyroAngle());
00116   spin_power = pid_spin.compute();
00117 
00118 }
00119 
00120 void mode5()
00121 {
00122     if(data_a)X_power -= 0.3;
00123     else X_power += 0.3;
00124 
00125     pid_y.setProcessValue(m.getOutY());
00126     Y_power += pid_y.compute();
00127 
00128     pid_spin.setProcessValue(m.getjyroAngle());
00129     spin_power = pid_spin.compute();
00130 
00131 }
00132 
00133 void mode6()
00134 {
00135     if(data_a)X_power -= 0.3;
00136     else X_power += 0.3;
00137 
00138     pid_y.setProcessValue(m.getOutY());
00139     Y_power += pid_y.compute();
00140 
00141     pid_spin.setProcessValue(m.getjyroAngle());
00142     spin_power = pid_spin.compute();
00143 
00144     if(m.getjyroAngle() >=10.0)Y_power -= 0.5;
00145 }
00146 void to_main()
00147 {
00148   unsigned char data[5];
00149   unsigned char getdata[2];
00150   int X_ = m.getOutX();
00151   int Y_ = m.getOutY();
00152   data[0] = mode;
00153   if(X_>0){
00154       data[1] = X_&0x00ff;
00155       data[2] = (X_>>8)&0x00ff;
00156   }
00157   else{
00158       X_*=-1;
00159       data[1] = X_&0x00ff;
00160       data[2] = ((X_>>8)&0x00ff )+ (1<<7);
00161   }
00162   if(Y_>0){
00163       data[3] = Y_&0x00ff;
00164       data[4] = (Y_>>8)&0x00ff;
00165   }
00166   else{
00167       Y_*=-1;
00168       data[3] = Y_&0x00ff;
00169       data[4] = ((Y_>>8)&0x00ff )+ (1<<7);
00170   }
00171   s.sendData(data,5);
00172   s.getData(getdata);
00173   instruction_mode = getdata[0];
00174   data_a = getdata[1];
00175   if(instruction_mode)debug_led_1 = !debug_led_1;
00176 }
00177 
00178 int main()
00179 {
00180   set_up();
00181   emergency_stop = 1;
00182   an.mode(PullUp);
00183   while(true){
00184     debug_led_0 = !debug_led_0;
00185     if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){
00186         debug_led_2 = 1;
00187     }
00188     else debug_led_2 = 0;
00189     X_power = 0.0;
00190     Y_power = 0.0;
00191     spin_power = 0.0;
00192     if(data_a)ir_distance = IR0.get_Averagingdistance();
00193     else ir_distance = IR1.get_Averagingdistance();
00194 
00195     to_main();
00196 
00197     if(instruction_mode == 1&&mode == 0){
00198 
00199       pid_x.reset();
00200       pid_x.setTunings(10.0,1.0,0.000001);
00201       pid_x.setInputLimits(-1000.0,1000.0);
00202       pid_x.setOutputLimits(-1.0,1.0);
00203       pid_x.setBias(0);
00204       pid_x.setMode(1);
00205       pid_x.setSetPoint(0.0);
00206 
00207       position_control_1.targetXY(1,int(y_point));
00208 
00209       pid_spin.reset();
00210       pid_spin.setTunings(10.0,1.0,0.000001);
00211       pid_spin.setInputLimits(-180.0,180.0);
00212       pid_spin.setOutputLimits(-0.5,0.5);
00213       pid_spin.setBias(0);
00214       pid_spin.setMode(1);
00215       pid_spin.setSetPoint(0.0);
00216 
00217       mode = 1;
00218     }
00219 
00220     if(instruction_mode == 2&&mode == 1){
00221 
00222       pid_y.reset();
00223       pid_y.setTunings(3.0,1.0,0.000001);
00224       pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
00225       pid_y.setOutputLimits(-1.0,1.0);
00226       pid_y.setBias(0);
00227       pid_y.setMode(1);
00228       pid_y.setSetPoint(y_point);
00229 
00230       pid_spin.reset();
00231       pid_spin.setTunings(5.0,1.0,0.000001);
00232       pid_spin.setInputLimits(-180.0,180.0);
00233       pid_spin.setOutputLimits(-0.5,0.5);
00234       pid_spin.setBias(0);
00235       pid_spin.setMode(1);
00236       pid_spin.setSetPoint(0.0);
00237 
00238       mode = 2;
00239     }
00240 
00241     if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){
00242       dai_x = m.getOutX();
00243       mode = 0xff;
00244     }
00245 
00246     if(instruction_mode == 3&&mode == 0xff){
00247       pid_x.reset();
00248       pid_x.setTunings(3.0,1.0,0.000001);
00249       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00250       pid_x.setOutputLimits(-1.0,1.0);
00251       pid_x.setBias(0);
00252       pid_x.setMode(1);
00253       pid_x.setSetPoint(dai_x);
00254 
00255       pid_spin.reset();
00256       pid_spin.setTunings(5.0,1.0,0.000001);
00257       pid_spin.setInputLimits(-180.0,180.0);
00258       pid_spin.setOutputLimits(-0.5,0.5);
00259       pid_spin.setBias(0);
00260       pid_spin.setMode(1);
00261       pid_spin.setSetPoint(0.0);
00262 
00263       mode = 3;
00264     }
00265 
00266     if(ir_distance>=20&&mode == 3){
00267       dai_high_y = m.getOutY();
00268 
00269       pid_x.reset();
00270       pid_x.setTunings(3.0,1.0,0.000001);
00271       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00272       pid_x.setOutputLimits(-1.0,1.0);
00273       pid_x.setBias(0);
00274       pid_x.setMode(1);
00275       pid_x.setSetPoint(dai_x);
00276 
00277       pid_y.reset();
00278       pid_y.setTunings(10.0,1.0,0.000001);
00279       pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0);
00280       pid_y.setOutputLimits(-1.0,1.0);
00281       pid_y.setBias(0);
00282       pid_y.setMode(1);
00283       pid_y.setSetPoint(dai_high_y + gap);
00284 
00285       pid_spin.reset();
00286       pid_spin.setTunings(5.0,1.0,0.000001);
00287       pid_spin.setInputLimits(-180.0,180.0);
00288       pid_spin.setOutputLimits(-0.5,0.5);
00289       pid_spin.setBias(0);
00290       pid_spin.setMode(1);
00291       pid_spin.setSetPoint(0.0);
00292 
00293       mode = 4;
00294     }
00295 
00296     if(instruction_mode == 5&&mode == 4){
00297       //gap = GAP - 30;
00298 
00299       pid_y.reset();
00300       pid_y.setTunings(5.0,1.0,0.000001);
00301       pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0);
00302       pid_y.setOutputLimits(-1.0,1.0);
00303       pid_y.setBias(0);
00304       pid_y.setMode(1);
00305       pid_y.setSetPoint(dai_high_y + gap);
00306 
00307       pid_spin.reset();
00308       pid_spin.setTunings(5.0,1.0,0.000001);
00309       pid_spin.setInputLimits(-180.0,180.0);
00310       pid_spin.setOutputLimits(-0.5,0.5);
00311       pid_spin.setBias(0);
00312       pid_spin.setMode(1);
00313       pid_spin.setSetPoint(0.0);
00314 
00315       mode = 5;
00316     }
00317 /*
00318     if(m.getOutX() <= -1600 && mode==5){
00319       //gap = GAP - 50;
00320 
00321       pid_y.reset();
00322       pid_y.setTunings(5.0,1.0,0.000001);
00323       pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0);
00324       pid_y.setOutputLimits(-1.0,1.0);
00325       pid_y.setBias(0);
00326       pid_y.setMode(1);
00327       pid_y.setSetPoint(dai_high_y + gap);
00328 
00329       pid_spin.reset();
00330       pid_spin.setTunings(5.0,1.0,0.000001);
00331       pid_spin.setInputLimits(-180.0,180.0);
00332       pid_spin.setOutputLimits(-0.5,0.5);
00333       pid_spin.setBias(0);
00334       pid_spin.setMode(1);
00335       pid_spin.setSetPoint(0.0);
00336 
00337       mode = 6;
00338 
00339     }
00340     */
00341 
00342     if(an.read()==0)y_point = 0;
00343 
00344     if(mode == 1)mode1();
00345     if(mode == 2)mode2();
00346     if(mode == 3)mode3();
00347     if(mode == 4)mode4();
00348     if(mode == 5)mode5();
00349     if(mode == 6)mode6();
00350     if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;}
00351 
00352 
00353     omni.computeXY(Y_power,-X_power,-spin_power);
00354 
00355     for(int i = 0; i < 4; i++){
00356       omni_power[i] = 0.0;
00357       omni_power[i] = omni.wheel[i];
00358       motor[i].setSpeed(omni_power[i]);
00359     }
00360 
00361     //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);
00362 
00363   }
00364 }