CatPot 2015-2016 / CatPot_2v10_T_Sub

ジャパンオープン用のLPC824専用プログラム

Dependencies:   mbed-src Ping SDFileSystem

main/format.cpp@8:7b9d70751bdf, 2016-03-27 (annotated)

Committer:
lilac0112_1
Date:
Sun Mar 27 13:05:30 2016 +0000
Revision:
8:7b9d70751bdf
Parent:
7:22d0c4a400c4 
JapanSoccerOpen2016 CatPot program(sub)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:6c79a352010b 1 #include "mbed.h"
lilac0112_1 0:6c79a352010b 2 #include "extern.h"
lilac0112_1 0:6c79a352010b 3
lilac0112_1 0:6c79a352010b 4 #if defined(ULTRA_SONIC) || defined(ULTRA_SONIC_2)
lilac0112_1 0:6c79a352010b 5
lilac0112_1 0:6c79a352010b 6 uint16_t packet, val, order;
lilac0112_1 0:6c79a352010b 7 uint16_t front_dis=0, rear_dis=0;
lilac0112_1 0:6c79a352010b 8
lilac0112_1 0:6c79a352010b 9 void emergency(void){
lilac0112_1 0:6c79a352010b 10 led[1]=1;
lilac0112_1 0:6c79a352010b 11 //front_dis = 0xAA;
lilac0112_1 0:6c79a352010b 12 //rear_dis = 0xBB;
lilac0112_1 0:6c79a352010b 13 val = nucleo.receive();
lilac0112_1 0:6c79a352010b 14 if(val==1){
lilac0112_1 0:6c79a352010b 15 order = nucleo.read();
lilac0112_1 0:6c79a352010b 16 if(order == 0xABCD){
lilac0112_1 0:6c79a352010b 17 packet = ((front_dis<<8)|(rear_dis))&0xFFFF;nucleo.reply(packet);
lilac0112_1 0:6c79a352010b 18 }
lilac0112_1 0:6c79a352010b 19 else{
lilac0112_1 0:6c79a352010b 20 packet = 0xFFFF;nucleo.reply(packet);
lilac0112_1 0:6c79a352010b 21 }
lilac0112_1 0:6c79a352010b 22 //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order);
lilac0112_1 0:6c79a352010b 23 }
lilac0112_1 0:6c79a352010b 24 else{
lilac0112_1 0:6c79a352010b 25 //pc.printf("FFFF\r\n");
lilac0112_1 0:6c79a352010b 26 }
lilac0112_1 0:6c79a352010b 27 wait_us(100);
lilac0112_1 0:6c79a352010b 28 led[1]=0;
lilac0112_1 0:6c79a352010b 29 }
lilac0112_1 0:6c79a352010b 30 void Usw_System(void){
lilac0112_1 0:6c79a352010b 31
lilac0112_1 0:6c79a352010b 32 nucleo.format(16, 3);
lilac0112_1 0:6c79a352010b 33 nucleo.frequency(1000000);
lilac0112_1 0:6c79a352010b 34 nucleo.reply(0x0000);
lilac0112_1 0:6c79a352010b 35
lilac0112_1 0:6c79a352010b 36 call.fall(&emergency);
lilac0112_1 0:6c79a352010b 37
lilac0112_1 0:6c79a352010b 38 //led[0]=led[1]=1;
lilac0112_1 0:6c79a352010b 39 while(1){
lilac0112_1 0:6c79a352010b 40
lilac0112_1 0:6c79a352010b 41 led[0]=1;
lilac0112_1 0:6c79a352010b 42 front.Send();
lilac0112_1 0:6c79a352010b 43 wait_ms(30);
lilac0112_1 0:6c79a352010b 44 front_dis = front.Read_cm();
lilac0112_1 0:6c79a352010b 45 if(front_dis>0xFE) front_dis=0xFE;
lilac0112_1 0:6c79a352010b 46
lilac0112_1 0:6c79a352010b 47
lilac0112_1 0:6c79a352010b 48 rear.Send();
lilac0112_1 0:6c79a352010b 49 wait_ms(30);
lilac0112_1 0:6c79a352010b 50 rear_dis = rear.Read_cm();
lilac0112_1 0:6c79a352010b 51 if(rear_dis>0xFE) rear_dis=0xFE;
lilac0112_1 0:6c79a352010b 52 led[0]=0;
lilac0112_1 0:6c79a352010b 53 wait_us(1000);
lilac0112_1 0:6c79a352010b 54 //pc.printf("%d\t%d\t\r\n", front_dis, rear_dis);
lilac0112_1 0:6c79a352010b 55
lilac0112_1 0:6c79a352010b 56 //emergency();
lilac0112_1 0:6c79a352010b 57 }
lilac0112_1 0:6c79a352010b 58 }
lilac0112_1 0:6c79a352010b 59 #endif /*(ULTRA_SONIC) || (ULTRA_SONIC_2)*/
lilac0112_1 0:6c79a352010b 60
lilac0112_1 0:6c79a352010b 61
lilac0112_1 0:6c79a352010b 62 #ifdef IR_SENSOR
lilac0112_1 0:6c79a352010b 63
lilac0112_1 0:6c79a352010b 64 uint8_t ir_notice=0;//4bit
lilac0112_1 1:ea7307724f1c 65 uint8_t ir_val_phase[2];
lilac0112_1 5:d666b328c093 66 uint8_t ir_dif_phase[2];
lilac0112_1 5:d666b328c093 67 uint16_t ir_val_diff[2];
lilac0112_1 5:d666b328c093 68 uint8_t ir_place;//0<=x<=20
lilac0112_1 0:6c79a352010b 69 static uint8_t const ir_key=IR_KEY;//4bit
lilac0112_1 0:6c79a352010b 70 uint8_t ir_position[2]={0};//4bit*2
lilac0112_1 0:6c79a352010b 71
lilac0112_1 0:6c79a352010b 72 void emergency(void){
lilac0112_1 1:ea7307724f1c 73
lilac0112_1 0:6c79a352010b 74 uint16_t order,packet,val;
lilac0112_1 0:6c79a352010b 75 //ir_key=0xA;
lilac0112_1 0:6c79a352010b 76 //ir_notice=0xC;
lilac0112_1 0:6c79a352010b 77 //ir_position[1]=0xE;
lilac0112_1 0:6c79a352010b 78 //ir_position[0]=0xF;
lilac0112_1 0:6c79a352010b 79 val = nucleo.receive();
lilac0112_1 0:6c79a352010b 80 if(val==1){
lilac0112_1 0:6c79a352010b 81 order = nucleo.read();
lilac0112_1 0:6c79a352010b 82 if(order == 0xABCD){
lilac0112_1 5:d666b328c093 83 //key/phaseL/phaseS/long_diff/position/
lilac0112_1 5:d666b328c093 84 // 2/ 3/ 3/ 3/ 5/
lilac0112_1 5:d666b328c093 85 packet = (
lilac0112_1 5:d666b328c093 86 ((0xC000)&(ir_key<<14)) |//1100000000000000
lilac0112_1 5:d666b328c093 87 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000
lilac0112_1 5:d666b328c093 88 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000
lilac0112_1 6:80c2f4fce2eb 89 ((0x00E0)&(ir_dif_phase[IR_LONG]<<5)) |//0000000011100000
lilac0112_1 5:d666b328c093 90 ((0x001F)&(ir_place<<0)) //0000000000011111
lilac0112_1 5:d666b328c093 91 )&0xFFFF;
lilac0112_1 5:d666b328c093 92
lilac0112_1 5:d666b328c093 93 nucleo.reply(packet);
lilac0112_1 5:d666b328c093 94 }
lilac0112_1 5:d666b328c093 95 else if(order == 0xBCDA){
lilac0112_1 1:ea7307724f1c 96 //key/phaseL/phaseS/long/short/
lilac0112_1 1:ea7307724f1c 97 // 2/ 3/ 3/ 4/ 4/
lilac0112_1 1:ea7307724f1c 98 packet = (
lilac0112_1 1:ea7307724f1c 99 ((0xC000)&(ir_key<<14)) |//1100000000000000
lilac0112_1 1:ea7307724f1c 100 ((0x3800)&(ir_val_phase[IR_LONG]<<11)) |//0011100000000000
lilac0112_1 1:ea7307724f1c 101 ((0x0700)&(ir_val_phase[IR_SHORT]<<8)) |//0000011100000000
lilac0112_1 1:ea7307724f1c 102 ((0x00F0)&(ir_position[IR_LONG]<<4)) |//0000000011110000
lilac0112_1 1:ea7307724f1c 103 ((0x000F)&(ir_position[IR_SHORT]<<0)) //0000000000001111
lilac0112_1 1:ea7307724f1c 104 )&0xFFFF;
lilac0112_1 1:ea7307724f1c 105
lilac0112_1 1:ea7307724f1c 106 nucleo.reply(packet);
lilac0112_1 0:6c79a352010b 107 }
lilac0112_1 0:6c79a352010b 108 else{
lilac0112_1 0:6c79a352010b 109 packet = 0xFFFF;nucleo.reply(packet);
lilac0112_1 0:6c79a352010b 110 }
lilac0112_1 0:6c79a352010b 111 //pc.printf("SSSSSSSSSSSSSSSSS%d\r\n", order);
lilac0112_1 0:6c79a352010b 112 }
lilac0112_1 0:6c79a352010b 113 else{
lilac0112_1 0:6c79a352010b 114 //pc.f("FFFF\r\n");
lilac0112_1 0:6c79a352010b 115 }
lilac0112_1 0:6c79a352010b 116 }
lilac0112_1 0:6c79a352010b 117 void Ir_System(void){
lilac0112_1 0:6c79a352010b 118
lilac0112_1 0:6c79a352010b 119 uint8_t i;//ic
lilac0112_1 0:6c79a352010b 120 uint8_t j;//ch
lilac0112_1 0:6c79a352010b 121
lilac0112_1 0:6c79a352010b 122 uint8_t num_long;
lilac0112_1 0:6c79a352010b 123 uint8_t num_short;
lilac0112_1 0:6c79a352010b 124
lilac0112_1 0:6c79a352010b 125 uint16_t ir_val_long[8];
lilac0112_1 0:6c79a352010b 126 uint16_t ir_val_short[12];
lilac0112_1 0:6c79a352010b 127
lilac0112_1 5:d666b328c093 128 uint16_t ir_val;
lilac0112_1 0:6c79a352010b 129
lilac0112_1 0:6c79a352010b 130 uint8_t ir_posi;
lilac0112_1 0:6c79a352010b 131 uint8_t ir_posi_long[8];
lilac0112_1 0:6c79a352010b 132 uint8_t ir_posi_short[12];
lilac0112_1 5:d666b328c093 133
lilac0112_1 5:d666b328c093 134 uint16_t ave_buff_long[LONG_AVE];
lilac0112_1 5:d666b328c093 135 uint16_t ave_buff_long2[LONG_AVE];
lilac0112_1 5:d666b328c093 136 uint32_t ave_sum_long;
lilac0112_1 5:d666b328c093 137 uint32_t ave_sum_long2;
lilac0112_1 7:22d0c4a400c4 138 //uint16_t ir_val_long_plus;
lilac0112_1 7:22d0c4a400c4 139 //uint16_t ir_val_long_plus2;
lilac0112_1 5:d666b328c093 140
lilac0112_1 5:d666b328c093 141
lilac0112_1 0:6c79a352010b 142 static uint8_t const ch_num[3]={8, 8, 4};
lilac0112_1 0:6c79a352010b 143
lilac0112_1 1:ea7307724f1c 144 static uint16_t const ir_dis_range[2][8]={
lilac0112_1 7:22d0c4a400c4 145 {0, 2000, 2250, 2500, 2600, 2700, 3000, 3500, },//IR_SHORT 0
lilac0112_1 7:22d0c4a400c4 146 {0, 1000, 1100, 1200, 1300, 1500, 1700, 1900, },//IR_LONG 1
lilac0112_1 5:d666b328c093 147 };
lilac0112_1 5:d666b328c093 148
lilac0112_1 5:d666b328c093 149
lilac0112_1 5:d666b328c093 150 static uint16_t const ir_dif_range[2][8]={
lilac0112_1 6:80c2f4fce2eb 151 {0, 500, 750, 1000, 1250, 1500, 1600, 1800, },//IR_SHORT 0
lilac0112_1 5:d666b328c093 152 {0, 500, 600, 650, 700, 800, 900, 1000, },//IR_LONG 1
lilac0112_1 3:8ef3afcf4ade 153 };
lilac0112_1 1:ea7307724f1c 154
lilac0112_1 0:6c79a352010b 155 nucleo.format(16, 3);
lilac0112_1 0:6c79a352010b 156 nucleo.frequency(1000000);
lilac0112_1 0:6c79a352010b 157 nucleo.reply(0x0000);
lilac0112_1 0:6c79a352010b 158
lilac0112_1 0:6c79a352010b 159 ir.format(8, 3);
lilac0112_1 0:6c79a352010b 160 ir.frequency(1000000);
lilac0112_1 0:6c79a352010b 161
lilac0112_1 0:6c79a352010b 162 //遠距離用電源
lilac0112_1 0:6c79a352010b 163 supply.period(0.010);//T=10[ms]
lilac0112_1 0:6c79a352010b 164 supply.write(0.9);//L...10%,H...90%
lilac0112_1 0:6c79a352010b 165
lilac0112_1 0:6c79a352010b 166 //Nucleoとの通信用のピン変化割り込み
lilac0112_1 0:6c79a352010b 167 //Nucleoとの通信をする際はTeraTermへの出力は控えるべき
lilac0112_1 0:6c79a352010b 168 call.fall(&emergency);
lilac0112_1 0:6c79a352010b 169
lilac0112_1 5:d666b328c093 170 for(i=0,ave_sum_long =2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long[i]=2100;
lilac0112_1 5:d666b328c093 171 for(i=0,ave_sum_long2=2100*LONG_AVE; i<LONG_AVE; i++) ave_buff_long2[i]=2100;
lilac0112_1 5:d666b328c093 172
lilac0112_1 0:6c79a352010b 173 for(i=0; i<IC_NUM; i++) cs[i]=1;
lilac0112_1 2:83b8bf9f0ea1 174
lilac0112_1 2:83b8bf9f0ea1 175 ir_val_phase[IR_SHORT] = DIS_7;
lilac0112_1 2:83b8bf9f0ea1 176 ir_val_phase[IR_LONG] = DIS_7;
lilac0112_1 0:6c79a352010b 177 while(1) {
lilac0112_1 0:6c79a352010b 178 //全ての素子の値を検出
lilac0112_1 0:6c79a352010b 179 num_short=0;
lilac0112_1 0:6c79a352010b 180 num_long=0;
lilac0112_1 0:6c79a352010b 181 for(i=0; i<IC_NUM; i++){//IC
lilac0112_1 0:6c79a352010b 182 for(j=0; j<ch_num[i]; j++){//Ch
lilac0112_1 0:6c79a352010b 183 ir_val = read_input(i, j);
lilac0112_1 0:6c79a352010b 184 ir_posi = read_position(i, j);
lilac0112_1 0:6c79a352010b 185
lilac0112_1 0:6c79a352010b 186 if(i==0){
lilac0112_1 0:6c79a352010b 187 ir_val_long[num_long] = ir_val;
lilac0112_1 0:6c79a352010b 188 ir_posi_long[num_long] = ir_posi;
lilac0112_1 0:6c79a352010b 189 num_long++;
lilac0112_1 0:6c79a352010b 190
lilac0112_1 0:6c79a352010b 191 }
lilac0112_1 0:6c79a352010b 192 else{
lilac0112_1 0:6c79a352010b 193 ir_val_short[num_short] = ir_val;
lilac0112_1 0:6c79a352010b 194 ir_posi_short[num_short] = ir_posi;
lilac0112_1 0:6c79a352010b 195 num_short++;
lilac0112_1 0:6c79a352010b 196 }
lilac0112_1 0:6c79a352010b 197
lilac0112_1 0:6c79a352010b 198 }
lilac0112_1 0:6c79a352010b 199 }
lilac0112_1 0:6c79a352010b 200 //バブルソート
lilac0112_1 0:6c79a352010b 201 BubbleSort(ir_val_short, ir_posi_short, 12);
lilac0112_1 0:6c79a352010b 202 BubbleSort(ir_val_long, ir_posi_long, 8);
lilac0112_1 0:6c79a352010b 203
lilac0112_1 5:d666b328c093 204 //移動平均
lilac0112_1 5:d666b328c093 205 ave_sum_long += ir_val_long[7];
lilac0112_1 5:d666b328c093 206 ave_sum_long -= ave_buff_long[LONG_AVE-1];
lilac0112_1 5:d666b328c093 207 for(i=LONG_AVE-1; i>0; i--){
lilac0112_1 5:d666b328c093 208 ave_buff_long[i] = ave_buff_long[i-1];
lilac0112_1 5:d666b328c093 209 }
lilac0112_1 5:d666b328c093 210 ave_buff_long[0] = ir_val_long[7];
lilac0112_1 5:d666b328c093 211
lilac0112_1 7:22d0c4a400c4 212 //ir_val_long_plus = ir_val_long[7];
lilac0112_1 5:d666b328c093 213 ir_val_long[7] = ave_sum_long/LONG_AVE;
lilac0112_1 5:d666b328c093 214
lilac0112_1 5:d666b328c093 215 //移動平均2
lilac0112_1 5:d666b328c093 216 ave_sum_long2 += ir_val_long[0];
lilac0112_1 5:d666b328c093 217 ave_sum_long2 -= ave_buff_long2[LONG_AVE-1];
lilac0112_1 5:d666b328c093 218 for(i=LONG_AVE-1; i>0; i--){
lilac0112_1 5:d666b328c093 219 ave_buff_long2[i] = ave_buff_long2[i-1];
lilac0112_1 5:d666b328c093 220 }
lilac0112_1 5:d666b328c093 221 ave_buff_long2[0] = ir_val_long[0];
lilac0112_1 5:d666b328c093 222
lilac0112_1 7:22d0c4a400c4 223 //ir_val_long_plus2 = ir_val_long[0];
lilac0112_1 5:d666b328c093 224 ir_val_long[0] = ave_sum_long2/LONG_AVE;
lilac0112_1 5:d666b328c093 225
lilac0112_1 5:d666b328c093 226
lilac0112_1 0:6c79a352010b 227 //検出した最大値と最小値の差
lilac0112_1 5:d666b328c093 228 ir_val_diff[IR_SHORT] = ir_val_short[0]-ir_val_short[11];//After bubble
lilac0112_1 5:d666b328c093 229 ir_val_diff[IR_LONG] = abs(ir_val_long[0]-ir_val_long[7]);//After bubble
lilac0112_1 0:6c79a352010b 230 //最もボールがあるとされる位置を代入
lilac0112_1 1:ea7307724f1c 231 ir_position[IR_SHORT] = ir_posi_short[11];//After bubble
lilac0112_1 1:ea7307724f1c 232 ir_position[IR_LONG] = ir_posi_long[7];//After bubble
lilac0112_1 0:6c79a352010b 233
lilac0112_1 0:6c79a352010b 234 /*
lilac0112_1 0:6c79a352010b 235 #define IR_NOTE_NONE 0x0
lilac0112_1 0:6c79a352010b 236 #define IR_NOTE_FAR 0x1
lilac0112_1 0:6c79a352010b 237 #define IR_NOTE_CLOSE 0x2
lilac0112_1 0:6c79a352010b 238 #define IR_NOTE_CLOSER 0x3
lilac0112_1 0:6c79a352010b 239 */
lilac0112_1 1:ea7307724f1c 240 //ボールが遠くにあるか，近くか，フィールドにないかを判断（8段階）
lilac0112_1 1:ea7307724f1c 241
lilac0112_1 1:ea7307724f1c 242
lilac0112_1 1:ea7307724f1c 243 //short
lilac0112_1 1:ea7307724f1c 244
lilac0112_1 1:ea7307724f1c 245 if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_7]){
lilac0112_1 1:ea7307724f1c 246 ir_val_phase[IR_SHORT] = DIS_7;
lilac0112_1 1:ea7307724f1c 247 }
lilac0112_1 1:ea7307724f1c 248 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_6]){
lilac0112_1 1:ea7307724f1c 249 ir_val_phase[IR_SHORT] = DIS_6;
lilac0112_1 1:ea7307724f1c 250 }
lilac0112_1 1:ea7307724f1c 251 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_5]){
lilac0112_1 1:ea7307724f1c 252 ir_val_phase[IR_SHORT] = DIS_5;
lilac0112_1 1:ea7307724f1c 253 }
lilac0112_1 1:ea7307724f1c 254 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_4]){
lilac0112_1 1:ea7307724f1c 255 ir_val_phase[IR_SHORT] = DIS_4;
lilac0112_1 1:ea7307724f1c 256 }
lilac0112_1 1:ea7307724f1c 257 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_3]){
lilac0112_1 1:ea7307724f1c 258 ir_val_phase[IR_SHORT] = DIS_3;
lilac0112_1 1:ea7307724f1c 259 }
lilac0112_1 1:ea7307724f1c 260 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_2]){
lilac0112_1 1:ea7307724f1c 261 ir_val_phase[IR_SHORT] = DIS_2;
lilac0112_1 1:ea7307724f1c 262 }
lilac0112_1 1:ea7307724f1c 263 else if(ir_val_short[11]>=ir_dis_range[IR_SHORT][DIS_1]){
lilac0112_1 1:ea7307724f1c 264 ir_val_phase[IR_SHORT] = DIS_1;
lilac0112_1 1:ea7307724f1c 265 }
lilac0112_1 1:ea7307724f1c 266 else{//ir_val_short[11]>=0
lilac0112_1 1:ea7307724f1c 267 ir_val_phase[IR_SHORT] = DIS_0;
lilac0112_1 1:ea7307724f1c 268 }
lilac0112_1 1:ea7307724f1c 269
lilac0112_1 1:ea7307724f1c 270 //long
lilac0112_1 1:ea7307724f1c 271
lilac0112_1 1:ea7307724f1c 272 if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_7]){
lilac0112_1 1:ea7307724f1c 273 ir_val_phase[IR_LONG] = DIS_7;
lilac0112_1 1:ea7307724f1c 274 }
lilac0112_1 1:ea7307724f1c 275 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_6]){
lilac0112_1 1:ea7307724f1c 276 ir_val_phase[IR_LONG] = DIS_6;
lilac0112_1 1:ea7307724f1c 277 }
lilac0112_1 1:ea7307724f1c 278 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_5]){
lilac0112_1 1:ea7307724f1c 279 ir_val_phase[IR_LONG] = DIS_5;
lilac0112_1 1:ea7307724f1c 280 }
lilac0112_1 1:ea7307724f1c 281 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_4]){
lilac0112_1 1:ea7307724f1c 282 ir_val_phase[IR_LONG] = DIS_4;
lilac0112_1 1:ea7307724f1c 283 }
lilac0112_1 1:ea7307724f1c 284 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_3]){
lilac0112_1 1:ea7307724f1c 285 ir_val_phase[IR_LONG] = DIS_3;
lilac0112_1 1:ea7307724f1c 286 }
lilac0112_1 1:ea7307724f1c 287 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_2]){
lilac0112_1 1:ea7307724f1c 288 ir_val_phase[IR_LONG] = DIS_2;
lilac0112_1 1:ea7307724f1c 289 }
lilac0112_1 1:ea7307724f1c 290 else if(ir_val_long[7]>=ir_dis_range[IR_LONG][DIS_1]){
lilac0112_1 1:ea7307724f1c 291 ir_val_phase[IR_LONG] = DIS_1;
lilac0112_1 1:ea7307724f1c 292 }
lilac0112_1 1:ea7307724f1c 293 else{//ir_val_long[7]>=0
lilac0112_1 1:ea7307724f1c 294 ir_val_phase[IR_LONG] = DIS_0;
lilac0112_1 1:ea7307724f1c 295 }
lilac0112_1 1:ea7307724f1c 296
lilac0112_1 5:d666b328c093 297 //long_dif
lilac0112_1 0:6c79a352010b 298
lilac0112_1 5:d666b328c093 299 if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_7]){
lilac0112_1 5:d666b328c093 300 ir_dif_phase[IR_LONG] = DIS_7;
lilac0112_1 5:d666b328c093 301 }
lilac0112_1 5:d666b328c093 302 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_6]){
lilac0112_1 5:d666b328c093 303 ir_dif_phase[IR_LONG] = DIS_6;
lilac0112_1 5:d666b328c093 304 }
lilac0112_1 5:d666b328c093 305 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_5]){
lilac0112_1 5:d666b328c093 306 ir_dif_phase[IR_LONG] = DIS_5;
lilac0112_1 5:d666b328c093 307 }
lilac0112_1 5:d666b328c093 308 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_4]){
lilac0112_1 5:d666b328c093 309 ir_dif_phase[IR_LONG] = DIS_4;
lilac0112_1 5:d666b328c093 310 }
lilac0112_1 5:d666b328c093 311 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_3]){
lilac0112_1 5:d666b328c093 312 ir_dif_phase[IR_LONG] = DIS_3;
lilac0112_1 5:d666b328c093 313 }
lilac0112_1 5:d666b328c093 314 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_2]){
lilac0112_1 5:d666b328c093 315 ir_dif_phase[IR_LONG] = DIS_2;
lilac0112_1 5:d666b328c093 316 }
lilac0112_1 5:d666b328c093 317 else if(ir_val_diff[IR_LONG]>=ir_dif_range[IR_LONG][DIS_1]){
lilac0112_1 5:d666b328c093 318 ir_dif_phase[IR_LONG] = DIS_1;
lilac0112_1 5:d666b328c093 319 }
lilac0112_1 5:d666b328c093 320 else{//ir_val_diff[IR_LONG]>=0
lilac0112_1 5:d666b328c093 321 ir_dif_phase[IR_LONG] = DIS_0;
lilac0112_1 5:d666b328c093 322 }
lilac0112_1 5:d666b328c093 323
lilac0112_1 5:d666b328c093 324 //ir_place
lilac0112_1 5:d666b328c093 325 if(ir_val_phase[IR_SHORT] >= DIS_7){
lilac0112_1 6:80c2f4fce2eb 326 ir_place = ir_position[IR_LONG]-1;
lilac0112_1 0:6c79a352010b 327 }
lilac0112_1 0:6c79a352010b 328 else{
lilac0112_1 6:80c2f4fce2eb 329 ir_place = ir_position[IR_SHORT]+7;
lilac0112_1 5:d666b328c093 330 }
lilac0112_1 0:6c79a352010b 331
lilac0112_1 0:6c79a352010b 332 //デバッグ用のTeraTermへの出力
lilac0112_1 0:6c79a352010b 333
lilac0112_1 6:80c2f4fce2eb 334 /*
lilac0112_1 0:6c79a352010b 335 pc.printf("val_short:%d\t", ir_val_short[11]);//最小検出値(近)
lilac0112_1 5:d666b328c093 336 pc.printf("val_long(AVE):%d\t", ir_val_long[7]);//最小検出値(遠)
lilac0112_1 5:d666b328c093 337 //pc.printf("val_long:%d\t", ir_val_long_plus);//最小検出値(遠)...平均なし
lilac0112_1 5:d666b328c093 338
lilac0112_1 0:6c79a352010b 339 pc.printf("state_short:%d\t", ir_position[0]);//ボール位置(近)
lilac0112_1 0:6c79a352010b 340 pc.printf("state_long:%d\t", ir_position[1]);//ボール位置(遠)
lilac0112_1 5:d666b328c093 341 //pc.printf("Diff_short:%d\t", ir_val_diff[IR_SHORT]);//検出した最大値と最小値の差(近)
lilac0112_1 5:d666b328c093 342 pc.printf("Diff_long:%d\t", ir_val_diff[IR_LONG]);//検出した最大値と最小値の差(遠)
lilac0112_1 2:83b8bf9f0ea1 343 pc.printf("ir_val_phase[IR_SHORT]:%d\t", ir_val_phase[IR_SHORT]);//判定されたボールの距離の段階(遠)
lilac0112_1 2:83b8bf9f0ea1 344 pc.printf("ir_val_phase[IR_LONG]:%d\t", ir_val_phase[IR_LONG]);//判定されたボールの距離の段階(遠)
lilac0112_1 7:22d0c4a400c4 345 pc.printf("notice:%d\t", ir_notice);//判定されたボールの状況
lilac0112_1 2:83b8bf9f0ea1 346
lilac0112_1 6:80c2f4fce2eb 347 pc.printf("ir_place:%d\t", ir_place);
lilac0112_1 6:80c2f4fce2eb 348
lilac0112_1 5:d666b328c093 349
lilac0112_1 0:6c79a352010b 350 pc.printf("\r\n");
lilac0112_1 6:80c2f4fce2eb 351 */
lilac0112_1 5:d666b328c093 352
lilac0112_1 0:6c79a352010b 353 /*
lilac0112_1 0:6c79a352010b 354 pc.printf("%Value is ");
lilac0112_1 0:6c79a352010b 355 pc.printf("Dirr_short is %d\t", ir_val_short[0]-ir_val_short[11]);//検出した最大値と最小値の差(近)
lilac0112_1 0:6c79a352010b 356 pc.printf("Dirr_long is %d\t", ir_val_long[0]-ir_val_long[7]);//検出した最大値と最小値の差(遠)
lilac0112_1 0:6c79a352010b 357 pc.printf("\r\n");
lilac0112_1 0:6c79a352010b 358 */
lilac0112_1 0:6c79a352010b 359
lilac0112_1 0:6c79a352010b 360 //検出した値全てを出力(バブルソートすると位置が変わるので注意)
lilac0112_1 0:6c79a352010b 361 /*pc.printf("%Value is ");
lilac0112_1 0:6c79a352010b 362 for(i=0; i<8; i++){
lilac0112_1 0:6c79a352010b 363 pc.printf("%d\t", ir_val_long[i]);//遠
lilac0112_1 0:6c79a352010b 364 }
lilac0112_1 0:6c79a352010b 365 for(i=0; i<12; i++){
lilac0112_1 0:6c79a352010b 366 pc.printf("%d\t", ir_val_short[i]);//近
lilac0112_1 0:6c79a352010b 367 }
lilac0112_1 0:6c79a352010b 368
lilac0112_1 0:6c79a352010b 369 pc.printf("\r\n");
lilac0112_1 0:6c79a352010b 370 */
lilac0112_1 0:6c79a352010b 371 }
lilac0112_1 0:6c79a352010b 372 }
lilac0112_1 0:6c79a352010b 373 uint16_t read_input(uint8_t ic, uint8_t channel)
lilac0112_1 0:6c79a352010b 374 {
lilac0112_1 0:6c79a352010b 375 uint16_t command_high = START_BIT | MODE_SINGLE | ((channel & 0x04) >> 2);
lilac0112_1 0:6c79a352010b 376 uint16_t command_low = (channel & 0x03) << 6;
lilac0112_1 0:6c79a352010b 377
lilac0112_1 0:6c79a352010b 378 cs[ic] = 0;
lilac0112_1 0:6c79a352010b 379
lilac0112_1 0:6c79a352010b 380 ir.write(command_high);
lilac0112_1 0:6c79a352010b 381 uint16_t high_byte = ir.write(command_low) & 0x0F;
lilac0112_1 0:6c79a352010b 382 uint16_t low_byte = ir.write(0);
lilac0112_1 0:6c79a352010b 383
lilac0112_1 0:6c79a352010b 384 wait_us(1);
lilac0112_1 0:6c79a352010b 385 cs[ic] = 1;
lilac0112_1 0:6c79a352010b 386
lilac0112_1 0:6c79a352010b 387 return (high_byte << 8) | low_byte;//0x0000...0xFFFF
lilac0112_1 0:6c79a352010b 388 }
lilac0112_1 0:6c79a352010b 389 uint8_t read_position(uint8_t ic, uint8_t channel){
lilac0112_1 0:6c79a352010b 390 //DigitalOut cs[IC_NUM] = {cs1, cs2, cs3};
lilac0112_1 0:6c79a352010b 391 static uint8_t const ir_location[3][8]=
lilac0112_1 0:6c79a352010b 392 {//0,1,2...7(ch)
lilac0112_1 0:6c79a352010b 393 {1, 2, 3, 4, 5, 6, 7, 8},//cs1,Long*8
lilac0112_1 0:6c79a352010b 394 {1, 2, 3, 4, 5, 6, 7, 8},//cs2,Short*8
lilac0112_1 0:6c79a352010b 395 {9, 10, 11, 12},//cs3,Short*4
lilac0112_1 0:6c79a352010b 396 };
lilac0112_1 0:6c79a352010b 397 return ir_location[ic][channel];
lilac0112_1 0:6c79a352010b 398 }
lilac0112_1 1:ea7307724f1c 399 void BubbleSort(uint16_t *data, uint8_t *data2, uint8_t n)//降順にする
lilac0112_1 0:6c79a352010b 400 {
lilac0112_1 0:6c79a352010b 401 bool flag;
lilac0112_1 0:6c79a352010b 402 uint8_t i, j;//inclement
lilac0112_1 0:6c79a352010b 403 uint16_t temp;//temporary for value
lilac0112_1 0:6c79a352010b 404 uint8_t temp2;//temporary for position
lilac0112_1 0:6c79a352010b 405
lilac0112_1 0:6c79a352010b 406 j=0;
lilac0112_1 0:6c79a352010b 407 do{
lilac0112_1 0:6c79a352010b 408 j++;
lilac0112_1 0:6c79a352010b 409 flag=0;
lilac0112_1 0:6c79a352010b 410 for (i=0; i<n-j; i++) {
lilac0112_1 0:6c79a352010b 411 if (data[i]<data[i+1]) {
lilac0112_1 0:6c79a352010b 412 // 左右の並びがおかしければ入れ替える
lilac0112_1 0:6c79a352010b 413 flag=1;
lilac0112_1 0:6c79a352010b 414 temp=data[i];
lilac0112_1 0:6c79a352010b 415 data[i]=data[i+1];
lilac0112_1 0:6c79a352010b 416 data[i+1]=temp;
lilac0112_1 0:6c79a352010b 417
lilac0112_1 0:6c79a352010b 418 temp2=data2[i];
lilac0112_1 0:6c79a352010b 419 data2[i]=data2[i+1];
lilac0112_1 0:6c79a352010b 420 data2[i+1]=temp2;
lilac0112_1 0:6c79a352010b 421 }
lilac0112_1 0:6c79a352010b 422 }
lilac0112_1 0:6c79a352010b 423 } while (flag==1); //入れ替えがある間，繰り返す．
lilac0112_1 0:6c79a352010b 424 }
lilac0112_1 0:6c79a352010b 425 #endif /*IR_SENSOR*/