Diff: main.cpp

Revision:

0:e742eb5a0f3e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 25 08:01:53 2015 +0000
@@ -0,0 +1,224 @@
+#include "mbed.h"
+#include "HMC6352.h"
+#include "Ping.h"
+#include "common.h"
+
+#define ALL_IR 11
+#define BREAK_TIME_IR 833
+#define ERROR_IR 0x9F6 //=2550
+#define NOT_F 255
+#define BUT_WAIT        0.3 //s
+#define ENTER   0
+#define EXIT    1
+Serial sensor(p13,p14);
+Serial pc(USBTX,USBRX);
+HMC6352 compass(p9,p10);
+Ping PING_F(p5);    /* 超音波距離センサのピンを設定 */
+Ping PING_L(p6);
+Ping PING_B(p7);
+Ping PING_R(p8);
+Timer timer_ir;
+DigitalIn CalibEnterButton(p15);
+
+
+
+DigitalIn ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30,p12};
+DigitalOut myled[4] = {LED1,LED2,LED3,LED4};
+
+extern void micon_tx();
+uint8_t compass_data[2] = {0};
+uint8_t ping_data[4] = {0};//DENGER: VALUE is N/A  end
+uint8_t ir_min = NOT_F, ir_num = NOT_F;//data format
+uint8_t ir_main = 0;
+const uint8_t flag_case[ALL_IR] = {1,1,1,1,1,1,1,1,1,1,1};// use ir_flag
+
+unsigned int moving_ave(uint8_t num, unsigned int data){
+    static unsigned int sum[ALL_IR] = {0};
+    static unsigned int temp[ALL_IR][10] = {0};
+    
+    sum[num] -= temp[num][9];
+    sum[num] += data;
+    temp[num][9] = temp[num][8];
+    temp[num][8] = temp[num][7];
+    temp[num][7] = temp[num][6];
+    temp[num][6] = temp[num][5];
+    temp[num][5] = temp[num][4];
+    temp[num][4] = temp[num][3];
+    temp[num][3] = temp[num][2];
+    temp[num][2] = temp[num][1];
+    temp[num][1] = temp[num][0];
+    temp[num][0] = data;
+    //sum[num]+=modulate[num];
+    
+    //return ir_data[num] = sum[num]/100;
+    return sum[num]/10;//return ir_value
+}
+
+unsigned int ir_function (uint8_t num){
+    uint8_t flag = 0;
+    unsigned int memory = 0;
+    unsigned int data = 0;
+       
+    flag = flag_case[num];
+    timer_ir.start();
+    if(ir[num]){
+        while(ir[num]){
+            if(timer_ir.read_us() >= BREAK_TIME_IR){
+                flag = 0;
+                break;
+            }
+        }
+    }
+    timer_ir.stop();
+    timer_ir.reset();
+    
+    if(flag){
+        timer_ir.start();
+        while(!ir[num]){
+            if(timer_ir.read_us() >= BREAK_TIME_IR){
+                break;
+            }
+        }
+        memory = timer_ir.read_us();
+        while(1){
+            if(!ir[num]){
+                data = (timer_ir.read_us() - memory);
+                timer_ir.stop();
+                timer_ir.reset();
+                //wait(0.01);
+                return data;
+            }
+            if((timer_ir.read_us()-memory)>2550){
+                break;
+            }
+        }
+    }
+    timer_ir.stop();
+    timer_ir.reset();
+    
+    return ERROR_IR;
+}
+void ir_fun(unsigned int ir_data[]){
+    uint8_t num;
+    uint8_t i;//count
+    unsigned int min;
+    
+    min = ERROR_IR;
+    num = NOT_F;
+    
+    for (i = 3;i < (ALL_IR+2) ;i++){
+        if(i ==10){
+            if(min > ir_data[0]){
+                min = ir_data[0];
+                num = 0;
+            }
+        }else if(i == 11){
+            if(min > ir_data[1]){
+                min = ir_data[1];
+                num = 1;
+            }
+        }else if(i == 12){
+            if(min > ir_data[2]){
+                min = ir_data[2];
+                num = 2;
+            }
+        }else{
+            if(min > ir_data[i]){
+            min = ir_data[i];
+            num = i;
+            }
+       }
+    }
+    if(num == 2){
+        if(ir_data[5]<ir_data[1]) num = 3;
+    }
+    if(num == 4){
+        if(ir_data[1]<ir_data[5]) num = 3;
+    }
+    
+    ir_min = min/10;
+    ir_num = num;
+
+
+}
+int main(void){
+    
+    int compassdef = 0,compass_A = 0;/*compass_A = original_value,used compass_data*/
+    uint8_t i = 0;
+    unsigned int ir_data[ALL_IR] = {0};  //DENGER: VALUE is N/A
+
+    
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    compassdef = (compass.sample() / 10);
+    sensor.attach(&micon_tx,Serial::TxIrq);//送信空き割り込み設定
+    sensor.putc(1);                         //送信開始+
+    
+    CalibEnterButton.mode(PullUp);
+        
+    if(!CalibEnterButton){
+            myled[3] =1;
+            compass.setCalibrationMode(ENTER);
+            while(!CalibEnterButton);
+            compass.setCalibrationMode(EXIT);
+            wait(BUT_WAIT);
+            myled[3] = 0;
+         }
+    while(1){
+        compass_A = ((compass.sample() / 10) + 540 - compassdef) % 360;
+        if(compass_A >255 ){
+            compass_data[0] = 255;
+            compass_data[1] = compass_A - 255 ;
+        }else{
+            compass_data[0] = compass_A;
+            compass_data[1] = 0;
+        }
+         /* 超音波発射 */
+        PING_F.Send();
+        PING_L.Send();
+        PING_B.Send();
+        PING_R.Send();
+ 
+        wait_ms(30);    // 待つ
+ 
+        /* 結果から距離を算出 */
+        ping_data[FRONT] = PING_F.Read_cm() / 2;  // FRONT距離を記録 [cm]
+        ping_data[LEFT]  = PING_L.Read_cm() / 2;  // LEFT距離を記録 [cm]
+        ping_data[BACK]  = PING_B.Read_cm() / 2;  // BACK距離を記録 [cm]
+        ping_data[RIGHT] = PING_R.Read_cm() / 2;  // RIGHT距離を記録 [cm]
+        
+            // PING_F.Send();  wait_ms(30);  Ping_F=PING_F.Read_cm()/2;
+            // PING_R.Send();  wait_ms(30);  Ping_R=PING_R.Read_cm()/2;
+            // PING_B.Send();  wait_ms(30);  Ping_B=PING_B.Read_cm()/2;
+            // PING_L.Send();  wait_ms(30);  Ping_L=PING_L.Read_cm()/2;
+            /* アクセス可能になるまで待機してから、値を代入 */
+            /*PING_slots.wait();
+                Ping_data[0] = Ping_F;
+                Ping_data[1] = Ping_R;
+                Ping_data[2] = Ping_B;
+                Ping_data[3] = Ping_L;
+            PING_slots.release();
+            */
+            //wait(0.1);
+        
+
+        for( i = 0; i < ALL_IR; i++){/*赤外線データの読み取り*/
+             ir_data[i] = moving_ave(i,ir_function(i));
+        }
+        
+        ir_fun(ir_data);
+        ir_main = ir_data[10]/10;
+        //
+        //pc.printf("%d\t%d\n",ir_function(8), (moving_ave(8,ir_function(8))));
+        //pc.printf("%d\n",ir_data[8]);
+        //pc.printf("compass: %d\n",compass_A);//check finish
+        //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping_data[0],ping_data[1],ping_data[2],ping_data[3]);
+        //pc.printf("ir_min:%d\tir_num:%d\n",ir_min,ir_num);
+        //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]);
+
+        
+    }
+    
+    
+    
+}
\ No newline at end of file