Hajime Muraki
sensor
Fork of
program_SensorBlock
by 
Fumiya Fujisawa
Diff: main.cpp
- Revision:
- 6:97015325107b
- Parent:
- 5:3457e0e66104
- Child:
- 8:f547b66f3c43
diff -r 3457e0e66104 -r 97015325107b main.cpp
--- 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