Hajime Muraki / Mbed 2 deprecated program_SensorBlock

sensor

Dependencies:   HMC6352 mbed

Fork of program_SensorBlock by Fumiya Fujisawa

main.cpp@6:97015325107b, 2014-03-08 (annotated)

Committer:
com3
Date:
Sat Mar 08 13:16:25 2014 +0000
Revision:
6:97015325107b
Parent:
5:3457e0e66104
Child:
8:f547b66f3c43
sensors

Who changed what in which revision?

UserRevisionLine numberNew 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;
com3 6:97015325107b 282 /*
ryuna 1:3ecb80796a7f 283 moving_ave(Ping,num[Ping],ping_function(num[Ping]));
com3 6:97015325107b 284 //moving_ave(Line,num[Line],line_function(num[Line]));
com3 6:97015325107b 285 //pc.printf("%d\n", line_function(2));
com3 6:97015325107b 286 if(line_function(num[Line]) > 500){
com3 6:97015325107b 287 value_line[num[Line]] = 1;
com3 6:97015325107b 288 } else {
com3 6:97015325107b 289 value_line[num[Line]] = 0;
com3 6:97015325107b 290 }
com3 6:97015325107b 291 moving_ave(Ir,num[Ir],ir_function(num[Ir]));
com3 6:97015325107b 292
com3 6:97015325107b 293 num[Ping]++;
com3 6:97015325107b 294 num[Ir]++;
com3 6:97015325107b 295 num[Line]++;
ryuna 0:010fe1eae7d2 296 if(num[Ping]>= ALL_PING){
ryuna 0:010fe1eae7d2 297 num[Ping] = 0;
com3 6:97015325107b 298
com3 6:97015325107b 299 //putchar('\n');
ryuna 0:010fe1eae7d2 300 }
ryuna 0:010fe1eae7d2 301 if(num[Ir]>= ALL_IR){
ryuna 0:010fe1eae7d2 302 num[Ir] = 0;
com3 6:97015325107b 303 ir_min_fun();
com3 6:97015325107b 304 //pc.printf("%d %d %d %d %d %d %d %d %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]);
com3 6:97015325107b 305 pc.printf("%d %d\n",value_ir_min,ir_min_num);
com3 6:97015325107b 306 //putchar('\n');
ryuna 0:010fe1eae7d2 307 }
ryuna 0:010fe1eae7d2 308 if(num[Line]>= ALL_LINE){
ryuna 0:010fe1eae7d2 309 num[Line] = 0;
com3 6:97015325107b 310 //putchar('\n');
ryuna 0:010fe1eae7d2 311 }
com3 6:97015325107b 312 */
ryuna 0:010fe1eae7d2 313
ryuna 0:010fe1eae7d2 314 }
com3 6:97015325107b 315 }