Fumiya Fujisawa / Mbed 2 deprecated program_SensorBlock

program_SensorBlock //{Ping,Ir,Lines,compass} Sensor

Dependencies:   HMC6352 mbed

Fork of program_BlockPIL by Fumiya Fujisawa

main.cpp@6:500e2c97a690, 2014-02-25 (annotated)

Committer:
ryuna
Date:
Tue Feb 25 08:18:22 2014 +0000
Revision:
6:500e2c97a690
Parent:
5:3457e0e66104 
sensors version.4

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