main.cpp

00001 #include "mbed.h"
00002 #include "HMC6352.h"
00003 #include "Ping.h"
00004 #include "common.h"
00005 
00006 #define ALL_IR 11
00007 #define BREAK_TIME_IR 833
00008 #define ERROR_IR 0x9F6 //=2550
00009 #define NOT_F 255
00010 #define BUT_WAIT        0.3 //s
00011 #define ENTER   0
00012 #define EXIT    1
00013 Serial sensor(p13,p14);
00014 Serial pc(USBTX,USBRX);
00015 HMC6352 compass(p9,p10);
00016 Ping PING_F(p5);    /* 超音波距離センサのピンを設定 */
00017 Ping PING_L(p6);
00018 Ping PING_B(p7);
00019 Ping PING_R(p8);
00020 Timer timer_ir;
00021 DigitalIn CalibEnterButton(p15);
00022 
00023 
00024 
00025 DigitalIn ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30,p12};
00026 DigitalOut myled[4] = {LED1,LED2,LED3,LED4};
00027 
00028 extern void micon_tx();
00029 uint8_t compass_data[2] = {0};
00030 uint8_t ping_data[4] = {0};//DENGER: VALUE is N/A  end
00031 uint8_t ir_min = NOT_F, ir_num = NOT_F;//data format
00032 uint8_t ir_main = 0;
00033 const uint8_t flag_case[ALL_IR] = {1,1,1,1,1,1,1,1,1,1,1};// use ir_flag
00034 
00035 unsigned int moving_ave(uint8_t num, unsigned int data){
00036     static unsigned int sum[ALL_IR] = {0};
00037     static unsigned int temp[ALL_IR][10] = {0};
00038     
00039     sum[num] -= temp[num][9];
00040     sum[num] += data;
00041     temp[num][9] = temp[num][8];
00042     temp[num][8] = temp[num][7];
00043     temp[num][7] = temp[num][6];
00044     temp[num][6] = temp[num][5];
00045     temp[num][5] = temp[num][4];
00046     temp[num][4] = temp[num][3];
00047     temp[num][3] = temp[num][2];
00048     temp[num][2] = temp[num][1];
00049     temp[num][1] = temp[num][0];
00050     temp[num][0] = data;
00051     //sum[num]+=modulate[num];
00052     
00053     //return ir_data[num] = sum[num]/100;
00054     return sum[num]/10;//return ir_value
00055 }
00056 
00057 unsigned int ir_function (uint8_t num){
00058     uint8_t flag = 0;
00059     unsigned int memory = 0;
00060     unsigned int data = 0;
00061        
00062     flag = flag_case[num];
00063     timer_ir.start();
00064     if(ir[num]){
00065         while(ir[num]){
00066             if(timer_ir.read_us() >= BREAK_TIME_IR){
00067                 flag = 0;
00068                 break;
00069             }
00070         }
00071     }
00072     timer_ir.stop();
00073     timer_ir.reset();
00074     
00075     if(flag){
00076         timer_ir.start();
00077         while(!ir[num]){
00078             if(timer_ir.read_us() >= BREAK_TIME_IR){
00079                 break;
00080             }
00081         }
00082         memory = timer_ir.read_us();
00083         while(1){
00084             if(!ir[num]){
00085                 data = (timer_ir.read_us() - memory);
00086                 timer_ir.stop();
00087                 timer_ir.reset();
00088                 //wait(0.01);
00089                 return data;
00090             }
00091             if((timer_ir.read_us()-memory)>2550){
00092                 break;
00093             }
00094         }
00095     }
00096     timer_ir.stop();
00097     timer_ir.reset();
00098     
00099     return ERROR_IR;
00100 }
00101 void ir_fun(unsigned int ir_data[]){
00102     uint8_t num;
00103     uint8_t i;//count
00104     unsigned int min;
00105     
00106     min = ERROR_IR;
00107     num = NOT_F;
00108     
00109     for (i = 3;i < (ALL_IR+2) ;i++){
00110         if(i ==10){
00111             if(min > ir_data[0]){
00112                 min = ir_data[0];
00113                 num = 0;
00114             }
00115         }else if(i == 11){
00116             if(min > ir_data[1]){
00117                 min = ir_data[1];
00118                 num = 1;
00119             }
00120         }else if(i == 12){
00121             if(min > ir_data[2]){
00122                 min = ir_data[2];
00123                 num = 2;
00124             }
00125         }else{
00126             if(min > ir_data[i]){
00127             min = ir_data[i];
00128             num = i;
00129             }
00130        }
00131     }
00132     if(num == 2){
00133         if(ir_data[5]<ir_data[1]) num = 3;
00134     }
00135     if(num == 4){
00136         if(ir_data[1]<ir_data[5]) num = 3;
00137     }
00138     
00139     ir_min = min/10;
00140     ir_num = num;
00141 
00142 
00143 }
00144 int main(void){
00145     
00146     int compassdef = 0,compass_A = 0;/*compass_A = original_value,used compass_data*/
00147     uint8_t i = 0;
00148     unsigned int ir_data[ALL_IR] = {0};  //DENGER: VALUE is N/A
00149 
00150     
00151     
00152     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00153     compassdef = (compass.sample() / 10);
00154     sensor.attach(&micon_tx,Serial::TxIrq);//送信空き割り込み設定
00155     sensor.putc(1);                         //送信開始+
00156     
00157     CalibEnterButton.mode(PullUp);
00158         
00159     if(!CalibEnterButton){
00160             myled[3] =1;
00161             compass.setCalibrationMode(ENTER);
00162             while(!CalibEnterButton);
00163             compass.setCalibrationMode(EXIT);
00164             wait(BUT_WAIT);
00165             myled[3] = 0;
00166          }
00167     while(1){
00168         compass_A = ((compass.sample() / 10) + 540 - compassdef) % 360;
00169         if(compass_A >255 ){
00170             compass_data[0] = 255;
00171             compass_data[1] = compass_A - 255 ;
00172         }else{
00173             compass_data[0] = compass_A;
00174             compass_data[1] = 0;
00175         }
00176          /* 超音波発射 */
00177         PING_F.Send();
00178         PING_L.Send();
00179         PING_B.Send();
00180         PING_R.Send();
00181  
00182         wait_ms(30);    // 待つ
00183  
00184         /* 結果から距離を算出 */
00185         ping_data[FRONT] = PING_F.Read_cm() / 2;  // FRONT距離を記録 [cm]
00186         ping_data[LEFT]  = PING_L.Read_cm() / 2;  // LEFT距離を記録 [cm]
00187         ping_data[BACK]  = PING_B.Read_cm() / 2;  // BACK距離を記録 [cm]
00188         ping_data[RIGHT] = PING_R.Read_cm() / 2;  // RIGHT距離を記録 [cm]
00189         
00190             // PING_F.Send();  wait_ms(30);  Ping_F=PING_F.Read_cm()/2;
00191             // PING_R.Send();  wait_ms(30);  Ping_R=PING_R.Read_cm()/2;
00192             // PING_B.Send();  wait_ms(30);  Ping_B=PING_B.Read_cm()/2;
00193             // PING_L.Send();  wait_ms(30);  Ping_L=PING_L.Read_cm()/2;
00194             /* アクセス可能になるまで待機してから、値を代入 */
00195             /*PING_slots.wait();
00196                 Ping_data[0] = Ping_F;
00197                 Ping_data[1] = Ping_R;
00198                 Ping_data[2] = Ping_B;
00199                 Ping_data[3] = Ping_L;
00200             PING_slots.release();
00201             */
00202             //wait(0.1);
00203         
00204 
00205         for( i = 0; i < ALL_IR; i++){/*赤外線データの読み取り*/
00206              ir_data[i] = moving_ave(i,ir_function(i));
00207         }
00208         
00209         ir_fun(ir_data);
00210         ir_main = ir_data[10]/10;
00211         //
00212         //pc.printf("%d\t%d\n",ir_function(8), (moving_ave(8,ir_function(8))));
00213         //pc.printf("%d\n",ir_data[8]);
00214         //pc.printf("compass: %d\n",compass_A);//check finish
00215         //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping_data[0],ping_data[1],ping_data[2],ping_data[3]);
00216         //pc.printf("ir_min:%d\tir_num:%d\n",ir_min,ir_num);
00217         //pc.printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d 9:%d \n",ir_data[0],ir_data[1],ir_data[2],ir_data[3],ir_data[4],ir_data[5],ir_data[6],ir_data[7],ir_data[8],ir_data[9]);
00218 
00219         
00220     }
00221     
00222     
00223     
00224 }