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@0:010fe1eae7d2, 2014-02-22 (annotated)

Committer:
ryuna
Date:
Sat Feb 22 07:05:09 2014 +0000
Revision:
0:010fe1eae7d2
Child:
1:3ecb80796a7f
Program_program_BlockPIL

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:010fe1eae7d2 1 #include "mbed.h"
ryuna 0:010fe1eae7d2 2 /*PING DEFINE */
ryuna 0:010fe1eae7d2 3 #define ERROR_PING 0xFFF
ryuna 0:010fe1eae7d2 4 #define ALL_PING 4
ryuna 0:010fe1eae7d2 5 /*IR DEFINE*/
ryuna 0:010fe1eae7d2 6 #define BREAKPT_IR 833
ryuna 0:010fe1eae7d2 7 #define ERROR_IR 0xFFF
ryuna 0:010fe1eae7d2 8 #define ALL_IR 10
ryuna 0:010fe1eae7d2 9 //#define OVERLINE_IR 46
ryuna 0:010fe1eae7d2 10 /*LINE DEFINE*/
ryuna 0:010fe1eae7d2 11 #define JUDGEVOL_LINE 0.8
ryuna 0:010fe1eae7d2 12 #define ALL_LINE 4
ryuna 0:010fe1eae7d2 13 //OTHERS
ryuna 0:010fe1eae7d2 14 #define ALL_KIND 3 //kind of sensors
ryuna 0:010fe1eae7d2 15 #define MAX_NUM 10 //sensors most number
ryuna 0:010fe1eae7d2 16 /*
ryuna 0:010fe1eae7d2 17 Ping , IrSensor , LineSensor
ryuna 0:010fe1eae7d2 18 */
ryuna 0:010fe1eae7d2 19
ryuna 0:010fe1eae7d2 20 //LINE sensor is float value
ryuna 0:010fe1eae7d2 21 Serial pc(USBTX,USBRX);
ryuna 0:010fe1eae7d2 22 Timer time_ping;
ryuna 0:010fe1eae7d2 23 Timer time_ir;
ryuna 0:010fe1eae7d2 24
ryuna 0:010fe1eae7d2 25 PinName num_ping[ALL_PING] = {p5,p6,p7,p8};
ryuna 0:010fe1eae7d2 26 PinName num_ir[ALL_IR] = {p21,p22,p23,p24,p25,p26,p27,p28,p29,p30};
ryuna 0:010fe1eae7d2 27 PinName num_line[ALL_LINE] = {p17,p18,p19,p20};
ryuna 0:010fe1eae7d2 28
ryuna 0:010fe1eae7d2 29 enum sensors{Ping,Ir,Line};
ryuna 0:010fe1eae7d2 30 volatile unsigned int value_ping[ALL_PING] = {0};
ryuna 0:010fe1eae7d2 31 volatile unsigned int value_ir[ALL_IR] = {0};
ryuna 0:010fe1eae7d2 32 volatile float value_line[ALL_LINE] = {0};
ryuna 0:010fe1eae7d2 33 DigitalOut myled(LED1);
ryuna 0:010fe1eae7d2 34
ryuna 0:010fe1eae7d2 35 unsigned int moving_ave(enum sensors kind,int num, unsigned int data){
ryuna 0:010fe1eae7d2 36 static unsigned int sum[ALL_KIND][MAX_NUM] = {0};
ryuna 0:010fe1eae7d2 37 static unsigned int temp[ALL_KIND][MAX_NUM][10] = {0};
ryuna 0:010fe1eae7d2 38 //static unsigned int ave[MAX_NUM] = {0};
ryuna 0:010fe1eae7d2 39 sum[kind][num] -= temp[kind][num][9];
ryuna 0:010fe1eae7d2 40 sum[kind][num] += data;
ryuna 0:010fe1eae7d2 41 temp[kind][num][9] = temp[kind][num][8];
ryuna 0:010fe1eae7d2 42 temp[kind][num][8] = temp[kind][num][7];
ryuna 0:010fe1eae7d2 43 temp[kind][num][7] = temp[kind][num][6];
ryuna 0:010fe1eae7d2 44 temp[kind][num][6] = temp[kind][num][5];
ryuna 0:010fe1eae7d2 45 temp[kind][num][5] = temp[kind][num][4];
ryuna 0:010fe1eae7d2 46 temp[kind][num][4] = temp[kind][num][3];
ryuna 0:010fe1eae7d2 47 temp[kind][num][3] = temp[kind][num][2];
ryuna 0:010fe1eae7d2 48 temp[kind][num][2] = temp[kind][num][1];
ryuna 0:010fe1eae7d2 49 temp[kind][num][1] = temp[kind][num][0];
ryuna 0:010fe1eae7d2 50 temp[kind][num][0] = data;
ryuna 0:010fe1eae7d2 51 //ave[kind][num] = sum[kind][num]/100;
ryuna 0:010fe1eae7d2 52
ryuna 0:010fe1eae7d2 53 switch (kind) {
ryuna 0:010fe1eae7d2 54 case Ping:
ryuna 0:010fe1eae7d2 55 value_ping[num] = sum[kind][num]/10;
ryuna 0:010fe1eae7d2 56 break;
ryuna 0:010fe1eae7d2 57 case Ir:
ryuna 0:010fe1eae7d2 58 value_ir[num] = sum[kind][num]/100;
ryuna 0:010fe1eae7d2 59 break;
ryuna 0:010fe1eae7d2 60 case Line:
ryuna 0:010fe1eae7d2 61 value_line[num] = sum[kind][num]/10000.0;
ryuna 0:010fe1eae7d2 62 break;
ryuna 0:010fe1eae7d2 63 default :
ryuna 0:010fe1eae7d2 64 break;
ryuna 0:010fe1eae7d2 65 }
ryuna 0:010fe1eae7d2 66
ryuna 0:010fe1eae7d2 67 return sum[kind][num];//return no config sum
ryuna 0:010fe1eae7d2 68
ryuna 0:010fe1eae7d2 69 }
ryuna 0:010fe1eae7d2 70 unsigned int ping_function (int num){
ryuna 0:010fe1eae7d2 71 DigitalInOut sensor_ping(num_ping[num]);
ryuna 0:010fe1eae7d2 72 unsigned int memory = 0;//return value
ryuna 0:010fe1eae7d2 73 time_ping.reset();
ryuna 0:010fe1eae7d2 74 /*make a pulse */
ryuna 0:010fe1eae7d2 75 sensor_ping.output();
ryuna 0:010fe1eae7d2 76 sensor_ping = 1;
ryuna 0:010fe1eae7d2 77 wait_us(5);
ryuna 0:010fe1eae7d2 78 sensor_ping = 0;
ryuna 0:010fe1eae7d2 79 sensor_ping.input();
ryuna 0:010fe1eae7d2 80 /*finish,and start timer*/
ryuna 0:010fe1eae7d2 81 time_ping.start();
ryuna 0:010fe1eae7d2 82 while(!sensor_ping){
ryuna 0:010fe1eae7d2 83 if(time_ping.read_ms() > 1){
ryuna 0:010fe1eae7d2 84 time_ping.stop();
ryuna 0:010fe1eae7d2 85 return ERROR_PING;
ryuna 0:010fe1eae7d2 86 }
ryuna 0:010fe1eae7d2 87 }
ryuna 0:010fe1eae7d2 88 time_ping.reset();
ryuna 0:010fe1eae7d2 89 while(sensor_ping){
ryuna 0:010fe1eae7d2 90 if(time_ping.read_ms() >18.5){
ryuna 0:010fe1eae7d2 91 time_ping.stop();
ryuna 0:010fe1eae7d2 92 return ERROR_PING;
ryuna 0:010fe1eae7d2 93 }
ryuna 0:010fe1eae7d2 94 }
ryuna 0:010fe1eae7d2 95 memory = time_ping.read_us();
ryuna 0:010fe1eae7d2 96 time_ping.stop();
ryuna 0:010fe1eae7d2 97
ryuna 0:010fe1eae7d2 98 return memory;
ryuna 0:010fe1eae7d2 99 }
ryuna 0:010fe1eae7d2 100 unsigned int ir_function (int num){
ryuna 0:010fe1eae7d2 101 DigitalIn sensor_ir(num_ir[num]);
ryuna 0:010fe1eae7d2 102 int flag = 0;
ryuna 0:010fe1eae7d2 103 unsigned int memory = 0;
ryuna 0:010fe1eae7d2 104 unsigned int temp = 0;
ryuna 0:010fe1eae7d2 105 flag = 1;
ryuna 0:010fe1eae7d2 106 time_ir.start();
ryuna 0:010fe1eae7d2 107 if((sensor_ir)){
ryuna 0:010fe1eae7d2 108 while((sensor_ir)){
ryuna 0:010fe1eae7d2 109 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 110 flag = 0;
ryuna 0:010fe1eae7d2 111 break;
ryuna 0:010fe1eae7d2 112 }
ryuna 0:010fe1eae7d2 113 }
ryuna 0:010fe1eae7d2 114 }
ryuna 0:010fe1eae7d2 115 time_ir.stop();
ryuna 0:010fe1eae7d2 116 time_ir.reset();
ryuna 0:010fe1eae7d2 117 if(flag){
ryuna 0:010fe1eae7d2 118 time_ir.start();
ryuna 0:010fe1eae7d2 119 while(!sensor_ir){//!
ryuna 0:010fe1eae7d2 120 if(time_ir.read_us() >= BREAKPT_IR){
ryuna 0:010fe1eae7d2 121 break;
ryuna 0:010fe1eae7d2 122 }
ryuna 0:010fe1eae7d2 123 }
ryuna 0:010fe1eae7d2 124 memory = time_ir.read_us();
ryuna 0:010fe1eae7d2 125 while(1){
ryuna 0:010fe1eae7d2 126 if(!sensor_ir){
ryuna 0:010fe1eae7d2 127 temp = (time_ir.read_us() - memory);
ryuna 0:010fe1eae7d2 128 time_ir.stop();
ryuna 0:010fe1eae7d2 129 time_ir.reset();
ryuna 0:010fe1eae7d2 130 wait(0.01);
ryuna 0:010fe1eae7d2 131 return temp;
ryuna 0:010fe1eae7d2 132 }
ryuna 0:010fe1eae7d2 133 }
ryuna 0:010fe1eae7d2 134 }else{//not found
ryuna 0:010fe1eae7d2 135 }
ryuna 0:010fe1eae7d2 136 time_ir.stop();
ryuna 0:010fe1eae7d2 137 time_ir.reset();
ryuna 0:010fe1eae7d2 138 return ERROR_IR;
ryuna 0:010fe1eae7d2 139 }
ryuna 0:010fe1eae7d2 140 int line_function(int num){
ryuna 0:010fe1eae7d2 141 float memory = 0;
ryuna 0:010fe1eae7d2 142 AnalogIn sensor_line(num_line[num]);
ryuna 0:010fe1eae7d2 143 memory = sensor_line;
ryuna 0:010fe1eae7d2 144 return (int)(memory*1000);
ryuna 0:010fe1eae7d2 145
ryuna 0:010fe1eae7d2 146 }
ryuna 0:010fe1eae7d2 147 int main() {
ryuna 0:010fe1eae7d2 148 //enum sensors kinds;
ryuna 0:010fe1eae7d2 149 int num[ALL_KIND] = {0};
ryuna 0:010fe1eae7d2 150 unsigned int value1[ALL_PING] = {0};
ryuna 0:010fe1eae7d2 151 unsigned int value2[ALL_IR] = {0};
ryuna 0:010fe1eae7d2 152 float value3[ALL_LINE] = {0};
ryuna 0:010fe1eae7d2 153 while(1) {
ryuna 0:010fe1eae7d2 154 //value1[num[Ping]] = moving_ave(Ping,num[Ping],ping_function(num[Ping]))/10;
ryuna 0:010fe1eae7d2 155 value2[num[Ir]] = moving_ave(Ir,num[Ir],ir_function(num[Ir]))/100;
ryuna 0:010fe1eae7d2 156 //value3[num[Line]]= moving_ave(Line,num[Line],line_function(num[Line]))/10000.0;
ryuna 0:010fe1eae7d2 157 //pc.printf("value1[%d]=%d ",num[Ping], value1[num[Ping]]);
ryuna 0:010fe1eae7d2 158 pc.printf("value2[%d]=%d ",num[Ir], value2[num[Ir]]);
ryuna 0:010fe1eae7d2 159 //pc.printf("value3[%d]=%lf ",num[Line], value3[num[Line]]);
ryuna 0:010fe1eae7d2 160 num[Ping]++;
ryuna 0:010fe1eae7d2 161 num[Ir]++;
ryuna 0:010fe1eae7d2 162 num[Line]++;
ryuna 0:010fe1eae7d2 163 if(num[Ping]>= ALL_PING){
ryuna 0:010fe1eae7d2 164 num[Ping] = 0;
ryuna 0:010fe1eae7d2 165 //putchar('\n');
ryuna 0:010fe1eae7d2 166 }
ryuna 0:010fe1eae7d2 167 if(num[Ir]>= ALL_IR){
ryuna 0:010fe1eae7d2 168 num[Ir] = 0;
ryuna 0:010fe1eae7d2 169 putchar('\n');
ryuna 0:010fe1eae7d2 170 }
ryuna 0:010fe1eae7d2 171 if(num[Line]>= ALL_LINE){
ryuna 0:010fe1eae7d2 172 num[Line] = 0;
ryuna 0:010fe1eae7d2 173 //putchar('\n');
ryuna 0:010fe1eae7d2 174 }
ryuna 0:010fe1eae7d2 175
ryuna 0:010fe1eae7d2 176 }
ryuna 0:010fe1eae7d2 177 }