main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "QEI.h"
00004 #include "Servo.h"
00005 #include "HMC6352.h"
00006 #include "main.h"
00007 
00008 //guro-baru
00009 double vl = 0;
00010 double vc = 0;
00011 double vs = 1;
00012 int clwise = 0;
00013 int anclwise = 0;
00014 
00015 int enkoda1;
00016 int enkoda2;
00017 
00018 int process = 0;
00019 int vc_count = 0;
00020 int mode_comp = 0;
00021 
00022 int inputPID = 0;
00023 
00024 //guro-baru end
00025 
00026 
00027 void PidUpdate(){
00028     static int state = WAIT,past_state = WAIT,next_state = WAIT;
00029 
00030     if(!mode_comp){
00031         if(vl && !vs){
00032             state = STRAIGHT;
00033         }
00034         if(vs){
00035             state = TURN;
00036         }
00037     }
00038     if((state != past_state)){
00039         mode_comp = 1;
00040     
00041         if(process == 0){
00042             if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
00043                 process++;
00044             }    
00045         }
00046         if(process == 1){
00047             if(state == STRAIGHT){
00048                 vl = 10;
00049                 vs = 0;
00050             }
00051             if(state == TURN){
00052                 vl = 0;
00053                 vs = 10;
00054             }        
00055             if(abs(vc) < 60){
00056                 vc_count++;
00057             }
00058             if(vc_count > 2){
00059                 vc_count = 0;
00060                 mode_comp = 0;
00061                 process = 0;
00062             }
00063         }
00064     }
00065 
00066     enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
00067     enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);
00068 
00069     if((!vs)&&(mode_comp == 0)){
00070         pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
00071     }else if((vs)&&(mode_comp == 0)){
00072         pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
00073     }
00074     
00075     if((!vs)&&(mode_comp)){
00076         pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
00077     }else if((vs)&&(mode_comp)){
00078         pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
00079     }
00080         
00081     vc = (int)pid.compute();
00082            
00083     printf("vc:%lf\n",vc);
00084     
00085     double fut_R = 0.5,fut_L = 0.5;
00086     
00087     if(abs(vc) > 250){
00088         vc = 0;
00089     }
00090 
00091     int vlR = 0;
00092     int vlL = 0;
00093     int vcR = vc;
00094     int vcL = vc;
00095     
00096     vlR = -vs;
00097     vlL =  vs;
00098     
00099     vl *= 0.5;
00100     vc *= 0.5;
00101     
00102     vlR *= 0.4;
00103     vlL *= 0.4;
00104     
00105     vcR *= 0.6;
00106     vcL *= 0.6;
00107 
00108     
00109     if(!vs){   
00110         if(vl > 0){
00111             fut_R = Convert_dekaruto((int)(vl - vc));
00112             fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); 
00113         }
00114         if(vl < 0){
00115             vc *= -1; 
00116             fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
00117             fut_L = Convert_dekaruto((int)(vl - vc)); 
00118         }
00119     }else{
00120         if(vlR < 0){
00121             vcR *= -1;  
00122         }
00123         
00124         if(vlL < 0){
00125             vcL *= -1;  
00126         }
00127         if(vs > 0){
00128             fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
00129             fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
00130         }
00131         
00132         if(vs < 0){
00133             fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
00134             fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
00135         }
00136     }
00137     
00138     servoR = fut_R;
00139     servoL = fut_L;
00140     
00141     if(!mode_comp){
00142         past_state = state;
00143     }    
00144 }
00145 
00146 void CompassUpdate(){
00147     inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
00148 }
00149 
00150 double vssOut(){
00151     double vss;
00152     vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
00153     
00154     if(abs(vss) < 10){
00155         vss = 0;
00156     }
00157     
00158     vss *= 3.0;
00159     
00160     
00161     if((vss)&&(abs(vss) < 500)){
00162         vss += (vss/abs(vss)) * 500;
00163     }
00164         
00165     if(abs(vss) > 1000){
00166         vss = 1000 * (vss/abs(vss));
00167     }
00168     
00169     return vss;
00170 }
00171 
00172 void move(int vll,int vss){
00173     if(!swR){
00174         wheel1.reset();
00175     }
00176     
00177     if(!swL){
00178         wheel2.reset();
00179     }
00180 
00181     vl = vll;
00182     vs = vss;
00183 }
00184 
00185 #define PINR_THR 2000
00186 
00187 int ping_button(int ping,int button){
00188     static int continue_flag = 0;
00189     static int change_flag = 0;
00190 
00191     if(continue_flag == 0){
00192         if(ping <= PINR_THR){
00193             ping_t.start();
00194             continue_flag = 1;
00195         }
00196     }    
00197     
00198     if(continue_flag == 1){
00199         //agatterutoki
00200         if(ping <= PINR_THR){
00201             if(change_flag == 0){
00202                 if(ping_t.read_ms() >= 150){
00203                     button = !button;
00204                     change_flag = 1;
00205                 }
00206             }
00207         }
00208         //tatisagari
00209         if(ping >= (PINR_THR+200)){
00210             ping_t.stop();
00211             ping_t.reset();
00212             continue_flag = 0;
00213             change_flag = 0;
00214         }       
00215     }
00216     return button;    
00217 }
00218 
00219 
00220 int main() {
00221     wait(3);
00222     
00223     timer2.start();
00224     ping_t.start();
00225 
00226     pid.setInputLimits(MINIMUM,MAXIMUM);
00227     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
00228     pid.setBias(PID_BIAS);
00229     pid.setMode(AUTO_MODE);
00230     pid.setSetPoint(REFERENCE);
00231     
00232     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00233     compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
00234     
00235     swR.mode(PullUp);
00236     swL.mode(PullUp);
00237     
00238     int setR = 0,setL = 0;
00239     int vll = 0,vss = 0;
00240     
00241     servoR = 0.595;
00242     servoL = 0.59;
00243     
00244     while(1){
00245         if(!swR){
00246             setR = 1;
00247         }
00248         
00249         if(!swL){
00250             setL = 1;
00251         }
00252         
00253         if(setR){
00254             servoR = 0.5;    
00255         }
00256         
00257         if(setL){
00258             servoL = 0.5;    
00259         }
00260         
00261         if(setR && setL){
00262             break;
00263         }
00264        
00265         wait(0.1);
00266     }
00267     
00268     wheel1.reset();
00269     wheel2.reset();
00270     
00271     pidUpdata.attach(&PidUpdate, PID_CYCLE);
00272     
00273     int button = 0;
00274 
00275     while(1) {
00276         vll = 0;
00277         vss = 0;
00278     
00279         Ultrasonic();
00280         
00281         button = ping_button(ultrasonicVal[0],button);
00282         
00283         if(button){
00284             vll = 400;
00285             led1 = 1;  
00286         }else{
00287             vll = -500;
00288             led1 = 0;
00289         }     
00290        
00291         move(vll,vss); 
00292     }
00293 }