main.cpp

00001 #define pi 3.141593
00002 #include "mbed.h"
00003 #include "Motor.h"
00004 #include "PID.h"
00005 #include "QEI.h"
00006 #include "Servo.h"
00007 #include "DebounceIn.h"
00008 #define RATE 0.05
00009 #define shoottime 0.5
00010 #define DEADTIME 500
00011 BusOut air(p5,p6);
00012 DigitalOut out(p7);
00013 Serial conn(p9,p10);
00014 Serial pc(USBTX,USBRX);
00015 Serial slave(p28,p27);
00016 PwmOut Ll(LED1);
00017 PwmOut Rl(LED2);
00018 BusOut mled(LED3,LED4);
00019 Servo L(p25);
00020 Servo R(p26);
00021 PID Tp(50,4000000,0,0.001);
00022 BusOut led(p8,p13,p14,p24);
00023 Motor ot(p23,p20,p19);
00024 QEI sensort(p29,p30,NC,624);
00025 //Motor sup1(p21,p15,p16);
00026 //Motor sup2(p22,p17,p18);
00027 SoftPWM sup1p(p21);
00028 BusOut sup1d(p15,p16);
00029 SoftPWM sup2p(p22);
00030 BusOut sup2d(p18,p17);
00031 DebounceIn limit1(p11);
00032 DebounceIn limit2(p12);
00033 Timeout ai;
00034 Timer timer1;
00035 Timer timer2;
00036 char read;
00037 int Rs = 0,Ls = 0,d=0,su1 = 0,su2 = 0;
00038 int i = 0;
00039 int old_limit1 = 0,old_limit2 = 0,new_limit1 = 0,new_limit2 = 0,count1 = 0,count2 = 0,target_count1 = 0,target_count2 = 0,supply_seq1 = 0,supply_seq2 = 0;
00040 void sup1_speed(float speed)
00041 {
00042     if(speed>0) {
00043         sup1d = 1;
00044     } else if(speed < 0) {
00045         sup1d =2;
00046     }
00047     sup1p = fabs(speed);
00048 }
00049 void sup2_speed(float speed)
00050 {
00051     if(speed>0) {
00052         sup2d = 1;
00053     } else if(speed < 0) {
00054         sup2d =2;
00055     }
00056     sup2p = fabs(speed);
00057 }
00058 void sup1_brake()
00059 {
00060     sup1d = 0;
00061     sup1p = 1;
00062 }
00063 void sup2_brake()
00064 {
00065     sup2d = 0;
00066     sup2p = 1;
00067 }
00068 void supply_1()
00069 {
00070     sup1_speed(0.4);
00071     target_count1 +=3;
00072     supply_seq1 = 1;
00073 }
00074 void supply_2()
00075 {
00076     sup2_speed(0.4);
00077     target_count2 +=3;
00078     supply_seq2 = 1;
00079 }
00080 void supply()
00081 {
00082     if (supply_seq1 ==0 && supply_seq2 == 0) {
00083         if(d%2) {
00084             supply_1();
00085         } else {
00086             supply_2();
00087         }
00088         d++;
00089     }
00090 }
00091 
00092 void count_check()
00093 {
00094     if((target_count1 == count1)&&(supply_seq1 == 1)) {
00095         sup1_speed(-0.4);
00096         target_count1 += 2;
00097         supply_seq1 = 2;
00098     } else if((target_count1 == count1) && (supply_seq1 == 2)) {
00099         sup1_brake();
00100         supply_seq1 = 0;
00101         mled = 0;
00102     }
00103     if((target_count2 == count2)&&(supply_seq2 == 1)) {
00104         sup2_speed(-0.4);
00105         target_count2 += 2;
00106         supply_seq2 = 2;
00107     } else if((target_count2 == count2) && (supply_seq2 == 2)) {
00108         sup2_brake();
00109         supply_seq2 = 0;
00110         mled = 0;
00111     }
00112 }
00113 void limit1_count()
00114 {
00115     new_limit1 = limit1;
00116     if ((new_limit1==0) && (old_limit1==1)) count1++;
00117     old_limit1 = new_limit1;
00118 }
00119 void limit2_count()
00120 {
00121     new_limit2 = limit2;
00122     if ((new_limit2==0) && (old_limit2==1)) count2++;
00123     old_limit2 = new_limit2;
00124 }
00125 void zero()
00126 {
00127     air = 0;
00128     i = 0;
00129     out = 1;
00130 }
00131 void rev()
00132 {
00133     air = 2;
00134     ai.attach(&zero,shoottime);
00135     out = 0;
00136 }
00137 int main()
00138 {
00139     L = 0.5;
00140     R = 0.5;
00141     limit1.mode(PullUp);
00142     limit2.mode(PullUp);
00143     led = 1;
00144     int dead1 = 0,dead2 = 0;
00145     double tilt = 0,lo = 0,ro = 0;
00146     int8_t ttilt = 60,tmpread = 0,tmpttilt = 0,ajst = 0;
00147     char tro = 0,tlo = 0,narasup = 60;
00148     Tp.setInputLimits(-45,45);
00149     Tp.setOutputLimits(-0.9,0.9);
00150     Tp.setMode(1);
00151     Tp.setBias(0.0);
00152     wait(3);
00153     led= 15;
00154     conn.baud(38400);
00155     while(1) {
00156         //ps3コントローラーシリアル受信
00157         if(conn.getc() == 255) {
00158             read = conn.getc();
00159             ttilt = conn.getc();
00160             tro =conn.getc();
00161             tlo = conn.getc();
00162             if (tro == 255){
00163                 ro =ro;
00164             }else{
00165                 ro = (tro-127)/127.0*0.9;
00166             }
00167             if(tlo ==255){
00168                 lo = lo;
00169             }else {
00170                 lo = (tlo-127)/127.0*0.9;
00171             }
00172         }
00173         //nara シリアル受信
00174         if(slave.readable()) {
00175             narasup = slave.getc();
00176             if((narasup-60) == 1 && supply_seq1 == 0) {
00177                 mled = 1;
00178                 supply_1();
00179             } else if((narasup-60) == 2 && supply_seq2 == 0) {
00180                 supply_2();
00181                 mled =2;
00182             }
00183         }
00184         if (read > 4){ //通信不良対策
00185             read = 0;
00186         }
00187         led = read;
00188         if(read==2 && i == 0) {
00189             air = 1;
00190             ai.attach(&rev,shoottime);
00191             i = 1;
00192             supply();
00193         }
00194         limit1_count();
00195         limit2_count();
00196         count_check();
00197         /*射角制御*/
00198         Tp.setSetPoint((ttilt-60)/2.0);
00199         tilt = double(-sensort.getPulses());
00200         tilt = tilt*49.0/1745.0;
00201         Tp.setProcessValue(tilt);
00202         /*足の出力が小さい場合はゼロとする*/
00203         /*if(timer1.read_ms() > DEADTIME) {
00204             dead1 = 0;
00205             timer1.stop();
00206             timer1.reset();
00207         }
00208         if(timer2.read_ms() > DEADTIME) {
00209             dead2 = 0;
00210             timer2.stop();
00211             timer2.reset();
00212         }
00213         if ((L >0.5 && (lo+1.0)/2.0<=0.5) || (L <0.5 && (lo+1.0)/2.0>=0.5) || dead1 ==1) {
00214             L = 0.5;
00215             Ll = 0.5;
00216             dead1 =1;
00217             timer1.start();
00218         } else {
00219             L = (lo+1.0)/2.0;
00220             Ll = (lo+1.0)/2.0;
00221         }
00222         if ( (R >0.5 && (ro+1.0)/2.0<=0.5) || (R <0.5 && (ro+1.0)/2.0>=0.5)  || dead2 ==1) {
00223             R = 0.5;
00224             Rl = 0.5;
00225             dead2 = 1;
00226             timer2.start();
00227         } else {
00228             R = (ro+1.0)/2.0;
00229             Rl = (ro+1.0)/2.0;
00230         }*/
00231         R = (ro+1.0)/2.0;
00232         Rl = (ro+1.0)/2.0;
00233         L = (lo+1.0)/2.0;
00234         Ll = (lo+1.0)/2.0;
00235         /*スレーブにreadを送信*/
00236         slave.putc(read);
00237         /*各モーターに出力*/
00238         //pc.printf("%d\n\r",narasup);
00239         ot.speed(Tp.compute());
00240         pc.printf("%f\n\r",R);
00241         //pc.printf("%f \n\r",Tp.compute());
00242     }
00243 }