![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sensor
Fork of program_SensorBlock by
Diff: main.cpp
- Revision:
- 6:97015325107b
- Parent:
- 5:3457e0e66104
- Child:
- 8:f547b66f3c43
--- a/main.cpp Tue Feb 25 02:29:32 2014 +0000 +++ b/main.cpp Sat Mar 08 13:16:25 2014 +0000 @@ -1,45 +1,52 @@ #include "mbed.h" #include "HMC6352.h" /*PING DEFINE */ -#define ERROR_PING 0xFFF +#define ERROR_PING 0xFF #define ALL_PING 4 /*IR DEFINE*/ #define BREAKPT_IR 833 #define ERROR_IR 0xFFF -#define ALL_IR 10 +#define ALL_IR 12 +#define NOTBALL 3 +#define NO_BALL 45 +#define ERR 7 //#define OVERLINE_IR 46 /*LINE DEFINE*/ -#define JUDGEVOL_LINE 0.9 +#define JUDGEVOL_LINE 0.1 #define ALL_LINE 4 //OTHERS #define ALL_KIND 3 //kind of sensors -#define MAX_NUM 10 //sensors most number +#define MAX_NUM 12 //sensors most number extern void micon_tx(); /* Ping , IrSensor , LineSensor ,compass */ -//LINE sensor is float value +//LINE sensor is float value + +BusOut myled(LED1, LED2, LED3, LED4); + Serial sensor(p13,p14); Serial pc(USBTX,USBRX); HMC6352 compass(p9, p10); - + Timer time_ping; Timer time_ir; - + PinName num_ping[ALL_PING] = {p5,p6,p7,p8}; -PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30}; +PinName num_ir[ALL_IR] = {p29,p21,p22,p23,p24,p25,p26,p27,p30,p29,p12,p15}; PinName num_line[ALL_LINE] = {p17,p18,p19,p20}; - + enum sensors{Ping,Ir,Line}; volatile unsigned int value_ping[ALL_PING] = {0}; volatile unsigned int value_ir[ALL_IR] = {0}; +volatile int hosei_ir[ALL_IR] = {-1,1,0,0,0,0,0,1,-2,-1,0,0}; volatile unsigned int value_ir_min = 0; volatile unsigned int ir_min_num = 0; volatile unsigned int value_line[ALL_LINE] = {0}; volatile unsigned int value_compass0 = 0; volatile unsigned int value_compass[2] = {0}; - - + + /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){ static unsigned int sum[ALL_KIND][MAX_NUM] = {0}; static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0}; @@ -71,11 +78,12 @@ }else{ value_line[num] = 0;//green } + //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); break; default : break; } - + //return sum[kind][num];//return no config sum } @@ -99,12 +107,13 @@ } time_ping.reset(); while(sensor_ping){ - if(time_ping.read_ms() >18.5){ + if(time_ping.read_ms() >18.5){//18.5 time_ping.stop(); return ERROR_PING; } } memory = time_ping.read_us(); + memory >>= 6; time_ping.stop(); return memory; @@ -134,7 +143,17 @@ } } memory = time_ir.read_us(); - while(1){ + while(sensor_ir){ + if((time_ir.read_us() - memory) >= BREAKPT_IR){ + break; + } + } + temp = (time_ir.read_us() - memory); + time_ir.stop(); + time_ir.reset(); + //wait(0.01); + return temp; + /*while(1){ if(!sensor_ir){ temp = (time_ir.read_us() - memory); time_ir.stop(); @@ -142,7 +161,7 @@ //wait(0.01); return temp; } - } + }*/ }else{//not found } time_ir.stop(); @@ -154,30 +173,77 @@ AnalogIn sensor_line(num_line[num]); memory = sensor_line; return (int)(memory*1000); - + } void ir_min_fun(){ static unsigned int min; - static int i, num = 0; + static int i, num = 0, sum = 0; + sum = 0; min = value_ir[0]; - for (i = 0;i <=ALL_IR -1;i++){ - if(min >= value_ir[i]){ + for (i = 0;i <= ALL_IR - NOTBALL;i++){ + value_ir[i] += hosei_ir[i]; + sum += value_ir[i]; + if(min > value_ir[i]){ min = value_ir[i]; num = i; - } + } } value_ir_min = min; - ir_min_num = num; + if(/*(value_ir_min < NO_BALL) || */(value_ir[11] > 45)){ + if(sum < 4000){ + //myled = 1; + if((num == 0) || (num == 8) || (num == 9)){ + //myled = 4; + if(value_ir_min <= 25){ + num = 0; + } else if((value_ir[1] >= value_ir[7] - ERR) && (value_ir[1] <= value_ir[7] + ERR)){ + num = 0; + myled = 0; + } else if(value_ir[1] > value_ir[7]){ + num = 9; + myled = 3; + } else if(value_ir[1] < value_ir[7]){ + num = 8; + myled = 12; + } + if((value_ir[0] + value_ir[8] + value_ir[9]) < 72){ + myled = 15; + } + /* + if(value_ir[0] == value_ir[8]){ + num = 10; + myled = 6; + } else if(value_ir[0] == value_ir[9]){ + num = 11; + myled = 12; + } + */ + } else if(num == 8){ + //myled = 2; + } else if(num == 9){ + //myled = 8; + } + + ir_min_num = num; + }else{ + ir_min_num = NO_BALL; + myled = 0; + } + } else { + ir_min_num = NO_BALL; + myled = 0; + } } int main() { //enum sensors kinds; int compassdef = 0; int num[ALL_KIND] = {0}; + int ping_flag = 0; + compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); //送信開始 sensor.putc(1); //送信空き割り込み設定 sensor.attach(&micon_tx,Serial::TxIrq); - compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); compassdef = (compass.sample() / 10); while(1) { @@ -189,25 +255,61 @@ value_compass[0] = value_compass0; value_compass[1] = 0; } + //myled = 1; + //pc.printf("%d\t%d\n", value_compass0, compassdef); + + for(num[Ir] = 0; num[Ir] < ALL_IR; num[Ir]++){ + moving_ave(Ir,num[Ir],ir_function(num[Ir])); + } + ir_min_fun(); + //pc.printf("%d %d\n", value_ir[9], value_compass0); + //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); + //pc.printf("%d\t%d\n",value_ir_min,ir_min_num); + //pc.printf("%d\n", value_ir[11]); + //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つ + //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つ + //myled = 2; + + if(ping_flag == 2){ + ping_flag = 0; + for(num[Ping] = 0; num[Ping] < ALL_PING; num[Ping]++){ + moving_ave(Ping,num[Ping],ping_function(num[Ping])); + } + //pc.printf("%d %d %d %d\n", value_ping[0], value_ping[1], value_ping[2], value_ping[3]); + } + ping_flag++; + //myled = 4; + /* moving_ave(Ping,num[Ping],ping_function(num[Ping])); - pc.printf("compass0=%d compass[0]= %d compass[1]= %d\n",value_compass0,value_compass[0],value_compass[1]); - //ir_min_fun(); - //num[Ping]++; - //num[Ir]++; - //num[Line]++; + //moving_ave(Line,num[Line],line_function(num[Line])); + //pc.printf("%d\n", line_function(2)); + if(line_function(num[Line]) > 500){ + value_line[num[Line]] = 1; + } else { + value_line[num[Line]] = 0; + } + moving_ave(Ir,num[Ir],ir_function(num[Ir])); + + num[Ping]++; + num[Ir]++; + num[Line]++; if(num[Ping]>= ALL_PING){ num[Ping] = 0; - putchar('\n'); + + //putchar('\n'); } if(num[Ir]>= ALL_IR){ num[Ir] = 0; - putchar('\n'); - pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num); + ir_min_fun(); + //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]); + pc.printf("%d %d\n",value_ir_min,ir_min_num); + //putchar('\n'); } if(num[Line]>= ALL_LINE){ num[Line] = 0; - putchar('\n'); + //putchar('\n'); } + */ } -} +} \ No newline at end of file