CatPot 2015-2016 / Mbed 2 deprecated Chronograph_Sensor_ver1

あああ

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?

UserRevisionLine numberNew 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 }