yusuke takahashi / Mbed 2 deprecated Ball_fanction

IRIR

main.cpp@1:d6c8be12a3de, 2013-03-05 (annotated)

Committer:
yusuke_robocup
Date:
Tue Mar 05 09:45:33 2013 +0000
Revision:
1:d6c8be12a3de
Parent:
0:0c7c6eefafe4
Child:
2:22b099fbdd14
Ball_fanction2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_robocup 0:0c7c6eefafe4 1 #include "mbed.h"
yusuke_robocup 0:0c7c6eefafe4 2
yusuke_robocup 0:0c7c6eefafe4 3 #define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位：us) */
yusuke_robocup 0:0c7c6eefafe4 4 #define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/
yusuke_robocup 1:d6c8be12a3de 5 #define ALL_IR 9
yusuke_robocup 0:0c7c6eefafe4 6 #define DIRECTION 16
yusuke_robocup 0:0c7c6eefafe4 7 #define TERM 0
yusuke_robocup 0:0c7c6eefafe4 8 #define SWAP(type,a,b) { type temp = a; a = b; b = temp; }
yusuke_robocup 0:0c7c6eefafe4 9
yusuke_robocup 0:0c7c6eefafe4 10 Serial pc(USBTX, USBRX); // tx, rx
yusuke_robocup 0:0c7c6eefafe4 11
yusuke_robocup 0:0c7c6eefafe4 12 Timer timer_ir; /* 赤外線用タイマー */
yusuke_robocup 0:0c7c6eefafe4 13
yusuke_robocup 0:0c7c6eefafe4 14 BusOut myleds(p18, p17, p16, p15, p14, p13, p12, p11); //出力用ポート
yusuke_robocup 0:0c7c6eefafe4 15
yusuke_robocup 0:0c7c6eefafe4 16 BusOut mbedleds(LED4,LED3,LED2,LED1);
yusuke_robocup 0:0c7c6eefafe4 17
yusuke_robocup 0:0c7c6eefafe4 18 /* 赤外線センサに使うpinを配列に格納 */
yusuke_robocup 0:0c7c6eefafe4 19 PinName ir_num[ALL_IR] = {
yusuke_robocup 0:0c7c6eefafe4 20 p13,
yusuke_robocup 0:0c7c6eefafe4 21 p14,
yusuke_robocup 0:0c7c6eefafe4 22 p15,
yusuke_robocup 0:0c7c6eefafe4 23 p16,
yusuke_robocup 0:0c7c6eefafe4 24 p17,
yusuke_robocup 0:0c7c6eefafe4 25 p18,
yusuke_robocup 0:0c7c6eefafe4 26 p19,
yusuke_robocup 1:d6c8be12a3de 27 p20,
yusuke_robocup 1:d6c8be12a3de 28 p30
yusuke_robocup 0:0c7c6eefafe4 29 };
yusuke_robocup 0:0c7c6eefafe4 30
yusuke_robocup 0:0c7c6eefafe4 31 int Convert_Direction[DIRECTION] = {
yusuke_robocup 1:d6c8be12a3de 32 90,
yusuke_robocup 1:d6c8be12a3de 33 67,
yusuke_robocup 0:0c7c6eefafe4 34 45,
yusuke_robocup 1:d6c8be12a3de 35 22,
yusuke_robocup 1:d6c8be12a3de 36 0,
yusuke_robocup 1:d6c8be12a3de 37 337,
yusuke_robocup 1:d6c8be12a3de 38 315,
yusuke_robocup 1:d6c8be12a3de 39 292,
yusuke_robocup 1:d6c8be12a3de 40 270,
yusuke_robocup 1:d6c8be12a3de 41 247,
yusuke_robocup 1:d6c8be12a3de 42 225,
yusuke_robocup 1:d6c8be12a3de 43 202,
yusuke_robocup 1:d6c8be12a3de 44 180,
yusuke_robocup 1:d6c8be12a3de 45 157,
yusuke_robocup 0:0c7c6eefafe4 46 135,
yusuke_robocup 1:d6c8be12a3de 47 112
yusuke_robocup 0:0c7c6eefafe4 48 };
yusuke_robocup 0:0c7c6eefafe4 49
yusuke_robocup 0:0c7c6eefafe4 50 int moving_ave(int data,int active_ir)
yusuke_robocup 0:0c7c6eefafe4 51 {
yusuke_robocup 0:0c7c6eefafe4 52 static int tmp[14][ALL_IR]= {{0,0}};
yusuke_robocup 0:0c7c6eefafe4 53 static int sum[ALL_IR] = {0};
yusuke_robocup 0:0c7c6eefafe4 54 int count = 0;
yusuke_robocup 0:0c7c6eefafe4 55
yusuke_robocup 0:0c7c6eefafe4 56 sum[active_ir] -= tmp[13][active_ir];
yusuke_robocup 0:0c7c6eefafe4 57 sum[active_ir] += data;
yusuke_robocup 0:0c7c6eefafe4 58 tmp[13][active_ir] = tmp[12][active_ir];
yusuke_robocup 0:0c7c6eefafe4 59 tmp[12][active_ir] = tmp[11][active_ir];
yusuke_robocup 0:0c7c6eefafe4 60 tmp[11][active_ir] = tmp[10][active_ir];
yusuke_robocup 0:0c7c6eefafe4 61 tmp[10][active_ir] = tmp[9][active_ir];
yusuke_robocup 0:0c7c6eefafe4 62 tmp[9][active_ir] = tmp[8][active_ir];
yusuke_robocup 0:0c7c6eefafe4 63 tmp[8][active_ir] = tmp[7][active_ir];
yusuke_robocup 0:0c7c6eefafe4 64 tmp[7][active_ir] = tmp[6][active_ir];
yusuke_robocup 0:0c7c6eefafe4 65 tmp[6][active_ir] = tmp[5][active_ir];
yusuke_robocup 0:0c7c6eefafe4 66 tmp[5][active_ir] = tmp[4][active_ir];
yusuke_robocup 0:0c7c6eefafe4 67 tmp[4][active_ir] = tmp[3][active_ir];
yusuke_robocup 0:0c7c6eefafe4 68 tmp[3][active_ir] = tmp[2][active_ir];
yusuke_robocup 0:0c7c6eefafe4 69 tmp[2][active_ir] = tmp[1][active_ir];
yusuke_robocup 0:0c7c6eefafe4 70 tmp[1][active_ir] = tmp[0][active_ir];
yusuke_robocup 0:0c7c6eefafe4 71 tmp[0][active_ir] = data;
yusuke_robocup 0:0c7c6eefafe4 72
yusuke_robocup 0:0c7c6eefafe4 73 for(int i=0;i<14;i++){
yusuke_robocup 0:0c7c6eefafe4 74 if(tmp[i][active_ir])count++;
yusuke_robocup 0:0c7c6eefafe4 75 }
yusuke_robocup 0:0c7c6eefafe4 76
yusuke_robocup 0:0c7c6eefafe4 77 return sum[active_ir]/count;
yusuke_robocup 0:0c7c6eefafe4 78 }
yusuke_robocup 0:0c7c6eefafe4 79
yusuke_robocup 0:0c7c6eefafe4 80 void IR_Position(int* direction,int* distance){
yusuke_robocup 0:0c7c6eefafe4 81
yusuke_robocup 0:0c7c6eefafe4 82 int ir_value[ALL_IR+100] = {0};
yusuke_robocup 0:0c7c6eefafe4 83
yusuke_robocup 0:0c7c6eefafe4 84 int active_ir = 0; /* 今回更新する赤外線の番号 */
yusuke_robocup 0:0c7c6eefafe4 85 int memory_ir = 0; /*赤外線時間カウンタ*/
yusuke_robocup 0:0c7c6eefafe4 86 int flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 87 int value = 0;
yusuke_robocup 0:0c7c6eefafe4 88
yusuke_robocup 0:0c7c6eefafe4 89 for(int i=0; i<ALL_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 90 flag_ir = 1;
yusuke_robocup 0:0c7c6eefafe4 91
yusuke_robocup 0:0c7c6eefafe4 92 DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
yusuke_robocup 0:0c7c6eefafe4 93
yusuke_robocup 0:0c7c6eefafe4 94 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 95
yusuke_robocup 0:0c7c6eefafe4 96 if(sensor_ir) { /* もし立ち上がっていたら */
yusuke_robocup 0:0c7c6eefafe4 97 while(sensor_ir) { /* 立ち下がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 98 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 99 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 100 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 101 }
yusuke_robocup 0:0c7c6eefafe4 102 }
yusuke_robocup 0:0c7c6eefafe4 103 }
yusuke_robocup 0:0c7c6eefafe4 104
yusuke_robocup 0:0c7c6eefafe4 105 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 106 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 107
yusuke_robocup 0:0c7c6eefafe4 108 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 109 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 110
yusuke_robocup 0:0c7c6eefafe4 111 while(!(sensor_ir)) { /* 立ち上がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 112 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 113 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 114 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 115 }
yusuke_robocup 0:0c7c6eefafe4 116 }
yusuke_robocup 0:0c7c6eefafe4 117 }
yusuke_robocup 0:0c7c6eefafe4 118
yusuke_robocup 0:0c7c6eefafe4 119 /*ボールが指定時間内に見つかっていたら*/
yusuke_robocup 0:0c7c6eefafe4 120 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 121 memory_ir = timer_ir.read_us();
yusuke_robocup 0:0c7c6eefafe4 122
yusuke_robocup 0:0c7c6eefafe4 123 while(1) {
yusuke_robocup 0:0c7c6eefafe4 124 if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
yusuke_robocup 0:0c7c6eefafe4 125
yusuke_robocup 0:0c7c6eefafe4 126 if(!(sensor_ir)) {
yusuke_robocup 0:0c7c6eefafe4 127 value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
yusuke_robocup 0:0c7c6eefafe4 128
yusuke_robocup 0:0c7c6eefafe4 129 break;
yusuke_robocup 0:0c7c6eefafe4 130 }
yusuke_robocup 0:0c7c6eefafe4 131 }
yusuke_robocup 0:0c7c6eefafe4 132 } else {
yusuke_robocup 0:0c7c6eefafe4 133 /*ボールが見つかっていない場合*/
yusuke_robocup 0:0c7c6eefafe4 134 value = 0;
yusuke_robocup 0:0c7c6eefafe4 135 }
yusuke_robocup 0:0c7c6eefafe4 136 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 137 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 138
yusuke_robocup 0:0c7c6eefafe4 139 memory_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 140
yusuke_robocup 0:0c7c6eefafe4 141 ir_value[active_ir] = value; //direction array
yusuke_robocup 0:0c7c6eefafe4 142
yusuke_robocup 0:0c7c6eefafe4 143 active_ir++;
yusuke_robocup 0:0c7c6eefafe4 144
yusuke_robocup 0:0c7c6eefafe4 145 if( active_ir >= ALL_IR) {
yusuke_robocup 0:0c7c6eefafe4 146 active_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 147
yusuke_robocup 0:0c7c6eefafe4 148 /***********direction***********/
yusuke_robocup 0:0c7c6eefafe4 149
yusuke_robocup 0:0c7c6eefafe4 150 int min = 100,youso_min = 100;
yusuke_robocup 0:0c7c6eefafe4 151
yusuke_robocup 0:0c7c6eefafe4 152 for(int i = 0; i<ALL_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 153 if((ir_value[i]<min)&&(ir_value[i])) {
yusuke_robocup 0:0c7c6eefafe4 154 min = ir_value[i];
yusuke_robocup 0:0c7c6eefafe4 155 youso_min = i;
yusuke_robocup 0:0c7c6eefafe4 156 }
yusuke_robocup 0:0c7c6eefafe4 157 }
yusuke_robocup 0:0c7c6eefafe4 158
yusuke_robocup 0:0c7c6eefafe4 159 double hiritu = 0;
yusuke_robocup 0:0c7c6eefafe4 160
yusuke_robocup 0:0c7c6eefafe4 161 int direc = 0;
yusuke_robocup 0:0c7c6eefafe4 162
yusuke_robocup 0:0c7c6eefafe4 163 if(youso_min == 0) {
yusuke_robocup 0:0c7c6eefafe4 164 hiritu = (double)ir_value[7]/(double)ir_value[1];
yusuke_robocup 0:0c7c6eefafe4 165 } else if(youso_min ==7) {
yusuke_robocup 0:0c7c6eefafe4 166 hiritu = (double)ir_value[6]/(double)ir_value[0];
yusuke_robocup 0:0c7c6eefafe4 167 } else {
yusuke_robocup 0:0c7c6eefafe4 168 hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
yusuke_robocup 0:0c7c6eefafe4 169 }
yusuke_robocup 1:d6c8be12a3de 170
yusuke_robocup 1:d6c8be12a3de 171 if((hiritu <= 0.85)&&(youso_min != 0)) {
yusuke_robocup 0:0c7c6eefafe4 172 direc = youso_min*2-1;
yusuke_robocup 1:d6c8be12a3de 173 }else if((hiritu <= 0.85)&&(youso_min == 0)){
yusuke_robocup 0:0c7c6eefafe4 174 direc = 15;
yusuke_robocup 1:d6c8be12a3de 175 }else if(hiritu >= 1.15) {
yusuke_robocup 0:0c7c6eefafe4 176 direc = youso_min*2+1;
yusuke_robocup 0:0c7c6eefafe4 177 } else {
yusuke_robocup 0:0c7c6eefafe4 178 direc = youso_min*2;
yusuke_robocup 0:0c7c6eefafe4 179 }
yusuke_robocup 1:d6c8be12a3de 180
yusuke_robocup 1:d6c8be12a3de 181 if(youso_min == 0){
yusuke_robocup 1:d6c8be12a3de 182 direc = 0;
yusuke_robocup 1:d6c8be12a3de 183 }
yusuke_robocup 0:0c7c6eefafe4 184
yusuke_robocup 0:0c7c6eefafe4 185 /******* direction end *******/
yusuke_robocup 0:0c7c6eefafe4 186
yusuke_robocup 0:0c7c6eefafe4 187 /******* distance *******/
yusuke_robocup 0:0c7c6eefafe4 188
yusuke_robocup 0:0c7c6eefafe4 189 int dista;
yusuke_robocup 0:0c7c6eefafe4 190
yusuke_robocup 0:0c7c6eefafe4 191 if((ir_value[youso_min]>0)&&(ir_value[youso_min]<=25 + TERM)){
yusuke_robocup 0:0c7c6eefafe4 192 dista = 10;
yusuke_robocup 0:0c7c6eefafe4 193 }else if((ir_value[youso_min]>25 + TERM)&&(ir_value[youso_min]<=28 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 194 dista = 30;
yusuke_robocup 0:0c7c6eefafe4 195 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 196 dista = 90;
yusuke_robocup 0:0c7c6eefafe4 197 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 198 dista = 120;
yusuke_robocup 0:0c7c6eefafe4 199 } else if( ir_value[youso_min]>40 + TERM) {
yusuke_robocup 0:0c7c6eefafe4 200 dista = 180;
yusuke_robocup 0:0c7c6eefafe4 201 } else {
yusuke_robocup 0:0c7c6eefafe4 202 dista = 0;
yusuke_robocup 0:0c7c6eefafe4 203 }
yusuke_robocup 0:0c7c6eefafe4 204
yusuke_robocup 1:d6c8be12a3de 205 if(dista <= 30){
yusuke_robocup 1:d6c8be12a3de 206 if((direc == 0)&&(ir_value[8] <= 30)){
yusuke_robocup 1:d6c8be12a3de 207 dista = 10;
yusuke_robocup 1:d6c8be12a3de 208 }else if((direc == 1)&&(ir_value[8] <= 30 )){
yusuke_robocup 1:d6c8be12a3de 209 dista = 10;
yusuke_robocup 1:d6c8be12a3de 210 }else if((direc == 2)&&(ir_value[8] <= 40 )){
yusuke_robocup 1:d6c8be12a3de 211 dista = 10;
yusuke_robocup 1:d6c8be12a3de 212 }else if((direc == 3)&&(ir_value[8] <= 43)){
yusuke_robocup 1:d6c8be12a3de 213 dista = 10;
yusuke_robocup 1:d6c8be12a3de 214 }else if((direc == 4)&&(ir_value[8] <= 42)){
yusuke_robocup 1:d6c8be12a3de 215 dista = 10;
yusuke_robocup 1:d6c8be12a3de 216 }else if((direc == 5)&&(ir_value[8] <= 42)){
yusuke_robocup 1:d6c8be12a3de 217 dista = 10;
yusuke_robocup 1:d6c8be12a3de 218 }else if((direc == 6)&&(ir_value[8] <= 42)){
yusuke_robocup 1:d6c8be12a3de 219 dista = 10;
yusuke_robocup 1:d6c8be12a3de 220 }else if((direc == 7)&&(ir_value[8] <= 40)){
yusuke_robocup 1:d6c8be12a3de 221 dista = 10;
yusuke_robocup 1:d6c8be12a3de 222 }else if((direc == 8)&&(ir_value[8] <= 38)){
yusuke_robocup 1:d6c8be12a3de 223 dista = 10;
yusuke_robocup 1:d6c8be12a3de 224 }else if((direc == 9)&&(ir_value[8] <= 34)){
yusuke_robocup 1:d6c8be12a3de 225 dista = 10;
yusuke_robocup 1:d6c8be12a3de 226 }else if((direc == 10)&&(ir_value[8] <= 40)){
yusuke_robocup 1:d6c8be12a3de 227 dista = 10;
yusuke_robocup 1:d6c8be12a3de 228 }else if((direc == 11)&&(ir_value[8] <= 42)){
yusuke_robocup 1:d6c8be12a3de 229 dista = 10;
yusuke_robocup 1:d6c8be12a3de 230 }else if((direc == 12)&&(ir_value[8] <= 39)){
yusuke_robocup 1:d6c8be12a3de 231 dista = 10;
yusuke_robocup 1:d6c8be12a3de 232 }else if((direc == 13)&&(ir_value[8] <= 40)){
yusuke_robocup 1:d6c8be12a3de 233 dista = 10;
yusuke_robocup 1:d6c8be12a3de 234 }else if((direc == 14)&&(ir_value[8] <= 41)){
yusuke_robocup 1:d6c8be12a3de 235 dista = 10;
yusuke_robocup 1:d6c8be12a3de 236 }else if((direc == 15)&&(ir_value[8] <= 33)){
yusuke_robocup 1:d6c8be12a3de 237 dista = 10;
yusuke_robocup 1:d6c8be12a3de 238 }
yusuke_robocup 1:d6c8be12a3de 239 }
yusuke_robocup 1:d6c8be12a3de 240
yusuke_robocup 0:0c7c6eefafe4 241 /******** distance end *******/
yusuke_robocup 0:0c7c6eefafe4 242
yusuke_robocup 0:0c7c6eefafe4 243 *direction = Convert_Direction[direc];
yusuke_robocup 0:0c7c6eefafe4 244 *distance = dista;
yusuke_robocup 0:0c7c6eefafe4 245 }
yusuke_robocup 0:0c7c6eefafe4 246 }
yusuke_robocup 0:0c7c6eefafe4 247 }
yusuke_robocup 0:0c7c6eefafe4 248
yusuke_robocup 0:0c7c6eefafe4 249 int main ()
yusuke_robocup 0:0c7c6eefafe4 250 {
yusuke_robocup 0:0c7c6eefafe4 251 pc.baud(9600);
yusuke_robocup 0:0c7c6eefafe4 252
yusuke_robocup 0:0c7c6eefafe4 253 int derection = 0;
yusuke_robocup 0:0c7c6eefafe4 254 int distance = 0;
yusuke_robocup 0:0c7c6eefafe4 255
yusuke_robocup 0:0c7c6eefafe4 256 for(;;){
yusuke_robocup 0:0c7c6eefafe4 257 IR_Position(&derection,&distance);
yusuke_robocup 0:0c7c6eefafe4 258
yusuke_robocup 0:0c7c6eefafe4 259 pc.printf("derection:%d distance:%d\n",derection,distance);
yusuke_robocup 0:0c7c6eefafe4 260 }
yusuke_robocup 0:0c7c6eefafe4 261 }