main.cpp

00001 #include "mbed.h"
00002 #include "ColorSensor.h"
00003 #include "TextLCD.h"
00004 #include "Servo.h"
00005 #include "HMC6352.h"
00006 
00007 #include "main.h"
00008 
00009 
00010 void tic_sensor()
00011 {
00012     Ultrasonic();
00013     
00014     colorUpdate();
00015     
00016     /*lcd.cls();
00017     lcd.locate(0,0);
00018     lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00019     */
00020 }
00021 
00022 ////////////////////////////////////////移動関数//////////////////////////////////////////////
00023 /////////////////////////////////////////////////////////////////////////////////////////////
00024 void move(int vl,int vs){
00025     double fut_R,fut_L,true_vs;
00026     
00027     true_vs = abs(vs)/SPIN;
00028     
00029     if(true_vs > 40){
00030         vl = 0;
00031         vs = 100*(vs/abs(vs));
00032     }
00033     
00034     fut_R = Convert_dekaruto((vl + vs));
00035     fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
00036     
00037     fut_R = Convert_dekaruto(-50);
00038     fut_L = Convert_dekaruto(-50);
00039  
00040     servoR = fut_R;
00041     servoL = fut_L;
00042        
00043     //printf("R:%lf   L:%lf\n",fut_R,fut_L);
00044     //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
00045 }
00046 
00047 /*void PidUpdate()
00048 {   
00049     inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
00050     //printf("%lf\n",inputPID);      
00051 }*/
00052 
00053 double vsOut(){
00054     double vs;
00055     vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
00056     vs = vs * 8;
00057     
00058     if(vs/abs(vs) < 0){
00059         //vs *= 1.3;   
00060     }
00061     
00062     if(abs(vs) > 90)vs = 90*(vs/abs(vs));
00063     if(abs(vs) < 25) vs = 25*(vs/abs(vs));
00064     
00065     return vs;
00066 }
00067 
00068 ////////////////////////////////////////超音波センサの////////////////////////////////////////
00069 ////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
00070 int ping_button(int ping,int button){
00071     static int continue_flag = 0;
00072     static int change_flag = 0;
00073 
00074     if(continue_flag == 0){
00075         if(ping <= PINR_THR){
00076             ping_t.start();
00077             continue_flag = 1;
00078         }
00079     }    
00080     
00081     if(continue_flag == 1){
00082         //agatterutoki
00083         if(ping <= PINR_THR){
00084             if(change_flag == 0){
00085                 if(ping_t.read_ms() >= 300){
00086                     button = !button;
00087                     change_flag = 1;
00088                 }
00089             }
00090         }
00091         //tatisagari
00092         if(ping >= (PINR_THR+200)){
00093             ping_t.stop();
00094             ping_t.reset();
00095             continue_flag = 0;
00096             change_flag = 0;
00097         }       
00098     }
00099     return button;    
00100 }
00101 
00102 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
00103 ////////////////////////////////////////補正プログラム////////////////////////////////////////
00104 void rivisedate()
00105 {
00106     unsigned long red = 0,green = 0,blue =0;
00107     static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00108     
00109     //最初の20回だけ平均を取る
00110     for (int i=0;i<=20;i++){
00111          color0.getRGB(R[0],G[0],B[0]);
00112          red       += R[0] ;
00113          green     += G[0] ;
00114          blue      += B[0] ;
00115          //pc.printf(" %d  %d\n",ptm(sum),sum);
00116     }
00117     
00118     rir = (double)green/ red ;
00119     rib = (double)green/ blue ;
00120 }
00121 
00122 void colorUpdate()
00123 {
00124     double colorSum[COLOR_NUM];
00125     unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00126 
00127     color0.getRGB(R[0],G[0],B[0]);
00128     color1.getRGB(R[1],G[1],B[1]);
00129     color2.getRGB(R[2],G[2],B[2]);
00130     /*color3.getRGB(R[3],G[3],B[3]);
00131     color4.getRGB(R[4],G[4],B[4]);
00132     color5.getRGB(R[5],G[5],B[5]);*/
00133     
00134     for (int i=0; i<COLOR_NUM; i++){
00135         colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
00136         redp[i]   = R[i]* rir * 100 / colorSum[i];
00137         greenp[i] = G[i]      * 100 / colorSum[i];
00138         bluep[i]  = B[i]* rib * 100 / colorSum[i];
00139     }
00140 }
00141 
00142 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
00143 ///////////////////////////////////////////////////////////////////////////////////////////
00144 uint8_t jumpcondition()
00145 {
00146     uint8_t threshold = 0, t[3] = {0};
00147     
00148     //青から赤に0.5秒以内に反応したらジャンプ
00149     for(int i=0; i<COLOR_NUM; i++){
00150         if(bluep[i] >= B_THR){
00151             color_t[i].reset();
00152             color_t[i].start();
00153             t[i] = 0;
00154         }else if(redp[i] >= R_THR){
00155             t[i] = color_t[i].read_ms();
00156         }else{
00157             t[i] = 0;
00158         }
00159  
00160         if((t[i] <= 500) && (t[i] != 0)){
00161             threshold++;
00162         }
00163     }
00164     
00165     return threshold;
00166 }
00167 
00168 void jumping(uint8_t threshold)
00169 {
00170     //超音波でジャンプのタイミング合わせる
00171     if(threshold >= 1){
00172             jump_t.reset();
00173             jump_t.start();
00174             while(ultrasonicVal[0] < 1700){
00175                 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
00176                 air[0] = 1;  air[1] = 0;
00177                 
00178                 if(jump_t.read_ms() > 1000)break;
00179             }
00180             led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
00181             air[0] = 0;  air[1] = 1;
00182             wait(0.5);
00183     }else{
00184             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00185     }
00186 }
00187 
00188 
00189 
00190 
00191 
00192 
00193 
00194 
00195 
00196 
00197 
00198 int main()
00199 {
00200     rivisedate();
00201 
00202     timer2.start();
00203     ping_t.start();
00204     
00205         
00206     //unsigned R, G, B;
00207     int vl;
00208     double vs;
00209     uint8_t button, state=0;
00210     
00211     //pc.baud(115200);
00212     air[0] = 0; air[1] = 1;
00213   
00214     
00215     //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00216     //pidUpdata.attach(&PidUpdate, PID_CYCLE);
00217     interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
00218   
00219   while(1)
00220   {
00221     
00222     
00223     
00224     pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00225     /*
00226     lcd.cls();
00227     lcd.locate(0,0);
00228     lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
00229     lcd.locate(0,1);
00230     lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
00231      */
00232     //pc.printf("%d\n", ultrasonicVal[0]);
00233     
00234     
00235     jumping(jumpcondition());
00236     
00237     
00238     
00239     button = ping_button(ultrasonicVal[1],button);
00240         
00241     if(button){
00242         state = GO;
00243     }else{
00244         state = STOP;
00245     }
00246     
00247     
00248     
00249     
00250     
00251     if(state == GO){
00252         vl = -90;//led[0] = 1; led[1] = 1;
00253     }else if(state == STOP){
00254         vl = 0;//led[0] = 0; led[1] = 0;
00255     }
00256     
00257     vl = vl      * STRAIGHT ;
00258     vs = vsOut() * SPIN     ;
00259     
00260     vl *= 0.5;
00261     vs *= 0.3;
00262         
00263     move(vl,(int)vs); 
00264   }
00265 }