あああ
Dependencies: HMC6352 Ping mbed
Diff: main.cpp
- Revision:
- 0:e742eb5a0f3e
diff -r 000000000000 -r e742eb5a0f3e main.cpp --- /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