Hajime Muraki
/
program_SensorBlock
sensor
Fork of program_SensorBlock by
main.cpp@8:f547b66f3c43, 2014-03-12 (annotated)
- Committer:
- com3
- Date:
- Wed Mar 12 04:47:18 2014 +0000
- Revision:
- 8:f547b66f3c43
- Parent:
- 6:97015325107b
sensor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ryuna | 0:010fe1eae7d2 | 1 | #include "mbed.h" |
ryuna | 1:3ecb80796a7f | 2 | #include "HMC6352.h" |
ryuna | 0:010fe1eae7d2 | 3 | /*PING DEFINE */ |
com3 | 6:97015325107b | 4 | #define ERROR_PING 0xFF |
ryuna | 0:010fe1eae7d2 | 5 | #define ALL_PING 4 |
ryuna | 0:010fe1eae7d2 | 6 | /*IR DEFINE*/ |
ryuna | 0:010fe1eae7d2 | 7 | #define BREAKPT_IR 833 |
ryuna | 0:010fe1eae7d2 | 8 | #define ERROR_IR 0xFFF |
com3 | 6:97015325107b | 9 | #define ALL_IR 12 |
com3 | 6:97015325107b | 10 | #define NOTBALL 3 |
com3 | 6:97015325107b | 11 | #define NO_BALL 45 |
com3 | 6:97015325107b | 12 | #define ERR 7 |
ryuna | 0:010fe1eae7d2 | 13 | //#define OVERLINE_IR 46 |
ryuna | 0:010fe1eae7d2 | 14 | /*LINE DEFINE*/ |
com3 | 6:97015325107b | 15 | #define JUDGEVOL_LINE 0.1 |
ryuna | 0:010fe1eae7d2 | 16 | #define ALL_LINE 4 |
ryuna | 0:010fe1eae7d2 | 17 | //OTHERS |
ryuna | 0:010fe1eae7d2 | 18 | #define ALL_KIND 3 //kind of sensors |
com3 | 6:97015325107b | 19 | #define MAX_NUM 12 //sensors most number |
ryuna | 1:3ecb80796a7f | 20 | extern void micon_tx(); |
ryuna | 0:010fe1eae7d2 | 21 | /* |
ryuna | 1:3ecb80796a7f | 22 | Ping , IrSensor , LineSensor ,compass |
ryuna | 0:010fe1eae7d2 | 23 | */ |
com3 | 6:97015325107b | 24 | //LINE sensor is float value |
com3 | 6:97015325107b | 25 | |
com3 | 6:97015325107b | 26 | BusOut myled(LED1, LED2, LED3, LED4); |
com3 | 6:97015325107b | 27 | |
ryuna | 1:3ecb80796a7f | 28 | Serial sensor(p13,p14); |
ryuna | 0:010fe1eae7d2 | 29 | Serial pc(USBTX,USBRX); |
ryuna | 1:3ecb80796a7f | 30 | HMC6352 compass(p9, p10); |
com3 | 6:97015325107b | 31 | |
ryuna | 0:010fe1eae7d2 | 32 | Timer time_ping; |
ryuna | 0:010fe1eae7d2 | 33 | Timer time_ir; |
com3 | 6:97015325107b | 34 | |
ryuna | 0:010fe1eae7d2 | 35 | PinName num_ping[ALL_PING] = {p5,p6,p7,p8}; |
com3 | 6:97015325107b | 36 | PinName num_ir[ALL_IR] = {p29,p21,p22,p23,p24,p25,p26,p27,p30,p29,p12,p15}; |
ryuna | 0:010fe1eae7d2 | 37 | PinName num_line[ALL_LINE] = {p17,p18,p19,p20}; |
com3 | 6:97015325107b | 38 | |
ryuna | 0:010fe1eae7d2 | 39 | enum sensors{Ping,Ir,Line}; |
ryuna | 0:010fe1eae7d2 | 40 | volatile unsigned int value_ping[ALL_PING] = {0}; |
ryuna | 0:010fe1eae7d2 | 41 | volatile unsigned int value_ir[ALL_IR] = {0}; |
com3 | 6:97015325107b | 42 | volatile int hosei_ir[ALL_IR] = {-1,1,0,0,0,0,0,1,-2,-1,0,0}; |
ryuna | 1:3ecb80796a7f | 43 | volatile unsigned int value_ir_min = 0; |
ryuna | 1:3ecb80796a7f | 44 | volatile unsigned int ir_min_num = 0; |
ryuna | 1:3ecb80796a7f | 45 | volatile unsigned int value_line[ALL_LINE] = {0}; |
ryuna | 3:7de2c50339bd | 46 | volatile unsigned int value_compass0 = 0; |
ryuna | 3:7de2c50339bd | 47 | volatile unsigned int value_compass[2] = {0}; |
com3 | 6:97015325107b | 48 | |
com3 | 6:97015325107b | 49 | |
ryuna | 1:3ecb80796a7f | 50 | /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){ |
ryuna | 0:010fe1eae7d2 | 51 | static unsigned int sum[ALL_KIND][MAX_NUM] = {0}; |
ryuna | 0:010fe1eae7d2 | 52 | static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0}; |
ryuna | 0:010fe1eae7d2 | 53 | //static unsigned int ave[MAX_NUM] = {0}; |
ryuna | 0:010fe1eae7d2 | 54 | sum[kind][num] -= temp[kind][num][9]; |
ryuna | 0:010fe1eae7d2 | 55 | sum[kind][num] += data; |
ryuna | 0:010fe1eae7d2 | 56 | temp[kind][num][9] = temp[kind][num][8]; |
ryuna | 0:010fe1eae7d2 | 57 | temp[kind][num][8] = temp[kind][num][7]; |
ryuna | 0:010fe1eae7d2 | 58 | temp[kind][num][7] = temp[kind][num][6]; |
ryuna | 0:010fe1eae7d2 | 59 | temp[kind][num][6] = temp[kind][num][5]; |
ryuna | 0:010fe1eae7d2 | 60 | temp[kind][num][5] = temp[kind][num][4]; |
ryuna | 0:010fe1eae7d2 | 61 | temp[kind][num][4] = temp[kind][num][3]; |
ryuna | 0:010fe1eae7d2 | 62 | temp[kind][num][3] = temp[kind][num][2]; |
ryuna | 0:010fe1eae7d2 | 63 | temp[kind][num][2] = temp[kind][num][1]; |
ryuna | 0:010fe1eae7d2 | 64 | temp[kind][num][1] = temp[kind][num][0]; |
ryuna | 0:010fe1eae7d2 | 65 | temp[kind][num][0] = data; |
ryuna | 0:010fe1eae7d2 | 66 | //ave[kind][num] = sum[kind][num]/100; |
ryuna | 0:010fe1eae7d2 | 67 | |
ryuna | 0:010fe1eae7d2 | 68 | switch (kind) { |
ryuna | 0:010fe1eae7d2 | 69 | case Ping: |
ryuna | 0:010fe1eae7d2 | 70 | value_ping[num] = sum[kind][num]/10; |
ryuna | 0:010fe1eae7d2 | 71 | break; |
ryuna | 0:010fe1eae7d2 | 72 | case Ir: |
ryuna | 0:010fe1eae7d2 | 73 | value_ir[num] = sum[kind][num]/100; |
ryuna | 0:010fe1eae7d2 | 74 | break; |
ryuna | 0:010fe1eae7d2 | 75 | case Line: |
ryuna | 1:3ecb80796a7f | 76 | if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){ |
ryuna | 1:3ecb80796a7f | 77 | value_line[num] = 1;//white |
ryuna | 1:3ecb80796a7f | 78 | }else{ |
ryuna | 1:3ecb80796a7f | 79 | value_line[num] = 0;//green |
ryuna | 1:3ecb80796a7f | 80 | } |
com3 | 6:97015325107b | 81 | //pc.printf("%f %f %f %f\n", sum[kind][0]/10000.0, sum[kind][1]/10000.0, sum[kind][2]/10000.0, sum[kind][3]/10000.0); |
ryuna | 0:010fe1eae7d2 | 82 | break; |
ryuna | 0:010fe1eae7d2 | 83 | default : |
ryuna | 0:010fe1eae7d2 | 84 | break; |
ryuna | 0:010fe1eae7d2 | 85 | } |
com3 | 6:97015325107b | 86 | |
ryuna | 1:3ecb80796a7f | 87 | //return sum[kind][num];//return no config sum |
ryuna | 0:010fe1eae7d2 | 88 | |
ryuna | 0:010fe1eae7d2 | 89 | } |
ryuna | 0:010fe1eae7d2 | 90 | unsigned int ping_function (int num){ |
ryuna | 0:010fe1eae7d2 | 91 | DigitalInOut sensor_ping(num_ping[num]); |
ryuna | 0:010fe1eae7d2 | 92 | unsigned int memory = 0;//return value |
ryuna | 0:010fe1eae7d2 | 93 | time_ping.reset(); |
ryuna | 0:010fe1eae7d2 | 94 | /*make a pulse */ |
ryuna | 0:010fe1eae7d2 | 95 | sensor_ping.output(); |
ryuna | 0:010fe1eae7d2 | 96 | sensor_ping = 1; |
ryuna | 0:010fe1eae7d2 | 97 | wait_us(5); |
ryuna | 0:010fe1eae7d2 | 98 | sensor_ping = 0; |
ryuna | 0:010fe1eae7d2 | 99 | sensor_ping.input(); |
ryuna | 0:010fe1eae7d2 | 100 | /*finish,and start timer*/ |
ryuna | 0:010fe1eae7d2 | 101 | time_ping.start(); |
ryuna | 0:010fe1eae7d2 | 102 | while(!sensor_ping){ |
ryuna | 0:010fe1eae7d2 | 103 | if(time_ping.read_ms() > 1){ |
ryuna | 0:010fe1eae7d2 | 104 | time_ping.stop(); |
ryuna | 0:010fe1eae7d2 | 105 | return ERROR_PING; |
ryuna | 0:010fe1eae7d2 | 106 | } |
ryuna | 0:010fe1eae7d2 | 107 | } |
ryuna | 0:010fe1eae7d2 | 108 | time_ping.reset(); |
ryuna | 0:010fe1eae7d2 | 109 | while(sensor_ping){ |
com3 | 6:97015325107b | 110 | if(time_ping.read_ms() >18.5){//18.5 |
ryuna | 0:010fe1eae7d2 | 111 | time_ping.stop(); |
ryuna | 0:010fe1eae7d2 | 112 | return ERROR_PING; |
ryuna | 0:010fe1eae7d2 | 113 | } |
ryuna | 0:010fe1eae7d2 | 114 | } |
ryuna | 0:010fe1eae7d2 | 115 | memory = time_ping.read_us(); |
com3 | 6:97015325107b | 116 | memory >>= 6; |
ryuna | 0:010fe1eae7d2 | 117 | time_ping.stop(); |
ryuna | 0:010fe1eae7d2 | 118 | |
ryuna | 0:010fe1eae7d2 | 119 | return memory; |
ryuna | 0:010fe1eae7d2 | 120 | } |
ryuna | 0:010fe1eae7d2 | 121 | unsigned int ir_function (int num){ |
ryuna | 0:010fe1eae7d2 | 122 | DigitalIn sensor_ir(num_ir[num]); |
ryuna | 0:010fe1eae7d2 | 123 | int flag = 0; |
ryuna | 0:010fe1eae7d2 | 124 | unsigned int memory = 0; |
ryuna | 0:010fe1eae7d2 | 125 | unsigned int temp = 0; |
ryuna | 0:010fe1eae7d2 | 126 | flag = 1; |
ryuna | 0:010fe1eae7d2 | 127 | time_ir.start(); |
ryuna | 0:010fe1eae7d2 | 128 | if((sensor_ir)){ |
ryuna | 0:010fe1eae7d2 | 129 | while((sensor_ir)){ |
ryuna | 0:010fe1eae7d2 | 130 | if(time_ir.read_us() >= BREAKPT_IR){ |
ryuna | 0:010fe1eae7d2 | 131 | flag = 0; |
ryuna | 0:010fe1eae7d2 | 132 | break; |
ryuna | 0:010fe1eae7d2 | 133 | } |
ryuna | 0:010fe1eae7d2 | 134 | } |
ryuna | 0:010fe1eae7d2 | 135 | } |
ryuna | 0:010fe1eae7d2 | 136 | time_ir.stop(); |
ryuna | 0:010fe1eae7d2 | 137 | time_ir.reset(); |
ryuna | 0:010fe1eae7d2 | 138 | if(flag){ |
ryuna | 0:010fe1eae7d2 | 139 | time_ir.start(); |
ryuna | 0:010fe1eae7d2 | 140 | while(!sensor_ir){//! |
ryuna | 0:010fe1eae7d2 | 141 | if(time_ir.read_us() >= BREAKPT_IR){ |
ryuna | 0:010fe1eae7d2 | 142 | break; |
ryuna | 0:010fe1eae7d2 | 143 | } |
ryuna | 0:010fe1eae7d2 | 144 | } |
ryuna | 0:010fe1eae7d2 | 145 | memory = time_ir.read_us(); |
com3 | 6:97015325107b | 146 | while(sensor_ir){ |
com3 | 6:97015325107b | 147 | if((time_ir.read_us() - memory) >= BREAKPT_IR){ |
com3 | 6:97015325107b | 148 | break; |
com3 | 6:97015325107b | 149 | } |
com3 | 6:97015325107b | 150 | } |
com3 | 6:97015325107b | 151 | temp = (time_ir.read_us() - memory); |
com3 | 6:97015325107b | 152 | time_ir.stop(); |
com3 | 6:97015325107b | 153 | time_ir.reset(); |
com3 | 6:97015325107b | 154 | //wait(0.01); |
com3 | 6:97015325107b | 155 | return temp; |
com3 | 6:97015325107b | 156 | /*while(1){ |
ryuna | 0:010fe1eae7d2 | 157 | if(!sensor_ir){ |
ryuna | 0:010fe1eae7d2 | 158 | temp = (time_ir.read_us() - memory); |
ryuna | 0:010fe1eae7d2 | 159 | time_ir.stop(); |
ryuna | 0:010fe1eae7d2 | 160 | time_ir.reset(); |
ryuna | 1:3ecb80796a7f | 161 | //wait(0.01); |
ryuna | 0:010fe1eae7d2 | 162 | return temp; |
ryuna | 0:010fe1eae7d2 | 163 | } |
com3 | 6:97015325107b | 164 | }*/ |
ryuna | 0:010fe1eae7d2 | 165 | }else{//not found |
ryuna | 0:010fe1eae7d2 | 166 | } |
ryuna | 0:010fe1eae7d2 | 167 | time_ir.stop(); |
ryuna | 0:010fe1eae7d2 | 168 | time_ir.reset(); |
ryuna | 0:010fe1eae7d2 | 169 | return ERROR_IR; |
ryuna | 0:010fe1eae7d2 | 170 | } |
ryuna | 0:010fe1eae7d2 | 171 | int line_function(int num){ |
ryuna | 0:010fe1eae7d2 | 172 | float memory = 0; |
ryuna | 0:010fe1eae7d2 | 173 | AnalogIn sensor_line(num_line[num]); |
ryuna | 0:010fe1eae7d2 | 174 | memory = sensor_line; |
ryuna | 0:010fe1eae7d2 | 175 | return (int)(memory*1000); |
com3 | 6:97015325107b | 176 | |
ryuna | 0:010fe1eae7d2 | 177 | } |
ryuna | 1:3ecb80796a7f | 178 | void ir_min_fun(){ |
ryuna | 1:3ecb80796a7f | 179 | static unsigned int min; |
com3 | 6:97015325107b | 180 | static int i, num = 0, sum = 0; |
com3 | 6:97015325107b | 181 | sum = 0; |
ryuna | 1:3ecb80796a7f | 182 | min = value_ir[0]; |
com3 | 6:97015325107b | 183 | for (i = 0;i <= ALL_IR - NOTBALL;i++){ |
com3 | 6:97015325107b | 184 | value_ir[i] += hosei_ir[i]; |
com3 | 6:97015325107b | 185 | sum += value_ir[i]; |
com3 | 6:97015325107b | 186 | if(min > value_ir[i]){ |
ryuna | 1:3ecb80796a7f | 187 | min = value_ir[i]; |
ryuna | 1:3ecb80796a7f | 188 | num = i; |
com3 | 6:97015325107b | 189 | } |
ryuna | 1:3ecb80796a7f | 190 | } |
ryuna | 1:3ecb80796a7f | 191 | value_ir_min = min; |
com3 | 6:97015325107b | 192 | if(/*(value_ir_min < NO_BALL) || */(value_ir[11] > 45)){ |
com3 | 6:97015325107b | 193 | if(sum < 4000){ |
com3 | 6:97015325107b | 194 | //myled = 1; |
com3 | 6:97015325107b | 195 | if((num == 0) || (num == 8) || (num == 9)){ |
com3 | 6:97015325107b | 196 | //myled = 4; |
com3 | 6:97015325107b | 197 | if(value_ir_min <= 25){ |
com3 | 6:97015325107b | 198 | num = 0; |
com3 | 6:97015325107b | 199 | } else if((value_ir[1] >= value_ir[7] - ERR) && (value_ir[1] <= value_ir[7] + ERR)){ |
com3 | 6:97015325107b | 200 | num = 0; |
com3 | 6:97015325107b | 201 | myled = 0; |
com3 | 6:97015325107b | 202 | } else if(value_ir[1] > value_ir[7]){ |
com3 | 6:97015325107b | 203 | num = 9; |
com3 | 6:97015325107b | 204 | myled = 3; |
com3 | 6:97015325107b | 205 | } else if(value_ir[1] < value_ir[7]){ |
com3 | 6:97015325107b | 206 | num = 8; |
com3 | 6:97015325107b | 207 | myled = 12; |
com3 | 6:97015325107b | 208 | } |
com3 | 6:97015325107b | 209 | if((value_ir[0] + value_ir[8] + value_ir[9]) < 72){ |
com3 | 6:97015325107b | 210 | myled = 15; |
com3 | 6:97015325107b | 211 | } |
com3 | 6:97015325107b | 212 | /* |
com3 | 6:97015325107b | 213 | if(value_ir[0] == value_ir[8]){ |
com3 | 6:97015325107b | 214 | num = 10; |
com3 | 6:97015325107b | 215 | myled = 6; |
com3 | 6:97015325107b | 216 | } else if(value_ir[0] == value_ir[9]){ |
com3 | 6:97015325107b | 217 | num = 11; |
com3 | 6:97015325107b | 218 | myled = 12; |
com3 | 6:97015325107b | 219 | } |
com3 | 6:97015325107b | 220 | */ |
com3 | 6:97015325107b | 221 | } else if(num == 8){ |
com3 | 6:97015325107b | 222 | //myled = 2; |
com3 | 6:97015325107b | 223 | } else if(num == 9){ |
com3 | 6:97015325107b | 224 | //myled = 8; |
com3 | 6:97015325107b | 225 | } |
com3 | 6:97015325107b | 226 | |
com3 | 6:97015325107b | 227 | ir_min_num = num; |
com3 | 6:97015325107b | 228 | }else{ |
com3 | 6:97015325107b | 229 | ir_min_num = NO_BALL; |
com3 | 6:97015325107b | 230 | myled = 0; |
com3 | 6:97015325107b | 231 | } |
com3 | 6:97015325107b | 232 | } else { |
com3 | 6:97015325107b | 233 | ir_min_num = NO_BALL; |
com3 | 6:97015325107b | 234 | myled = 0; |
com3 | 6:97015325107b | 235 | } |
ryuna | 1:3ecb80796a7f | 236 | } |
ryuna | 0:010fe1eae7d2 | 237 | int main() { |
ryuna | 0:010fe1eae7d2 | 238 | //enum sensors kinds; |
ryuna | 3:7de2c50339bd | 239 | int compassdef = 0; |
ryuna | 0:010fe1eae7d2 | 240 | int num[ALL_KIND] = {0}; |
com3 | 6:97015325107b | 241 | int ping_flag = 0; |
com3 | 6:97015325107b | 242 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
ryuna | 1:3ecb80796a7f | 243 | //送信開始 |
ryuna | 1:3ecb80796a7f | 244 | sensor.putc(1); |
ryuna | 1:3ecb80796a7f | 245 | //送信空き割り込み設定 |
ryuna | 1:3ecb80796a7f | 246 | sensor.attach(&micon_tx,Serial::TxIrq); |
ryuna | 1:3ecb80796a7f | 247 | compassdef = (compass.sample() / 10); |
ryuna | 1:3ecb80796a7f | 248 | |
ryuna | 0:010fe1eae7d2 | 249 | while(1) { |
ryuna | 3:7de2c50339bd | 250 | value_compass0 = ((compass.sample() / 10) + 540 - compassdef) % 360; |
ryuna | 3:7de2c50339bd | 251 | if(value_compass0 >=255 ){ |
ryuna | 3:7de2c50339bd | 252 | value_compass[0] = 255; |
ryuna | 3:7de2c50339bd | 253 | value_compass[1] = value_compass0 - 255 ; |
ryuna | 3:7de2c50339bd | 254 | }else{ |
ryuna | 3:7de2c50339bd | 255 | value_compass[0] = value_compass0; |
ryuna | 3:7de2c50339bd | 256 | value_compass[1] = 0; |
ryuna | 3:7de2c50339bd | 257 | } |
com3 | 6:97015325107b | 258 | //myled = 1; |
com3 | 6:97015325107b | 259 | //pc.printf("%d\t%d\n", value_compass0, compassdef); |
com3 | 6:97015325107b | 260 | |
com3 | 6:97015325107b | 261 | for(num[Ir] = 0; num[Ir] < ALL_IR; num[Ir]++){ |
com3 | 6:97015325107b | 262 | moving_ave(Ir,num[Ir],ir_function(num[Ir])); |
com3 | 6:97015325107b | 263 | } |
com3 | 6:97015325107b | 264 | ir_min_fun(); |
com3 | 6:97015325107b | 265 | //pc.printf("%d %d\n", value_ir[9], value_compass0); |
com3 | 6:97015325107b | 266 | //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t %d %d\n", value_ir[0], value_ir[1], value_ir[2], value_ir[3], value_ir[4], value_ir[5], value_ir[6], value_ir[7], value_ir[8], value_ir[9], value_ir[10], value_ir[11], value_ir_min, ir_min_num); |
com3 | 6:97015325107b | 267 | //pc.printf("%d\t%d\n",value_ir_min,ir_min_num); |
com3 | 6:97015325107b | 268 | //pc.printf("%d\n", value_ir[11]); |
com3 | 6:97015325107b | 269 | //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\n", value_ir[1], value_ir[8], value_ir[0], value_ir[9], value_ir[7], ir_min_num); //前5つ |
com3 | 6:97015325107b | 270 | //pc.printf("%d\t%d\t%d\t%d\t%d\n", value_ir[3], value_ir[2], value_ir[1], value_ir[8], ir_min_num); //左4つ |
com3 | 6:97015325107b | 271 | //myled = 2; |
com3 | 6:97015325107b | 272 | |
com3 | 6:97015325107b | 273 | if(ping_flag == 2){ |
com3 | 6:97015325107b | 274 | ping_flag = 0; |
com3 | 6:97015325107b | 275 | for(num[Ping] = 0; num[Ping] < ALL_PING; num[Ping]++){ |
com3 | 6:97015325107b | 276 | moving_ave(Ping,num[Ping],ping_function(num[Ping])); |
com3 | 6:97015325107b | 277 | } |
com3 | 6:97015325107b | 278 | //pc.printf("%d %d %d %d\n", value_ping[0], value_ping[1], value_ping[2], value_ping[3]); |
com3 | 6:97015325107b | 279 | } |
com3 | 6:97015325107b | 280 | ping_flag++; |
com3 | 6:97015325107b | 281 | //myled = 4; |
ryuna | 0:010fe1eae7d2 | 282 | |
ryuna | 0:010fe1eae7d2 | 283 | } |
com3 | 6:97015325107b | 284 | } |