Hajime Muraki / Mbed 2 deprecated program_SensorBlock

sensor

Dependencies:   HMC6352 mbed

Fork of program_SensorBlock by Fumiya Fujisawa

main.cpp@2:f7da3aa0520a, 2014-02-24 (annotated)

Committer:
ryuna
Date:
Mon Feb 24 05:23:18 2014 +0000
Revision:
2:f7da3aa0520a
Parent:
1:3ecb80796a7f
Child:
3:7de2c50339bd
aaa

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 */
ryuna 0:010fe1eae7d2 4 #define ERROR_PING 0xFFF
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
ryuna 0:010fe1eae7d2 9 #define ALL_IR 10
ryuna 0:010fe1eae7d2 10 //#define OVERLINE_IR 46
ryuna 0:010fe1eae7d2 11 /*LINE DEFINE*/
ryuna 1:3ecb80796a7f 12 #define JUDGEVOL_LINE 0.9
ryuna 0:010fe1eae7d2 13 #define ALL_LINE 4
ryuna 0:010fe1eae7d2 14 //OTHERS
ryuna 0:010fe1eae7d2 15 #define ALL_KIND 3 //kind of sensors
ryuna 0:010fe1eae7d2 16 #define MAX_NUM 10 //sensors most number
ryuna 1:3ecb80796a7f 17 extern void micon_tx();
ryuna 0:010fe1eae7d2 18 /*
ryuna 1:3ecb80796a7f 19 Ping , IrSensor , LineSensor ,compass
ryuna 0:010fe1eae7d2 20 */
ryuna 0:010fe1eae7d2 21 //LINE sensor is float value
ryuna 1:3ecb80796a7f 22 Serial sensor(p13,p14);
ryuna 0:010fe1eae7d2 23 Serial pc(USBTX,USBRX);
ryuna 1:3ecb80796a7f 24 HMC6352 compass(p9, p10);
ryuna 0:010fe1eae7d2 25 Timer time_ping;
ryuna 0:010fe1eae7d2 26 Timer time_ir;
ryuna 0:010fe1eae7d2 27
ryuna 0:010fe1eae7d2 28 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
ryuna 0:010fe1eae7d2 29 PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
ryuna 0:010fe1eae7d2 30 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
ryuna 0:010fe1eae7d2 31
ryuna 0:010fe1eae7d2 32 enum sensors{Ping,Ir,Line};
ryuna 0:010fe1eae7d2 33 volatile unsigned int value_ping[ALL_PING] = {0};
ryuna 0:010fe1eae7d2 34 volatile unsigned int value_ir[ALL_IR] = {0};
ryuna 1:3ecb80796a7f 35 volatile unsigned int value_ir_min = 0;
ryuna 1:3ecb80796a7f 36 volatile unsigned int ir_min_num = 0;
ryuna 1:3ecb80796a7f 37 volatile unsigned int value_line[ALL_LINE] = {0};
ryuna 1:3ecb80796a7f 38 volatile unsigned int value_compass = 0;
ryuna 0:010fe1eae7d2 39
ryuna 1:3ecb80796a7f 40
ryuna 1:3ecb80796a7f 41 /*unsigned int*/void moving_ave(enum sensors kind,int num, unsigned int data){
ryuna 0:010fe1eae7d2 42 static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
ryuna 0:010fe1eae7d2 43 static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
ryuna 0:010fe1eae7d2 44 //static unsigned int ave[MAX_NUM] = {0};
ryuna 0:010fe1eae7d2 45 sum[kind][num] -= temp[kind][num][9];
ryuna 0:010fe1eae7d2 46 sum[kind][num] += data;
ryuna 0:010fe1eae7d2 47 temp[kind][num][9] = temp[kind][num][8];
ryuna 0:010fe1eae7d2 48 temp[kind][num][8] = temp[kind][num][7];
ryuna 0:010fe1eae7d2 49 temp[kind][num][7] = temp[kind][num][6];
ryuna 0:010fe1eae7d2 50 temp[kind][num][6] = temp[kind][num][5];
ryuna 0:010fe1eae7d2 51 temp[kind][num][5] = temp[kind][num][4];
ryuna 0:010fe1eae7d2 52 temp[kind][num][4] = temp[kind][num][3];
ryuna 0:010fe1eae7d2 53 temp[kind][num][3] = temp[kind][num][2];
ryuna 0:010fe1eae7d2 54 temp[kind][num][2] = temp[kind][num][1];
ryuna 0:010fe1eae7d2 55 temp[kind][num][1] = temp[kind][num][0];
ryuna 0:010fe1eae7d2 56 temp[kind][num][0] = data;
ryuna 0:010fe1eae7d2 57 //ave[kind][num] = sum[kind][num]/100;
ryuna 0:010fe1eae7d2 58
ryuna 0:010fe1eae7d2 59 switch (kind) {
ryuna 0:010fe1eae7d2 60 case Ping:
ryuna 0:010fe1eae7d2 61 value_ping[num] = sum[kind][num]/10;
ryuna 0:010fe1eae7d2 62 break;
ryuna 0:010fe1eae7d2 63 case Ir:
ryuna 0:010fe1eae7d2 64 value_ir[num] = sum[kind][num]/100;
ryuna 0:010fe1eae7d2 65 break;
ryuna 0:010fe1eae7d2 66 case Line:
ryuna 1:3ecb80796a7f 67 if((sum[kind][num]/10000.0) >=JUDGEVOL_LINE){
ryuna 1:3ecb80796a7f 68 value_line[num] = 1;//white
ryuna 1:3ecb80796a7f 69 }else{
ryuna 1:3ecb80796a7f 70 value_line[num] = 0;//green
ryuna 1:3ecb80796a7f 71 }
ryuna 0:010fe1eae7d2 72 break;
ryuna 0:010fe1eae7d2 73 default :
ryuna 0:010fe1eae7d2 74 break;
ryuna 0:010fe1eae7d2 75 }
ryuna 0:010fe1eae7d2 76
ryuna 1:3ecb80796a7f 77 //return sum[kind][num];//return no config sum
ryuna 0:010fe1eae7d2 78
ryuna 0:010fe1eae7d2 79 }
ryuna 0:010fe1eae7d2 80 unsigned int ping_function (int num){
ryuna 0:010fe1eae7d2 81 DigitalInOut sensor_ping(num_ping[num]);
ryuna 0:010fe1eae7d2 82 unsigned int memory = 0;//return value
ryuna 0:010fe1eae7d2 83 time_ping.reset();
ryuna 0:010fe1eae7d2 84 /*make a pulse */
ryuna 0:010fe1eae7d2 85 sensor_ping.output();
ryuna 0:010fe1eae7d2 86 sensor_ping = 1;
ryuna 0:010fe1eae7d2 87 wait_us(5);
ryuna 0:010fe1eae7d2 88 sensor_ping = 0;
ryuna 0:010fe1eae7d2 89 sensor_ping.input();
ryuna 0:010fe1eae7d2 90 /*finish,and start timer*/
ryuna 0:010fe1eae7d2 91 time_ping.start();
ryuna 0:010fe1eae7d2 92 while(!sensor_ping){
ryuna 0:010fe1eae7d2 93 if(time_ping.read_ms() > 1){
ryuna 0:010fe1eae7d2 94 time_ping.stop();
ryuna 0:010fe1eae7d2 95 return ERROR_PING;
ryuna 0:010fe1eae7d2 96 }
ryuna 0:010fe1eae7d2 97 }
ryuna 0:010fe1eae7d2 98 time_ping.reset();
ryuna 0:010fe1eae7d2 99 while(sensor_ping){
ryuna 0:010fe1eae7d2 100 if(time_ping.read_ms() >18.5){
ryuna 0:010fe1eae7d2 101 time_ping.stop();
ryuna 0:010fe1eae7d2 102 return ERROR_PING;
ryuna 0:010fe1eae7d2 103 }
ryuna 0:010fe1eae7d2 104 }
ryuna 0:010fe1eae7d2 105 memory = time_ping.read_us();
ryuna 0:010fe1eae7d2 106 time_ping.stop();
ryuna 0:010fe1eae7d2 107
ryuna 0:010fe1eae7d2 108 return memory;
ryuna 0:010fe1eae7d2 109 }
ryuna 0:010fe1eae7d2 110 unsigned int ir_function (int num){
ryuna 0:010fe1eae7d2 111 DigitalIn sensor_ir(num_ir[num]);
ryuna 0:010fe1eae7d2 112 int flag = 0;
ryuna 0:010fe1eae7d2 113 unsigned int memory = 0;
ryuna 0:010fe1eae7d2 114 unsigned int temp = 0;
ryuna 0:010fe1eae7d2 115 flag = 1;
ryuna 0:010fe1eae7d2 116 time_ir.start();
ryuna 0:010fe1eae7d2 117 if((sensor_ir)){
ryuna 0:010fe1eae7d2 118 while((sensor_ir)){
ryuna 0:010fe1eae7d2 119 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 120 flag = 0;
ryuna 0:010fe1eae7d2 121 break;
ryuna 0:010fe1eae7d2 122 }
ryuna 0:010fe1eae7d2 123 }
ryuna 0:010fe1eae7d2 124 }
ryuna 0:010fe1eae7d2 125 time_ir.stop();
ryuna 0:010fe1eae7d2 126 time_ir.reset();
ryuna 0:010fe1eae7d2 127 if(flag){
ryuna 0:010fe1eae7d2 128 time_ir.start();
ryuna 0:010fe1eae7d2 129 while(!sensor_ir){//!
ryuna 0:010fe1eae7d2 130 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 131 break;
ryuna 0:010fe1eae7d2 132 }
ryuna 0:010fe1eae7d2 133 }
ryuna 0:010fe1eae7d2 134 memory = time_ir.read_us();
ryuna 0:010fe1eae7d2 135 while(1){
ryuna 0:010fe1eae7d2 136 if(!sensor_ir){
ryuna 0:010fe1eae7d2 137 temp = (time_ir.read_us() - memory);
ryuna 0:010fe1eae7d2 138 time_ir.stop();
ryuna 0:010fe1eae7d2 139 time_ir.reset();
ryuna 1:3ecb80796a7f 140 //wait(0.01);
ryuna 0:010fe1eae7d2 141 return temp;
ryuna 0:010fe1eae7d2 142 }
ryuna 0:010fe1eae7d2 143 }
ryuna 0:010fe1eae7d2 144 }else{//not found
ryuna 0:010fe1eae7d2 145 }
ryuna 0:010fe1eae7d2 146 time_ir.stop();
ryuna 0:010fe1eae7d2 147 time_ir.reset();
ryuna 0:010fe1eae7d2 148 return ERROR_IR;
ryuna 0:010fe1eae7d2 149 }
ryuna 0:010fe1eae7d2 150 int line_function(int num){
ryuna 0:010fe1eae7d2 151 float memory = 0;
ryuna 0:010fe1eae7d2 152 AnalogIn sensor_line(num_line[num]);
ryuna 0:010fe1eae7d2 153 memory = sensor_line;
ryuna 0:010fe1eae7d2 154 return (int)(memory*1000);
ryuna 0:010fe1eae7d2 155
ryuna 0:010fe1eae7d2 156 }
ryuna 1:3ecb80796a7f 157 void ir_min_fun(){
ryuna 1:3ecb80796a7f 158 static unsigned int min;
ryuna 1:3ecb80796a7f 159 static int i, num = 0;
ryuna 1:3ecb80796a7f 160 min = value_ir[0];
ryuna 1:3ecb80796a7f 161 for (i = 0;i <=ALL_IR -1;i++){
ryuna 1:3ecb80796a7f 162 if(min >= value_ir[i]){
ryuna 1:3ecb80796a7f 163 min = value_ir[i];
ryuna 1:3ecb80796a7f 164 num = i;
ryuna 1:3ecb80796a7f 165 }
ryuna 1:3ecb80796a7f 166 }
ryuna 1:3ecb80796a7f 167 value_ir_min = min;
ryuna 1:3ecb80796a7f 168 ir_min_num = num;
ryuna 1:3ecb80796a7f 169 }
ryuna 0:010fe1eae7d2 170 int main() {
ryuna 0:010fe1eae7d2 171 //enum sensors kinds;
ryuna 1:3ecb80796a7f 172 uint8_t compassdef = 0;
ryuna 0:010fe1eae7d2 173 int num[ALL_KIND] = {0};
ryuna 1:3ecb80796a7f 174 //送信開始
ryuna 1:3ecb80796a7f 175 sensor.putc(1);
ryuna 1:3ecb80796a7f 176 //送信空き割り込み設定
ryuna 1:3ecb80796a7f 177 sensor.attach(&micon_tx,Serial::TxIrq);
ryuna 1:3ecb80796a7f 178 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 1:3ecb80796a7f 179 compassdef = (compass.sample() / 10);
ryuna 1:3ecb80796a7f 180
ryuna 0:010fe1eae7d2 181 while(1) {
ryuna 1:3ecb80796a7f 182 value_compass = ((compass.sample() / 10) + 540 - compassdef) % 360;
ryuna 1:3ecb80796a7f 183 moving_ave(Ping,num[Ping],ping_function(num[Ping]));
ryuna 1:3ecb80796a7f 184
ryuna 1:3ecb80796a7f 185 pc.printf("value_compass=%d\n",value_compass);
ryuna 1:3ecb80796a7f 186 //ir_min_fun();
ryuna 1:3ecb80796a7f 187 //num[Ping]++;
ryuna 1:3ecb80796a7f 188 //num[Ir]++;
ryuna 1:3ecb80796a7f 189 //num[Line]++;
ryuna 0:010fe1eae7d2 190 if(num[Ping]>= ALL_PING){
ryuna 0:010fe1eae7d2 191 num[Ping] = 0;
ryuna 1:3ecb80796a7f 192 putchar('\n');
ryuna 0:010fe1eae7d2 193 }
ryuna 0:010fe1eae7d2 194 if(num[Ir]>= ALL_IR){
ryuna 0:010fe1eae7d2 195 num[Ir] = 0;
ryuna 0:010fe1eae7d2 196 putchar('\n');
ryuna 1:3ecb80796a7f 197 pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num);
ryuna 0:010fe1eae7d2 198 }
ryuna 0:010fe1eae7d2 199 if(num[Line]>= ALL_LINE){
ryuna 0:010fe1eae7d2 200 num[Line] = 0;
ryuna 1:3ecb80796a7f 201 putchar('\n');
ryuna 0:010fe1eae7d2 202 }
ryuna 0:010fe1eae7d2 203
ryuna 0:010fe1eae7d2 204 }
ryuna 0:010fe1eae7d2 205 }