Fumiya Fujisawa
/
program_BlockPIL
program_BlockPIL //{Ping,Ir,Lines} Sensor
main.cpp@1:3ecb80796a7f, 2014-02-24 (annotated)
- Committer:
- ryuna
- Date:
- Mon Feb 24 05:20:20 2014 +0000
- Revision:
- 1:3ecb80796a7f
- Parent:
- 0:010fe1eae7d2
program_Sensors value
Who changed what in which revision?
User | Revision | Line number | New 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 | //unsigned int value1[ALL_PING] = {0}; |
ryuna | 1:3ecb80796a7f | 175 | //unsigned int value2[ALL_IR] = {0}; |
ryuna | 1:3ecb80796a7f | 176 | //float value3[ALL_LINE] = {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 | 1:3ecb80796a7f | 185 | value_compass = ((compass.sample() / 10) + 540 - compassdef) % 360; |
ryuna | 1:3ecb80796a7f | 186 | moving_ave(Ping,num[Ping],ping_function(num[Ping])); |
ryuna | 1:3ecb80796a7f | 187 | |
ryuna | 1:3ecb80796a7f | 188 | pc.printf("value_compass=%d\n",value_compass); |
ryuna | 1:3ecb80796a7f | 189 | //ir_min_fun(); |
ryuna | 1:3ecb80796a7f | 190 | //num[Ping]++; |
ryuna | 1:3ecb80796a7f | 191 | //num[Ir]++; |
ryuna | 1:3ecb80796a7f | 192 | //num[Line]++; |
ryuna | 0:010fe1eae7d2 | 193 | if(num[Ping]>= ALL_PING){ |
ryuna | 0:010fe1eae7d2 | 194 | num[Ping] = 0; |
ryuna | 1:3ecb80796a7f | 195 | putchar('\n'); |
ryuna | 0:010fe1eae7d2 | 196 | } |
ryuna | 0:010fe1eae7d2 | 197 | if(num[Ir]>= ALL_IR){ |
ryuna | 0:010fe1eae7d2 | 198 | num[Ir] = 0; |
ryuna | 0:010fe1eae7d2 | 199 | putchar('\n'); |
ryuna | 1:3ecb80796a7f | 200 | pc.printf("min=%d ,num = %d\n",value_ir_min,ir_min_num); |
ryuna | 0:010fe1eae7d2 | 201 | } |
ryuna | 0:010fe1eae7d2 | 202 | if(num[Line]>= ALL_LINE){ |
ryuna | 0:010fe1eae7d2 | 203 | num[Line] = 0; |
ryuna | 1:3ecb80796a7f | 204 | putchar('\n'); |
ryuna | 0:010fe1eae7d2 | 205 | } |
ryuna | 0:010fe1eae7d2 | 206 | |
ryuna | 0:010fe1eae7d2 | 207 | } |
ryuna | 0:010fe1eae7d2 | 208 | } |