main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "QEI.h"
00004 #include "Servo.h"
00005 #include "HMC6352.h"
00006 #include "ColorSensor.h"
00007 #include "TextLCD.h"
00008 
00009 #include "main.h"
00010 
00011 
00012 
00013 
00014 
00015 void tic_sensor()
00016 {
00017     Ultrasonic();
00018     
00019     colorUpdate();
00020     
00021     /*lcd.cls();
00022     lcd.locate(0,0);
00023     lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00024     */
00025 }
00026 
00027 
00028 
00029 
00030 
00031 ////////////////////////////////////////移動関数//////////////////////////////////////////////
00032 /////////////////////////////////////////////////////////////////////////////////////////////
00033 void PidUpdate(){
00034     static int state = WAIT,past_state = WAIT,next_state = WAIT;
00035 
00036     if(!mode_comp){
00037         if(vl && !vs){
00038             state = STRAIGHT;
00039         }
00040         if(vs){
00041             state = TURN;
00042         }
00043     }
00044     if((state != past_state)){
00045         mode_comp = 1;
00046     
00047         if(process == 0){
00048             if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
00049                 process++;
00050             }    
00051         }
00052         if(process == 1){
00053             if(state == STRAIGHT){
00054                 vl = 10;
00055                 vs = 0;
00056             }
00057             if(state == TURN){
00058                 vl = 0;
00059                 vs = 10;
00060             }        
00061             if(abs(vc) < 60){
00062                 vc_count++;
00063             }
00064             if(vc_count > 2){
00065                 vc_count = 0;
00066                 mode_comp = 0;
00067                 process = 0;
00068             }
00069         }
00070     }
00071 
00072     enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
00073     enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);
00074 
00075     if((!vs)&&(mode_comp == 0)){
00076         pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
00077     }else if((vs)&&(mode_comp == 0)){
00078         pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
00079     }
00080     
00081     if((!vs)&&(mode_comp)){
00082         pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
00083     }else if((vs)&&(mode_comp)){
00084         pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
00085     }
00086         
00087     vc = (int)pid.compute();
00088            
00089     printf("vc:%lf\n",vc);
00090     
00091     double fut_R = 0.5,fut_L = 0.5;
00092     
00093     if(abs(vc) > 250){
00094         vc = 0;
00095     }
00096 
00097     int vlR = 0;
00098     int vlL = 0;
00099     int vcR = vc;
00100     int vcL = vc;
00101     
00102     vlR = -vs;
00103     vlL =  vs;
00104     
00105     vl *= 0.5;
00106     vc *= 0.5;
00107     
00108     vlR *= 0.4;
00109     vlL *= 0.4;
00110     
00111     vcR *= 0.6;
00112     vcL *= 0.6;
00113 
00114     
00115     if(!vs){   
00116         if(vl > 0){
00117             fut_R = Convert_dekaruto((int)(vl - vc));
00118             fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); 
00119         }
00120         if(vl < 0){
00121             vc *= -1; 
00122             fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
00123             fut_L = Convert_dekaruto((int)(vl - vc)); 
00124         }
00125     }else{
00126         if(vlR < 0){
00127             vcR *= -1;  
00128         }
00129         
00130         if(vlL < 0){
00131             vcL *= -1;  
00132         }
00133         if(vs > 0){
00134             fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
00135             fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
00136         }
00137         
00138         if(vs < 0){
00139             fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
00140             fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
00141         }
00142     }
00143     
00144     servoR = fut_R;
00145     servoL = fut_L;
00146     
00147     if(!mode_comp){
00148         past_state = state;
00149     }    
00150 }
00151 
00152 void CompassUpdate(){
00153     inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
00154 }
00155 
00156 double vssOut(){
00157     double vss;
00158     vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
00159     
00160     if(abs(vss) < 10){
00161         vss = 0;
00162     }
00163     
00164     vss *= 3.0;
00165     
00166     
00167     if((vss)&&(abs(vss) < 500)){
00168         vss += (vss/abs(vss)) * 500;
00169     }
00170         
00171     if(abs(vss) > 1000){
00172         vss = 1000 * (vss/abs(vss));
00173     }
00174     
00175     return vss;
00176 }
00177 
00178 void move(int vll,int vss){
00179     if(!swR){
00180         wheel1.reset();
00181     }
00182     
00183     if(!swL){
00184         wheel2.reset();
00185     }
00186 
00187     vl = vll;
00188     vs = vss;
00189 }
00190 
00191 
00192 ////////////////////////////////////////超音波センサの////////////////////////////////////////
00193 ////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
00194 int ping_button(int ping,int button){
00195     static int continue_flag = 0;
00196     static int change_flag = 0;
00197 
00198     if(continue_flag == 0){
00199         if(ping <= PINR_THR){
00200             ping_t.start();
00201             continue_flag = 1;
00202         }
00203     }    
00204     
00205     if(continue_flag == 1){
00206         //agatterutoki
00207         if(ping <= PINR_THR){
00208             if(change_flag == 0){
00209                 if(ping_t.read_ms() >= 150){
00210                     button = !button;
00211                     change_flag = 1;
00212                 }
00213             }
00214         }
00215         //tatisagari
00216         if(ping >= (PINR_THR+200)){
00217             ping_t.stop();
00218             ping_t.reset();
00219             continue_flag = 0;
00220             change_flag = 0;
00221         }       
00222     }
00223     return button;    
00224 }
00225 
00226 
00227 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
00228 ////////////////////////////////////////補正プログラム////////////////////////////////////////
00229 void rivisedate()
00230 {
00231     unsigned long red = 0,green = 0,blue =0;
00232     static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00233     
00234     //最初の20回だけ平均を取る
00235     for (int i=0;i<=20;i++){
00236          color0.getRGB(R[0],G[0],B[0]);
00237          red       += R[0] ;
00238          green     += G[0] ;
00239          blue      += B[0] ;
00240          //pc.printf(" %d  %d\n",ptm(sum),sum);
00241     }
00242     
00243     rir = (double)green/ red ;
00244     rib = (double)green/ blue ;
00245 }
00246 
00247 void colorUpdate()
00248 {
00249     double colorSum[COLOR_NUM];
00250     unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00251 
00252     color0.getRGB(R[0],G[0],B[0]);
00253     color1.getRGB(R[1],G[1],B[1]);
00254     color2.getRGB(R[2],G[2],B[2]);
00255     /*color3.getRGB(R[3],G[3],B[3]);
00256     color4.getRGB(R[4],G[4],B[4]);
00257     color5.getRGB(R[5],G[5],B[5]);*/
00258     
00259     for (int i=0; i<COLOR_NUM; i++){
00260         colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
00261         redp[i]   = R[i]* rir * 100 / colorSum[i];
00262         greenp[i] = G[i]      * 100 / colorSum[i];
00263         bluep[i]  = B[i]* rib * 100 / colorSum[i];
00264     }
00265 }
00266 
00267 
00268 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
00269 ///////////////////////////////////////////////////////////////////////////////////////////
00270 uint8_t jumpcondition()
00271 {
00272     uint8_t threshold = 0, t[3] = {0};
00273     
00274     //青から赤に0.5秒以内に反応したらジャンプ
00275     for(int i=0; i<COLOR_NUM; i++){
00276         if(bluep[i] >= B_THR){
00277             color_t[i].reset();
00278             color_t[i].start();
00279             t[i] = 0;
00280         }else if(redp[i] >= R_THR){
00281             t[i] = color_t[i].read_ms();
00282         }else{
00283             t[i] = 0;
00284         }
00285  
00286         if((t[i] <= 500) && (t[i] != 0)){
00287             threshold++;
00288         }
00289     }
00290     
00291     return threshold;
00292 }
00293 
00294 void jumping(uint8_t threshold)
00295 {
00296     //超音波でジャンプのタイミング合わせる
00297     if(threshold >= 1){
00298             jump_t.reset();
00299             jump_t.start();
00300             while(ultrasonicVal[0] < 1700){
00301                 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
00302                 air[0] = 1;  air[1] = 0;
00303                 
00304                 if(jump_t.read_ms() > 1000)break;
00305             }
00306             led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
00307             air[0] = 0;  air[1] = 1;
00308             wait(0.5);
00309     }else{
00310             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00311     }
00312 }
00313 
00314 
00315 uint8_t robostop()
00316 {
00317     if(bluep[1] >= B_THR){
00318         return 1;
00319     }else{
00320         return 0;
00321     }
00322 }
00323 
00324 
00325 
00326 
00327 
00328 
00329 
00330 
00331 
00332 
00333 
00334 int main() {
00335     rivisedate();
00336     wait(3);
00337     
00338     timer2.start();
00339     ping_t.start();
00340 
00341     pid.setInputLimits(MINIMUM,MAXIMUM);
00342     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00343     pid.setBias(PID_BIAS);
00344     pid.setMode(AUTO_MODE);
00345     pid.setSetPoint(REFERENCE);
00346     
00347     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00348     compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
00349     
00350     swR.mode(PullUp);
00351     swL.mode(PullUp);
00352         
00353     air[0] = 0; air[1] = 1;
00354  
00355     int setR = 0,setL = 0;
00356     int vll = 0,vss = 0;
00357     
00358     servoR = 0.595;
00359     servoL = 0.59;
00360     
00361     while(1){
00362         if(!swR){
00363             setR = 1;
00364         }
00365         
00366         if(!swL){
00367             setL = 1;
00368         }
00369         
00370         if(setR){
00371             servoR = 0.5;    
00372         }
00373         
00374         if(setL){
00375             servoL = 0.5;    
00376         }
00377         
00378         if(setR && setL){
00379             break;
00380         }
00381        
00382         wait(0.1);
00383     }
00384     
00385     wheel1.reset();
00386     wheel2.reset();
00387     
00388     interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
00389     pidUpdata.attach(&PidUpdate, PID_CYCLE);
00390     
00391     uint8_t button, state=0;
00392 
00393     while(1) {
00394         vll = 0;
00395         vss = 0;
00396     
00397         pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00398         jumping(jumpcondition());
00399     
00400     
00401     
00402         button = ping_button(ultrasonicVal[1],button);
00403             
00404         if(button){
00405             state = GO;
00406         }else{
00407             state = STOP;
00408         }
00409         
00410         
00411         
00412         
00413         
00414         if(state == GO){
00415             vll = 700;//led[0] = 1; led[1] = 1;
00416         }else if(state == STOP){
00417             vll = -700;//led[0] = 0; led[1] = 0;
00418         }
00419            
00420         move(vll,vss); 
00421     }
00422 }