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@5:3457e0e66104, 2014-02-25 (annotated)

Committer:
ryuna
Date:
Tue Feb 25 02:29:32 2014 +0000
Revision:
5:3457e0e66104
Parent:
4:1d54610dba80
Child:
6:500e2c97a690
program sensors version.2;

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 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 0:010fe1eae7d2 102 if(time_ping.read_ms() >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 0:010fe1eae7d2 108 time_ping.stop();
ryuna 0:010fe1eae7d2 109
ryuna 0:010fe1eae7d2 110 return memory;
ryuna 0:010fe1eae7d2 111 }
ryuna 0:010fe1eae7d2 112 unsigned int ir_function (int num){
ryuna 0:010fe1eae7d2 113 DigitalIn sensor_ir(num_ir[num]);
ryuna 0:010fe1eae7d2 114 int flag = 0;
ryuna 0:010fe1eae7d2 115 unsigned int memory = 0;
ryuna 0:010fe1eae7d2 116 unsigned int temp = 0;
ryuna 0:010fe1eae7d2 117 flag = 1;
ryuna 0:010fe1eae7d2 118 time_ir.start();
ryuna 0:010fe1eae7d2 119 if((sensor_ir)){
ryuna 0:010fe1eae7d2 120 while((sensor_ir)){
ryuna 0:010fe1eae7d2 121 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 122 flag = 0;
ryuna 0:010fe1eae7d2 123 break;
ryuna 0:010fe1eae7d2 124 }
ryuna 0:010fe1eae7d2 125 }
ryuna 0:010fe1eae7d2 126 }
ryuna 0:010fe1eae7d2 127 time_ir.stop();
ryuna 0:010fe1eae7d2 128 time_ir.reset();
ryuna 0:010fe1eae7d2 129 if(flag){
ryuna 0:010fe1eae7d2 130 time_ir.start();
ryuna 0:010fe1eae7d2 131 while(!sensor_ir){//!
ryuna 0:010fe1eae7d2 132 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 133 break;
ryuna 0:010fe1eae7d2 134 }
ryuna 0:010fe1eae7d2 135 }
ryuna 0:010fe1eae7d2 136 memory = time_ir.read_us();
ryuna 0:010fe1eae7d2 137 while(1){
ryuna 0:010fe1eae7d2 138 if(!sensor_ir){
ryuna 0:010fe1eae7d2 139 temp = (time_ir.read_us() - memory);
ryuna 0:010fe1eae7d2 140 time_ir.stop();
ryuna 0:010fe1eae7d2 141 time_ir.reset();
ryuna 1:3ecb80796a7f 142 //wait(0.01);
ryuna 0:010fe1eae7d2 143 return temp;
ryuna 0:010fe1eae7d2 144 }
ryuna 0:010fe1eae7d2 145 }
ryuna 0:010fe1eae7d2 146 }else{//not found
ryuna 0:010fe1eae7d2 147 }
ryuna 0:010fe1eae7d2 148 time_ir.stop();
ryuna 0:010fe1eae7d2 149 time_ir.reset();
ryuna 0:010fe1eae7d2 150 return ERROR_IR;
ryuna 0:010fe1eae7d2 151 }
ryuna 0:010fe1eae7d2 152 int line_function(int num){
ryuna 0:010fe1eae7d2 153 float memory = 0;
ryuna 0:010fe1eae7d2 154 AnalogIn sensor_line(num_line[num]);
ryuna 0:010fe1eae7d2 155 memory = sensor_line;
ryuna 0:010fe1eae7d2 156 return (int)(memory*1000);
ryuna 0:010fe1eae7d2 157
ryuna 0:010fe1eae7d2 158 }
ryuna 1:3ecb80796a7f 159 void ir_min_fun(){
ryuna 1:3ecb80796a7f 160 static unsigned int min;
ryuna 1:3ecb80796a7f 161 static int i, num = 0;
ryuna 1:3ecb80796a7f 162 min = value_ir[0];
ryuna 1:3ecb80796a7f 163 for (i = 0;i <=ALL_IR -1;i++){
ryuna 1:3ecb80796a7f 164 if(min >= value_ir[i]){
ryuna 1:3ecb80796a7f 165 min = value_ir[i];
ryuna 1:3ecb80796a7f 166 num = i;
ryuna 1:3ecb80796a7f 167 }
ryuna 1:3ecb80796a7f 168 }
ryuna 1:3ecb80796a7f 169 value_ir_min = min;
ryuna 1:3ecb80796a7f 170 ir_min_num = num;
ryuna 1:3ecb80796a7f 171 }
ryuna 0:010fe1eae7d2 172 int main() {
ryuna 0:010fe1eae7d2 173 //enum sensors kinds;
ryuna 3:7de2c50339bd 174 int compassdef = 0;
ryuna 0:010fe1eae7d2 175 int num[ALL_KIND] = {0};
ryuna 1:3ecb80796a7f 176 //送信開始
ryuna 1:3ecb80796a7f 177 sensor.putc(1);
ryuna 1:3ecb80796a7f 178 //送信空き割り込み設定
ryuna 1:3ecb80796a7f 179 sensor.attach(&micon_tx,Serial::TxIrq);
ryuna 1:3ecb80796a7f 180 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 1:3ecb80796a7f 181 compassdef = (compass.sample() / 10);
ryuna 1:3ecb80796a7f 182
ryuna 0:010fe1eae7d2 183 while(1) {
ryuna 3:7de2c50339bd 184 value_compass0 = ((compass.sample() / 10) + 540 - compassdef) % 360;
ryuna 3:7de2c50339bd 185 if(value_compass0 >=255 ){
ryuna 3:7de2c50339bd 186 value_compass[0] = 255;
ryuna 3:7de2c50339bd 187 value_compass[1] = value_compass0 - 255 ;
ryuna 3:7de2c50339bd 188 }else{
ryuna 3:7de2c50339bd 189 value_compass[0] = value_compass0;
ryuna 3:7de2c50339bd 190 value_compass[1] = 0;
ryuna 3:7de2c50339bd 191 }
ryuna 1:3ecb80796a7f 192 moving_ave(Ping,num[Ping],ping_function(num[Ping]));
ryuna 3:7de2c50339bd 193 pc.printf("compass0=%d compass[0]= %d compass[1]= %d\n",value_compass0,value_compass[0],value_compass[1]);
ryuna 1:3ecb80796a7f 194 //ir_min_fun();
ryuna 1:3ecb80796a7f 195 //num[Ping]++;
ryuna 1:3ecb80796a7f 196 //num[Ir]++;
ryuna 1:3ecb80796a7f 197 //num[Line]++;
ryuna 0:010fe1eae7d2 198 if(num[Ping]>= ALL_PING){
ryuna 0:010fe1eae7d2 199 num[Ping] = 0;
ryuna 1:3ecb80796a7f 200 putchar('\n');
ryuna 0:010fe1eae7d2 201 }
ryuna 0:010fe1eae7d2 202 if(num[Ir]>= ALL_IR){
ryuna 0:010fe1eae7d2 203 num[Ir] = 0;
ryuna 0:010fe1eae7d2 204 putchar('\n');
ryuna 1:3ecb80796a7f 205 pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num);
ryuna 0:010fe1eae7d2 206 }
ryuna 0:010fe1eae7d2 207 if(num[Line]>= ALL_LINE){
ryuna 0:010fe1eae7d2 208 num[Line] = 0;
ryuna 1:3ecb80796a7f 209 putchar('\n');
ryuna 0:010fe1eae7d2 210 }
ryuna 0:010fe1eae7d2 211
ryuna 0:010fe1eae7d2 212 }
ryuna 0:010fe1eae7d2 213 }