CatPot 2015-2016 / CatPot_2v30_T_Sub

LPC824's Program for RoboCup 2016 Leipzig

Dependencies:   mbed-src Ping SDFileSystem

main/format.cpp@0:3e24cf7f8a27, 2016-06-14 (annotated)

Committer:
lilac0112_1
Date:
Tue Jun 14 07:24:23 2016 +0000
Revision:
0:3e24cf7f8a27
Code of Japan open.

Who changed what in which revision?

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