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 DigitalOut emergency_stop(EMERGENCY_STOP);
00036 
00037 IRsensor IR0(IR_0);
00038 
00039 int mode;
00040 int instruction_mode;
00041 double omni_power[4];
00042 double X_power,Y_power;
00043 double spin_power;
00044 float y_point = YPOINT;
00045 
00046 double dai_x,dai_high_y;
00047 int gap = GAP;
00048 
00049 void set_up()
00050 {
00051   float theta = PIII / 4;
00052   omni.wheel[0].setRadian(PIII - theta);
00053   omni.wheel[1].setRadian(theta);
00054   omni.wheel[2].setRadian(-theta);
00055   omni.wheel[3].setRadian(PIII + theta);
00056 
00057   s.setHeaders('A','Z');
00058   s.startReceive(1);
00059 
00060   IR0.startAveraging(5);
00061 
00062 }
00063 
00064 void mode1()
00065 {
00066   pid_x.setProcessValue(m.getOutX());
00067   X_power += pid_x.compute();
00068 
00069   position_control_1.compute(0.0,m.getOutY());
00070   Y_power += position_control_1.getVelocityY();
00071 
00072   pid_spin.setProcessValue(m.getjyroAngle());
00073   spin_power = pid_spin.compute();
00074 
00075 }
00076 
00077 void mode2()
00078 {
00079   X_power -= 0.3;
00080 
00081   pid_y.setProcessValue(m.getOutY());
00082   Y_power += pid_y.compute();
00083 
00084   pid_spin.setProcessValue(m.getjyroAngle());
00085   spin_power = pid_spin.compute();
00086 
00087 }
00088 
00089 void mode3()
00090 {
00091   pid_x.setProcessValue(m.getOutX());
00092   X_power += pid_x.compute();
00093 
00094   Y_power += 0.3;
00095 
00096   pid_spin.setProcessValue(m.getjyroAngle());
00097   spin_power = pid_spin.compute();
00098 
00099 }
00100 
00101 void mode4()
00102 {
00103   pid_x.setProcessValue(m.getOutX());
00104   X_power += pid_x.compute();
00105 
00106   pid_y.setProcessValue(m.getOutY());
00107   Y_power += pid_y.compute();
00108 
00109   pid_spin.setProcessValue(m.getjyroAngle());
00110   spin_power = pid_spin.compute();
00111 
00112 }
00113 
00114 void to_main()
00115 {
00116   unsigned char data[5];
00117   unsigned char getdata[1];
00118   int X_ = m.getOutX();
00119   int Y_ = m.getOutY();
00120   data[0] = mode;
00121   if(X_>0){
00122       data[1] = X_&0x00ff;
00123       data[2] = (X_>>8)&0x00ff;
00124   }
00125   else{
00126       X_*=-1;
00127       data[1] = X_&0x00ff;
00128       data[2] = ((X_>>8)&0x00ff )+ (1<<7);
00129   }
00130   if(Y_>0){
00131       data[3] = Y_&0x00ff;
00132       data[4] = (Y_>>8)&0x00ff;
00133   }
00134   else{
00135       Y_*=-1;
00136       data[3] = Y_&0x00ff;
00137       data[4] = ((Y_>>8)&0x00ff )+ (1<<7);
00138   }
00139   s.sendData(data,5);
00140   s.getData(getdata);
00141   instruction_mode = getdata[0];
00142   if(instruction_mode)debug_led_1 = !debug_led_1;
00143 }
00144 
00145 int main()
00146 {
00147   set_up();
00148   emergency_stop = 1;
00149   while(true){
00150     debug_led_0 = !debug_led_0;
00151     X_power = 0.0;
00152     Y_power = 0.0;
00153     spin_power = 0.0;
00154 
00155     to_main();
00156 
00157     if(instruction_mode == 1&&mode == 0){
00158 
00159       pid_x.reset();
00160       pid_x.setTunings(3.0,1.0,0.000001);
00161       pid_x.setInputLimits(-1000.0,1000.0);
00162       pid_x.setOutputLimits(-1.0,1.0);
00163       pid_x.setBias(0);
00164       pid_x.setMode(1);
00165       pid_x.setSetPoint(0.0);
00166 
00167       position_control_1.targetXY(1,int(y_point));
00168 
00169       pid_spin.reset();
00170       pid_spin.setTunings(5.0,1.0,0.000001);
00171       pid_spin.setInputLimits(-180.0,180.0);
00172       pid_spin.setOutputLimits(-0.5,0.5);
00173       pid_spin.setBias(0);
00174       pid_spin.setMode(1);
00175       pid_spin.setSetPoint(0.0);
00176 
00177       mode = 1;
00178     }
00179 
00180     if(instruction_mode == 2&&mode == 1){
00181 
00182       pid_y.reset();
00183       pid_y.setTunings(1.0,1.0,0.000001);
00184       pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
00185       pid_y.setOutputLimits(-1.0,1.0);
00186       pid_y.setBias(0);
00187       pid_y.setMode(1);
00188       pid_y.setSetPoint(y_point);
00189 
00190       pid_spin.reset();
00191       pid_spin.setTunings(5.0,1.0,0.000001);
00192       pid_spin.setInputLimits(-180.0,180.0);
00193       pid_spin.setOutputLimits(-0.5,0.5);
00194       pid_spin.setBias(0);
00195       pid_spin.setMode(1);
00196       pid_spin.setSetPoint(0.0);
00197 
00198       mode = 2;
00199     }
00200 
00201     if(IR0.get_Averagingdistance()<=20&&mode == 2){
00202       dai_x = m.getOutX();
00203       mode = 0xff;
00204     }
00205 
00206     if(instruction_mode == 3&&mode == 0xff){
00207       pid_x.reset();
00208       pid_x.setTunings(3.0,1.0,0.000001);
00209       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00210       pid_x.setOutputLimits(-1.0,1.0);
00211       pid_x.setBias(0);
00212       pid_x.setMode(1);
00213       pid_x.setSetPoint(dai_x);
00214 
00215       pid_spin.reset();
00216       pid_spin.setTunings(5.0,1.0,0.000001);
00217       pid_spin.setInputLimits(-180.0,180.0);
00218       pid_spin.setOutputLimits(-0.5,0.5);
00219       pid_spin.setBias(0);
00220       pid_spin.setMode(1);
00221       pid_spin.setSetPoint(0.0);
00222 
00223       mode = 3;
00224     }
00225 
00226     if(IR0.get_Averagingdistance()>=50&&mode == 3){
00227       dai_high_y = m.getOutY();
00228 
00229       pid_x.reset();
00230       pid_x.setTunings(3.0,1.0,0.000001);
00231       pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
00232       pid_x.setOutputLimits(-1.0,1.0);
00233       pid_x.setBias(0);
00234       pid_x.setMode(1);
00235       pid_x.setSetPoint(dai_x);
00236 
00237       pid_y.reset();
00238       pid_y.setTunings(1.0,1.0,0.000001);
00239       pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0);
00240       pid_y.setOutputLimits(-1.0,1.0);
00241       pid_y.setBias(0);
00242       pid_y.setMode(1);
00243       pid_y.setSetPoint(dai_high_y + gap);
00244 
00245       pid_spin.reset();
00246       pid_spin.setTunings(5.0,1.0,0.000001);
00247       pid_spin.setInputLimits(-180.0,180.0);
00248       pid_spin.setOutputLimits(-0.5,0.5);
00249       pid_spin.setBias(0);
00250       pid_spin.setMode(1);
00251       pid_spin.setSetPoint(0.0);
00252 
00253       mode = 4;
00254     }
00255 
00256     if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
00257       mode = 0xff;
00258     }
00259 
00260     if(mode == 1)mode1();
00261     if(mode == 2)mode2();
00262     if(mode == 3)mode3();
00263     if(mode == 4)mode4();
00264 
00265     omni.computeXY(Y_power,-X_power,-spin_power);
00266 
00267     for(int i = 0; i < 4; i++){
00268       omni_power[i] = 0.0;
00269       omni_power[i] = omni.wheel[i];
00270       motor[i].setSpeed(omni_power[i]);
00271     }
00272 
00273   }
00274 }