あああ
Dependencies: HMC6352 Ping mbed
main.cpp@0:e742eb5a0f3e, 2015-02-25 (annotated)
- Committer:
- ryuna
- Date:
- Wed Feb 25 08:01:53 2015 +0000
- Revision:
- 0:e742eb5a0f3e
?????????sensor??
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryuna | 0:e742eb5a0f3e | 1 | #include "mbed.h" |
ryuna | 0:e742eb5a0f3e | 2 | #include "HMC6352.h" |
ryuna | 0:e742eb5a0f3e | 3 | #include "Ping.h" |
ryuna | 0:e742eb5a0f3e | 4 | #include "common.h" |
ryuna | 0:e742eb5a0f3e | 5 | |
ryuna | 0:e742eb5a0f3e | 6 | #define ALL_IR 11 |
ryuna | 0:e742eb5a0f3e | 7 | #define BREAK_TIME_IR 833 |
ryuna | 0:e742eb5a0f3e | 8 | #define ERROR_IR 0x9F6 //=2550 |
ryuna | 0:e742eb5a0f3e | 9 | #define NOT_F 255 |
ryuna | 0:e742eb5a0f3e | 10 | #define BUT_WAIT 0.3 //s |
ryuna | 0:e742eb5a0f3e | 11 | #define ENTER 0 |
ryuna | 0:e742eb5a0f3e | 12 | #define EXIT 1 |
ryuna | 0:e742eb5a0f3e | 13 | Serial sensor(p13,p14); |
ryuna | 0:e742eb5a0f3e | 14 | Serial pc(USBTX,USBRX); |
ryuna | 0:e742eb5a0f3e | 15 | HMC6352 compass(p9,p10); |
ryuna | 0:e742eb5a0f3e | 16 | Ping PING_F(p5); /* 超音波距離センサのピンを設定 */ |
ryuna | 0:e742eb5a0f3e | 17 | Ping PING_L(p6); |
ryuna | 0:e742eb5a0f3e | 18 | Ping PING_B(p7); |
ryuna | 0:e742eb5a0f3e | 19 | Ping PING_R(p8); |
ryuna | 0:e742eb5a0f3e | 20 | Timer timer_ir; |
ryuna | 0:e742eb5a0f3e | 21 | DigitalIn CalibEnterButton(p15); |
ryuna | 0:e742eb5a0f3e | 22 | |
ryuna | 0:e742eb5a0f3e | 23 | |
ryuna | 0:e742eb5a0f3e | 24 | |
ryuna | 0:e742eb5a0f3e | 25 | DigitalIn ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30,p12}; |
ryuna | 0:e742eb5a0f3e | 26 | DigitalOut myled[4] = {LED1,LED2,LED3,LED4}; |
ryuna | 0:e742eb5a0f3e | 27 | |
ryuna | 0:e742eb5a0f3e | 28 | extern void micon_tx(); |
ryuna | 0:e742eb5a0f3e | 29 | uint8_t compass_data[2] = {0}; |
ryuna | 0:e742eb5a0f3e | 30 | uint8_t ping_data[4] = {0};//DENGER: VALUE is N/A end |
ryuna | 0:e742eb5a0f3e | 31 | uint8_t ir_min = NOT_F, ir_num = NOT_F;//data format |
ryuna | 0:e742eb5a0f3e | 32 | uint8_t ir_main = 0; |
ryuna | 0:e742eb5a0f3e | 33 | const uint8_t flag_case[ALL_IR] = {1,1,1,1,1,1,1,1,1,1,1};// use ir_flag |
ryuna | 0:e742eb5a0f3e | 34 | |
ryuna | 0:e742eb5a0f3e | 35 | unsigned int moving_ave(uint8_t num, unsigned int data){ |
ryuna | 0:e742eb5a0f3e | 36 | static unsigned int sum[ALL_IR] = {0}; |
ryuna | 0:e742eb5a0f3e | 37 | static unsigned int temp[ALL_IR][10] = {0}; |
ryuna | 0:e742eb5a0f3e | 38 | |
ryuna | 0:e742eb5a0f3e | 39 | sum[num] -= temp[num][9]; |
ryuna | 0:e742eb5a0f3e | 40 | sum[num] += data; |
ryuna | 0:e742eb5a0f3e | 41 | temp[num][9] = temp[num][8]; |
ryuna | 0:e742eb5a0f3e | 42 | temp[num][8] = temp[num][7]; |
ryuna | 0:e742eb5a0f3e | 43 | temp[num][7] = temp[num][6]; |
ryuna | 0:e742eb5a0f3e | 44 | temp[num][6] = temp[num][5]; |
ryuna | 0:e742eb5a0f3e | 45 | temp[num][5] = temp[num][4]; |
ryuna | 0:e742eb5a0f3e | 46 | temp[num][4] = temp[num][3]; |
ryuna | 0:e742eb5a0f3e | 47 | temp[num][3] = temp[num][2]; |
ryuna | 0:e742eb5a0f3e | 48 | temp[num][2] = temp[num][1]; |
ryuna | 0:e742eb5a0f3e | 49 | temp[num][1] = temp[num][0]; |
ryuna | 0:e742eb5a0f3e | 50 | temp[num][0] = data; |
ryuna | 0:e742eb5a0f3e | 51 | //sum[num]+=modulate[num]; |
ryuna | 0:e742eb5a0f3e | 52 | |
ryuna | 0:e742eb5a0f3e | 53 | //return ir_data[num] = sum[num]/100; |
ryuna | 0:e742eb5a0f3e | 54 | return sum[num]/10;//return ir_value |
ryuna | 0:e742eb5a0f3e | 55 | } |
ryuna | 0:e742eb5a0f3e | 56 | |
ryuna | 0:e742eb5a0f3e | 57 | unsigned int ir_function (uint8_t num){ |
ryuna | 0:e742eb5a0f3e | 58 | uint8_t flag = 0; |
ryuna | 0:e742eb5a0f3e | 59 | unsigned int memory = 0; |
ryuna | 0:e742eb5a0f3e | 60 | unsigned int data = 0; |
ryuna | 0:e742eb5a0f3e | 61 | |
ryuna | 0:e742eb5a0f3e | 62 | flag = flag_case[num]; |
ryuna | 0:e742eb5a0f3e | 63 | timer_ir.start(); |
ryuna | 0:e742eb5a0f3e | 64 | if(ir[num]){ |
ryuna | 0:e742eb5a0f3e | 65 | while(ir[num]){ |
ryuna | 0:e742eb5a0f3e | 66 | if(timer_ir.read_us() >= BREAK_TIME_IR){ |
ryuna | 0:e742eb5a0f3e | 67 | flag = 0; |
ryuna | 0:e742eb5a0f3e | 68 | break; |
ryuna | 0:e742eb5a0f3e | 69 | } |
ryuna | 0:e742eb5a0f3e | 70 | } |
ryuna | 0:e742eb5a0f3e | 71 | } |
ryuna | 0:e742eb5a0f3e | 72 | timer_ir.stop(); |
ryuna | 0:e742eb5a0f3e | 73 | timer_ir.reset(); |
ryuna | 0:e742eb5a0f3e | 74 | |
ryuna | 0:e742eb5a0f3e | 75 | if(flag){ |
ryuna | 0:e742eb5a0f3e | 76 | timer_ir.start(); |
ryuna | 0:e742eb5a0f3e | 77 | while(!ir[num]){ |
ryuna | 0:e742eb5a0f3e | 78 | if(timer_ir.read_us() >= BREAK_TIME_IR){ |
ryuna | 0:e742eb5a0f3e | 79 | break; |
ryuna | 0:e742eb5a0f3e | 80 | } |
ryuna | 0:e742eb5a0f3e | 81 | } |
ryuna | 0:e742eb5a0f3e | 82 | memory = timer_ir.read_us(); |
ryuna | 0:e742eb5a0f3e | 83 | while(1){ |
ryuna | 0:e742eb5a0f3e | 84 | if(!ir[num]){ |
ryuna | 0:e742eb5a0f3e | 85 | data = (timer_ir.read_us() - memory); |
ryuna | 0:e742eb5a0f3e | 86 | timer_ir.stop(); |
ryuna | 0:e742eb5a0f3e | 87 | timer_ir.reset(); |
ryuna | 0:e742eb5a0f3e | 88 | //wait(0.01); |
ryuna | 0:e742eb5a0f3e | 89 | return data; |
ryuna | 0:e742eb5a0f3e | 90 | } |
ryuna | 0:e742eb5a0f3e | 91 | if((timer_ir.read_us()-memory)>2550){ |
ryuna | 0:e742eb5a0f3e | 92 | break; |
ryuna | 0:e742eb5a0f3e | 93 | } |
ryuna | 0:e742eb5a0f3e | 94 | } |
ryuna | 0:e742eb5a0f3e | 95 | } |
ryuna | 0:e742eb5a0f3e | 96 | timer_ir.stop(); |
ryuna | 0:e742eb5a0f3e | 97 | timer_ir.reset(); |
ryuna | 0:e742eb5a0f3e | 98 | |
ryuna | 0:e742eb5a0f3e | 99 | return ERROR_IR; |
ryuna | 0:e742eb5a0f3e | 100 | } |
ryuna | 0:e742eb5a0f3e | 101 | void ir_fun(unsigned int ir_data[]){ |
ryuna | 0:e742eb5a0f3e | 102 | uint8_t num; |
ryuna | 0:e742eb5a0f3e | 103 | uint8_t i;//count |
ryuna | 0:e742eb5a0f3e | 104 | unsigned int min; |
ryuna | 0:e742eb5a0f3e | 105 | |
ryuna | 0:e742eb5a0f3e | 106 | min = ERROR_IR; |
ryuna | 0:e742eb5a0f3e | 107 | num = NOT_F; |
ryuna | 0:e742eb5a0f3e | 108 | |
ryuna | 0:e742eb5a0f3e | 109 | for (i = 3;i < (ALL_IR+2) ;i++){ |
ryuna | 0:e742eb5a0f3e | 110 | if(i ==10){ |
ryuna | 0:e742eb5a0f3e | 111 | if(min > ir_data[0]){ |
ryuna | 0:e742eb5a0f3e | 112 | min = ir_data[0]; |
ryuna | 0:e742eb5a0f3e | 113 | num = 0; |
ryuna | 0:e742eb5a0f3e | 114 | } |
ryuna | 0:e742eb5a0f3e | 115 | }else if(i == 11){ |
ryuna | 0:e742eb5a0f3e | 116 | if(min > ir_data[1]){ |
ryuna | 0:e742eb5a0f3e | 117 | min = ir_data[1]; |
ryuna | 0:e742eb5a0f3e | 118 | num = 1; |
ryuna | 0:e742eb5a0f3e | 119 | } |
ryuna | 0:e742eb5a0f3e | 120 | }else if(i == 12){ |
ryuna | 0:e742eb5a0f3e | 121 | if(min > ir_data[2]){ |
ryuna | 0:e742eb5a0f3e | 122 | min = ir_data[2]; |
ryuna | 0:e742eb5a0f3e | 123 | num = 2; |
ryuna | 0:e742eb5a0f3e | 124 | } |
ryuna | 0:e742eb5a0f3e | 125 | }else{ |
ryuna | 0:e742eb5a0f3e | 126 | if(min > ir_data[i]){ |
ryuna | 0:e742eb5a0f3e | 127 | min = ir_data[i]; |
ryuna | 0:e742eb5a0f3e | 128 | num = i; |
ryuna | 0:e742eb5a0f3e | 129 | } |
ryuna | 0:e742eb5a0f3e | 130 | } |
ryuna | 0:e742eb5a0f3e | 131 | } |
ryuna | 0:e742eb5a0f3e | 132 | if(num == 2){ |
ryuna | 0:e742eb5a0f3e | 133 | if(ir_data[5]<ir_data[1]) num = 3; |
ryuna | 0:e742eb5a0f3e | 134 | } |
ryuna | 0:e742eb5a0f3e | 135 | if(num == 4){ |
ryuna | 0:e742eb5a0f3e | 136 | if(ir_data[1]<ir_data[5]) num = 3; |
ryuna | 0:e742eb5a0f3e | 137 | } |
ryuna | 0:e742eb5a0f3e | 138 | |
ryuna | 0:e742eb5a0f3e | 139 | ir_min = min/10; |
ryuna | 0:e742eb5a0f3e | 140 | ir_num = num; |
ryuna | 0:e742eb5a0f3e | 141 | |
ryuna | 0:e742eb5a0f3e | 142 | |
ryuna | 0:e742eb5a0f3e | 143 | } |
ryuna | 0:e742eb5a0f3e | 144 | int main(void){ |
ryuna | 0:e742eb5a0f3e | 145 | |
ryuna | 0:e742eb5a0f3e | 146 | int compassdef = 0,compass_A = 0;/*compass_A = original_value,used compass_data*/ |
ryuna | 0:e742eb5a0f3e | 147 | uint8_t i = 0; |
ryuna | 0:e742eb5a0f3e | 148 | unsigned int ir_data[ALL_IR] = {0}; //DENGER: VALUE is N/A |
ryuna | 0:e742eb5a0f3e | 149 | |
ryuna | 0:e742eb5a0f3e | 150 | |
ryuna | 0:e742eb5a0f3e | 151 | |
ryuna | 0:e742eb5a0f3e | 152 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
ryuna | 0:e742eb5a0f3e | 153 | compassdef = (compass.sample() / 10); |
ryuna | 0:e742eb5a0f3e | 154 | sensor.attach(&micon_tx,Serial::TxIrq);//送信空き割り込み設定 |
ryuna | 0:e742eb5a0f3e | 155 | sensor.putc(1); //送信開始+ |
ryuna | 0:e742eb5a0f3e | 156 | |
ryuna | 0:e742eb5a0f3e | 157 | CalibEnterButton.mode(PullUp); |
ryuna | 0:e742eb5a0f3e | 158 | |
ryuna | 0:e742eb5a0f3e | 159 | if(!CalibEnterButton){ |
ryuna | 0:e742eb5a0f3e | 160 | myled[3] =1; |
ryuna | 0:e742eb5a0f3e | 161 | compass.setCalibrationMode(ENTER); |
ryuna | 0:e742eb5a0f3e | 162 | while(!CalibEnterButton); |
ryuna | 0:e742eb5a0f3e | 163 | compass.setCalibrationMode(EXIT); |
ryuna | 0:e742eb5a0f3e | 164 | wait(BUT_WAIT); |
ryuna | 0:e742eb5a0f3e | 165 | myled[3] = 0; |
ryuna | 0:e742eb5a0f3e | 166 | } |
ryuna | 0:e742eb5a0f3e | 167 | while(1){ |
ryuna | 0:e742eb5a0f3e | 168 | compass_A = ((compass.sample() / 10) + 540 - compassdef) % 360; |
ryuna | 0:e742eb5a0f3e | 169 | if(compass_A >255 ){ |
ryuna | 0:e742eb5a0f3e | 170 | compass_data[0] = 255; |
ryuna | 0:e742eb5a0f3e | 171 | compass_data[1] = compass_A - 255 ; |
ryuna | 0:e742eb5a0f3e | 172 | }else{ |
ryuna | 0:e742eb5a0f3e | 173 | compass_data[0] = compass_A; |
ryuna | 0:e742eb5a0f3e | 174 | compass_data[1] = 0; |
ryuna | 0:e742eb5a0f3e | 175 | } |
ryuna | 0:e742eb5a0f3e | 176 | /* 超音波発射 */ |
ryuna | 0:e742eb5a0f3e | 177 | PING_F.Send(); |
ryuna | 0:e742eb5a0f3e | 178 | PING_L.Send(); |
ryuna | 0:e742eb5a0f3e | 179 | PING_B.Send(); |
ryuna | 0:e742eb5a0f3e | 180 | PING_R.Send(); |
ryuna | 0:e742eb5a0f3e | 181 | |
ryuna | 0:e742eb5a0f3e | 182 | wait_ms(30); // 待つ |
ryuna | 0:e742eb5a0f3e | 183 | |
ryuna | 0:e742eb5a0f3e | 184 | /* 結果から距離を算出 */ |
ryuna | 0:e742eb5a0f3e | 185 | ping_data[FRONT] = PING_F.Read_cm() / 2; // FRONT距離を記録 [cm] |
ryuna | 0:e742eb5a0f3e | 186 | ping_data[LEFT] = PING_L.Read_cm() / 2; // LEFT距離を記録 [cm] |
ryuna | 0:e742eb5a0f3e | 187 | ping_data[BACK] = PING_B.Read_cm() / 2; // BACK距離を記録 [cm] |
ryuna | 0:e742eb5a0f3e | 188 | ping_data[RIGHT] = PING_R.Read_cm() / 2; // RIGHT距離を記録 [cm] |
ryuna | 0:e742eb5a0f3e | 189 | |
ryuna | 0:e742eb5a0f3e | 190 | // PING_F.Send(); wait_ms(30); Ping_F=PING_F.Read_cm()/2; |
ryuna | 0:e742eb5a0f3e | 191 | // PING_R.Send(); wait_ms(30); Ping_R=PING_R.Read_cm()/2; |
ryuna | 0:e742eb5a0f3e | 192 | // PING_B.Send(); wait_ms(30); Ping_B=PING_B.Read_cm()/2; |
ryuna | 0:e742eb5a0f3e | 193 | // PING_L.Send(); wait_ms(30); Ping_L=PING_L.Read_cm()/2; |
ryuna | 0:e742eb5a0f3e | 194 | /* アクセス可能になるまで待機してから、値を代入 */ |
ryuna | 0:e742eb5a0f3e | 195 | /*PING_slots.wait(); |
ryuna | 0:e742eb5a0f3e | 196 | Ping_data[0] = Ping_F; |
ryuna | 0:e742eb5a0f3e | 197 | Ping_data[1] = Ping_R; |
ryuna | 0:e742eb5a0f3e | 198 | Ping_data[2] = Ping_B; |
ryuna | 0:e742eb5a0f3e | 199 | Ping_data[3] = Ping_L; |
ryuna | 0:e742eb5a0f3e | 200 | PING_slots.release(); |
ryuna | 0:e742eb5a0f3e | 201 | */ |
ryuna | 0:e742eb5a0f3e | 202 | //wait(0.1); |
ryuna | 0:e742eb5a0f3e | 203 | |
ryuna | 0:e742eb5a0f3e | 204 | |
ryuna | 0:e742eb5a0f3e | 205 | for( i = 0; i < ALL_IR; i++){/*赤外線データの読み取り*/ |
ryuna | 0:e742eb5a0f3e | 206 | ir_data[i] = moving_ave(i,ir_function(i)); |
ryuna | 0:e742eb5a0f3e | 207 | } |
ryuna | 0:e742eb5a0f3e | 208 | |
ryuna | 0:e742eb5a0f3e | 209 | ir_fun(ir_data); |
ryuna | 0:e742eb5a0f3e | 210 | ir_main = ir_data[10]/10; |
ryuna | 0:e742eb5a0f3e | 211 | // |
ryuna | 0:e742eb5a0f3e | 212 | //pc.printf("%d\t%d\n",ir_function(8), (moving_ave(8,ir_function(8)))); |
ryuna | 0:e742eb5a0f3e | 213 | //pc.printf("%d\n",ir_data[8]); |
ryuna | 0:e742eb5a0f3e | 214 | //pc.printf("compass: %d\n",compass_A);//check finish |
ryuna | 0:e742eb5a0f3e | 215 | //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping_data[0],ping_data[1],ping_data[2],ping_data[3]); |
ryuna | 0:e742eb5a0f3e | 216 | //pc.printf("ir_min:%d\tir_num:%d\n",ir_min,ir_num); |
ryuna | 0:e742eb5a0f3e | 217 | //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]); |
ryuna | 0:e742eb5a0f3e | 218 | |
ryuna | 0:e742eb5a0f3e | 219 | |
ryuna | 0:e742eb5a0f3e | 220 | } |
ryuna | 0:e742eb5a0f3e | 221 | |
ryuna | 0:e742eb5a0f3e | 222 | |
ryuna | 0:e742eb5a0f3e | 223 | |
ryuna | 0:e742eb5a0f3e | 224 | } |