main.cpp

00001 #include "mbed.h"
00002 #include "ColorSensor.h"
00003 #include "TextLCD.h"
00004 
00005 #include "main.h"
00006 
00007 
00008 void tic_sensor()
00009 {
00010 
00011     colorUpdate(1);
00012     //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00013     /*lcd.cls();
00014     lcd.locate(0,0);
00015     lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00016     */
00017 }
00018 
00019 
00020 
00021 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
00022 ////////////////////////////////////////補正プログラム////////////////////////////////////////
00023 void rivisedate()
00024 {
00025     unsigned long red = 0,green = 0,blue =0;
00026     static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00027     
00028     //最初の20回だけ平均を取る
00029     for (int i=0;i<=20;i++){
00030          color0.getRGB(R[0],G[0],B[0]);
00031          red       += R[0] ;
00032          green     += G[0] ;
00033          blue      += B[0] ;
00034          //pc.printf(" %d  %d\n",ptm(sum),sum);
00035     }
00036     
00037     rir = (double)green/ red ;
00038     rib = (double)green/ blue ;
00039 }
00040 
00041 void colorUpdate(uint8_t mode)
00042 {
00043     double colorSum[COLOR_NUM];
00044     unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
00045 
00046 
00047     for (int i=0; i<COLOR_NUM; i++){
00048         R[i] = 0;
00049         G[i] = 0;
00050         B[i] = 0;
00051         redp[i]   = 0;
00052         greenp[i] = 0;
00053         bluep[i]  = 0;
00054     }
00055     
00056     //カラーセンサの切り替え    
00057     if(mode == 1){
00058         color0.getRGB(R[0],G[0],B[0]);
00059         color1.getRGB(R[1],G[1],B[1]);
00060         color2.getRGB(R[2],G[2],B[2]);
00061     }else{
00062         color3.getRGB(R[3],G[3],B[3]);
00063         color4.getRGB(R[4],G[4],B[4]);
00064         color5.getRGB(R[5],G[5],B[5]);
00065     }
00066     
00067     /*for (int i=0; i<COLOR_NUM; i++){
00068         colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
00069         redp[i]   = R[i]* rir * 100 / colorSum[i];
00070         greenp[i] = G[i]      * 100 / colorSum[i];
00071         bluep[i]  = B[i]* rib * 100 / colorSum[i];
00072     }*/ 
00073  
00074     for (int i=0; i<COLOR_NUM; i++){
00075         colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
00076         redp[i]   = R[i]*0.65 * 100 / colorSum[i];
00077         greenp[i] = G[i]      * 100 / colorSum[i];
00078         bluep[i]  = B[i]*1.3 * 100 / colorSum[i];
00079     }
00080 }
00081 
00082 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
00083 ///////////////////////////////////////////////////////////////////////////////////////////
00084 uint8_t jumpcondition()
00085 {
00086     uint8_t threshold = 0, t[COLOR_NUM] = {0};
00087     
00088     //青から赤に0.5秒以内に反応したらジャンプ
00089     for(int i=0; i<COLOR_NUM; i++){
00090         if(bluep[i] >= B_THR){
00091             color_t[i].reset();
00092             color_t[i].start();
00093             t[i] = 0;
00094         }else if(redp[i] >= R_THR){
00095             t[i] = color_t[i].read_ms();
00096         }else{
00097             t[i] = 0;
00098         }
00099  
00100         if((t[i] <= 500) && (t[i] != 0)){
00101             threshold++;
00102         }
00103     }
00104     
00105     return threshold;
00106 }
00107 /*
00108 void jumping(uint8_t threshold)
00109 {
00110     //超音波でジャンプのタイミング合わせる
00111     if(threshold >= 1){
00112             jump_t.reset();
00113             jump_t.start();
00114             while(ultrasonicVal[0] < 1700){
00115                 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
00116                 air[0] = 1;  air[1] = 0;
00117                 
00118                 if(jump_t.read_ms() > 1000)break;
00119             }
00120             led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
00121             air[0] = 0;  air[1] = 1;
00122             wait(0.5);
00123     }else{
00124             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00125     }
00126 }*/
00127 
00128 uint8_t robostop()
00129 {
00130     if(bluep[1] >= B_THR){
00131         return 1;
00132     }else{
00133         return 0;
00134     }
00135 }
00136 
00137 #define SEND_DATA_NUM       7
00138 #define RECEIVE_DATA_NUM    7
00139 
00140 #define KEYCODE             120
00141 
00142 Serial device(p28,p27);
00143 
00144 static uint8_t SendData0[SEND_DATA_NUM];
00145 
00146 void dev_tx()
00147 {
00148     static uint8_t count = 0;
00149     
00150     SendData0[0] = KEYCODE;
00151 
00152     device.putc(SendData0[count]);
00153      
00154     count++;
00155     
00156     if(count > SEND_DATA_NUM){
00157         count = 0;
00158     }
00159     
00160     led[4] = 1;
00161 }
00162 
00163 
00164 int main() {
00165     //init
00166 
00167     device.baud(9600);
00168     device.printf("START");
00169     device.attach(&dev_tx,Serial::TxIrq);
00170 
00171     //rivisedate();
00172     wait(3);
00173     
00174     
00175         
00176     //air[0] = 0; air[1] = 1;    
00177 
00178     interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない
00179     
00180     //uint8_t button, state=0;
00181     
00182     //init end
00183 
00184     while(1) {
00185         SendData0[1] = jumpcondition();    
00186         
00187         pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00188         if(jumpcondition()>=1){
00189             led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
00190         }else{
00191             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00192         }
00193     }
00194 }