yusuke takahashi / Mbed 2 deprecated Ball_fanction

IRIR

main.cpp@0:0c7c6eefafe4, 2013-03-04 (annotated)

Committer:
yusuke_robocup
Date:
Mon Mar 04 07:50:18 2013 +0000
Revision:
0:0c7c6eefafe4
Child:
1:d6c8be12a3de
IRIR

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 0:0c7c6eefafe4 5 #define ALL_IR 8
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 0:0c7c6eefafe4 27 p20
yusuke_robocup 0:0c7c6eefafe4 28 };
yusuke_robocup 0:0c7c6eefafe4 29
yusuke_robocup 0:0c7c6eefafe4 30 int Convert_Direction[DIRECTION] = {
yusuke_robocup 0:0c7c6eefafe4 31 0,
yusuke_robocup 0:0c7c6eefafe4 32 23,
yusuke_robocup 0:0c7c6eefafe4 33 45,
yusuke_robocup 0:0c7c6eefafe4 34 68,
yusuke_robocup 0:0c7c6eefafe4 35 90,
yusuke_robocup 0:0c7c6eefafe4 36 113,
yusuke_robocup 0:0c7c6eefafe4 37 135,
yusuke_robocup 0:0c7c6eefafe4 38 158,
yusuke_robocup 0:0c7c6eefafe4 39 180,
yusuke_robocup 0:0c7c6eefafe4 40 -158,
yusuke_robocup 0:0c7c6eefafe4 41 -135,
yusuke_robocup 0:0c7c6eefafe4 42 -113,
yusuke_robocup 0:0c7c6eefafe4 43 -90,
yusuke_robocup 0:0c7c6eefafe4 44 -68,
yusuke_robocup 0:0c7c6eefafe4 45 -45,
yusuke_robocup 0:0c7c6eefafe4 46 -23
yusuke_robocup 0:0c7c6eefafe4 47 };
yusuke_robocup 0:0c7c6eefafe4 48
yusuke_robocup 0:0c7c6eefafe4 49 int moving_ave(int data,int active_ir)
yusuke_robocup 0:0c7c6eefafe4 50 {
yusuke_robocup 0:0c7c6eefafe4 51 static int tmp[14][ALL_IR]= {{0,0}};
yusuke_robocup 0:0c7c6eefafe4 52 static int sum[ALL_IR] = {0};
yusuke_robocup 0:0c7c6eefafe4 53 int count = 0;
yusuke_robocup 0:0c7c6eefafe4 54
yusuke_robocup 0:0c7c6eefafe4 55 sum[active_ir] -= tmp[13][active_ir];
yusuke_robocup 0:0c7c6eefafe4 56 sum[active_ir] += data;
yusuke_robocup 0:0c7c6eefafe4 57 tmp[13][active_ir] = tmp[12][active_ir];
yusuke_robocup 0:0c7c6eefafe4 58 tmp[12][active_ir] = tmp[11][active_ir];
yusuke_robocup 0:0c7c6eefafe4 59 tmp[11][active_ir] = tmp[10][active_ir];
yusuke_robocup 0:0c7c6eefafe4 60 tmp[10][active_ir] = tmp[9][active_ir];
yusuke_robocup 0:0c7c6eefafe4 61 tmp[9][active_ir] = tmp[8][active_ir];
yusuke_robocup 0:0c7c6eefafe4 62 tmp[8][active_ir] = tmp[7][active_ir];
yusuke_robocup 0:0c7c6eefafe4 63 tmp[7][active_ir] = tmp[6][active_ir];
yusuke_robocup 0:0c7c6eefafe4 64 tmp[6][active_ir] = tmp[5][active_ir];
yusuke_robocup 0:0c7c6eefafe4 65 tmp[5][active_ir] = tmp[4][active_ir];
yusuke_robocup 0:0c7c6eefafe4 66 tmp[4][active_ir] = tmp[3][active_ir];
yusuke_robocup 0:0c7c6eefafe4 67 tmp[3][active_ir] = tmp[2][active_ir];
yusuke_robocup 0:0c7c6eefafe4 68 tmp[2][active_ir] = tmp[1][active_ir];
yusuke_robocup 0:0c7c6eefafe4 69 tmp[1][active_ir] = tmp[0][active_ir];
yusuke_robocup 0:0c7c6eefafe4 70 tmp[0][active_ir] = data;
yusuke_robocup 0:0c7c6eefafe4 71
yusuke_robocup 0:0c7c6eefafe4 72 for(int i=0;i<14;i++){
yusuke_robocup 0:0c7c6eefafe4 73 if(tmp[i][active_ir])count++;
yusuke_robocup 0:0c7c6eefafe4 74 }
yusuke_robocup 0:0c7c6eefafe4 75
yusuke_robocup 0:0c7c6eefafe4 76 return sum[active_ir]/count;
yusuke_robocup 0:0c7c6eefafe4 77 }
yusuke_robocup 0:0c7c6eefafe4 78
yusuke_robocup 0:0c7c6eefafe4 79 void IR_Position(int* direction,int* distance){
yusuke_robocup 0:0c7c6eefafe4 80
yusuke_robocup 0:0c7c6eefafe4 81 int ir_value[ALL_IR+100] = {0};
yusuke_robocup 0:0c7c6eefafe4 82
yusuke_robocup 0:0c7c6eefafe4 83 int active_ir = 0; /* 今回更新する赤外線の番号 */
yusuke_robocup 0:0c7c6eefafe4 84 int memory_ir = 0; /*赤外線時間カウンタ*/
yusuke_robocup 0:0c7c6eefafe4 85 int flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 86 int value = 0;
yusuke_robocup 0:0c7c6eefafe4 87
yusuke_robocup 0:0c7c6eefafe4 88 for(int i=0; i<ALL_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 89 flag_ir = 1;
yusuke_robocup 0:0c7c6eefafe4 90
yusuke_robocup 0:0c7c6eefafe4 91 DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
yusuke_robocup 0:0c7c6eefafe4 92
yusuke_robocup 0:0c7c6eefafe4 93 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 94
yusuke_robocup 0:0c7c6eefafe4 95 if(sensor_ir) { /* もし立ち上がっていたら */
yusuke_robocup 0:0c7c6eefafe4 96 while(sensor_ir) { /* 立ち下がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 97 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 98 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 99 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 100 }
yusuke_robocup 0:0c7c6eefafe4 101 }
yusuke_robocup 0:0c7c6eefafe4 102 }
yusuke_robocup 0:0c7c6eefafe4 103
yusuke_robocup 0:0c7c6eefafe4 104 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 105 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 106
yusuke_robocup 0:0c7c6eefafe4 107 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 108 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 109
yusuke_robocup 0:0c7c6eefafe4 110 while(!(sensor_ir)) { /* 立ち上がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 111 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 112 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 113 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 114 }
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 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 120 memory_ir = timer_ir.read_us();
yusuke_robocup 0:0c7c6eefafe4 121
yusuke_robocup 0:0c7c6eefafe4 122 while(1) {
yusuke_robocup 0:0c7c6eefafe4 123 if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
yusuke_robocup 0:0c7c6eefafe4 124
yusuke_robocup 0:0c7c6eefafe4 125 if(!(sensor_ir)) {
yusuke_robocup 0:0c7c6eefafe4 126 value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
yusuke_robocup 0:0c7c6eefafe4 127
yusuke_robocup 0:0c7c6eefafe4 128 break;
yusuke_robocup 0:0c7c6eefafe4 129 }
yusuke_robocup 0:0c7c6eefafe4 130 }
yusuke_robocup 0:0c7c6eefafe4 131 } else {
yusuke_robocup 0:0c7c6eefafe4 132 /*ボールが見つかっていない場合*/
yusuke_robocup 0:0c7c6eefafe4 133 value = 0;
yusuke_robocup 0:0c7c6eefafe4 134 }
yusuke_robocup 0:0c7c6eefafe4 135 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 136 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 137
yusuke_robocup 0:0c7c6eefafe4 138 memory_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 139
yusuke_robocup 0:0c7c6eefafe4 140 ir_value[active_ir] = value; //direction array
yusuke_robocup 0:0c7c6eefafe4 141
yusuke_robocup 0:0c7c6eefafe4 142 active_ir++;
yusuke_robocup 0:0c7c6eefafe4 143
yusuke_robocup 0:0c7c6eefafe4 144 if( active_ir >= ALL_IR) {
yusuke_robocup 0:0c7c6eefafe4 145 active_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 146
yusuke_robocup 0:0c7c6eefafe4 147 /***********direction***********/
yusuke_robocup 0:0c7c6eefafe4 148
yusuke_robocup 0:0c7c6eefafe4 149 int min = 100,youso_min = 100;
yusuke_robocup 0:0c7c6eefafe4 150
yusuke_robocup 0:0c7c6eefafe4 151 for(int i = 0; i<ALL_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 152 if((ir_value[i]<min)&&(ir_value[i])) {
yusuke_robocup 0:0c7c6eefafe4 153 min = ir_value[i];
yusuke_robocup 0:0c7c6eefafe4 154 youso_min = i;
yusuke_robocup 0:0c7c6eefafe4 155 }
yusuke_robocup 0:0c7c6eefafe4 156 }
yusuke_robocup 0:0c7c6eefafe4 157
yusuke_robocup 0:0c7c6eefafe4 158 double hiritu = 0;
yusuke_robocup 0:0c7c6eefafe4 159
yusuke_robocup 0:0c7c6eefafe4 160 int direc = 0;
yusuke_robocup 0:0c7c6eefafe4 161
yusuke_robocup 0:0c7c6eefafe4 162 if(youso_min == 0) {
yusuke_robocup 0:0c7c6eefafe4 163 hiritu = (double)ir_value[7]/(double)ir_value[1];
yusuke_robocup 0:0c7c6eefafe4 164 } else if(youso_min ==7) {
yusuke_robocup 0:0c7c6eefafe4 165 hiritu = (double)ir_value[6]/(double)ir_value[0];
yusuke_robocup 0:0c7c6eefafe4 166 } else {
yusuke_robocup 0:0c7c6eefafe4 167 hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
yusuke_robocup 0:0c7c6eefafe4 168 }
yusuke_robocup 0:0c7c6eefafe4 169
yusuke_robocup 0:0c7c6eefafe4 170 if((hiritu <= 0.83)&&(youso_min != 0)) {
yusuke_robocup 0:0c7c6eefafe4 171 direc = youso_min*2-1;
yusuke_robocup 0:0c7c6eefafe4 172 }else if((hiritu <= 0.83)&&(youso_min == 0)){
yusuke_robocup 0:0c7c6eefafe4 173 direc = 15;
yusuke_robocup 0:0c7c6eefafe4 174 }else if(hiritu >= 1.17) {
yusuke_robocup 0:0c7c6eefafe4 175 direc = youso_min*2+1;
yusuke_robocup 0:0c7c6eefafe4 176 } else {
yusuke_robocup 0:0c7c6eefafe4 177 direc = youso_min*2;
yusuke_robocup 0:0c7c6eefafe4 178 }
yusuke_robocup 0:0c7c6eefafe4 179
yusuke_robocup 0:0c7c6eefafe4 180 /******* direction end *******/
yusuke_robocup 0:0c7c6eefafe4 181
yusuke_robocup 0:0c7c6eefafe4 182 /******* distance *******/
yusuke_robocup 0:0c7c6eefafe4 183
yusuke_robocup 0:0c7c6eefafe4 184 int dista;
yusuke_robocup 0:0c7c6eefafe4 185
yusuke_robocup 0:0c7c6eefafe4 186 if((ir_value[youso_min]>0)&&(ir_value[youso_min]<=25 + TERM)){
yusuke_robocup 0:0c7c6eefafe4 187 dista = 10;
yusuke_robocup 0:0c7c6eefafe4 188 }else if((ir_value[youso_min]>25 + TERM)&&(ir_value[youso_min]<=28 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 189 dista = 30;
yusuke_robocup 0:0c7c6eefafe4 190 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 191 dista = 90;
yusuke_robocup 0:0c7c6eefafe4 192 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 193 dista = 120;
yusuke_robocup 0:0c7c6eefafe4 194 } else if( ir_value[youso_min]>40 + TERM) {
yusuke_robocup 0:0c7c6eefafe4 195 dista = 180;
yusuke_robocup 0:0c7c6eefafe4 196 } else {
yusuke_robocup 0:0c7c6eefafe4 197 dista = 0;
yusuke_robocup 0:0c7c6eefafe4 198 }
yusuke_robocup 0:0c7c6eefafe4 199
yusuke_robocup 0:0c7c6eefafe4 200 /******** distance end *******/
yusuke_robocup 0:0c7c6eefafe4 201
yusuke_robocup 0:0c7c6eefafe4 202 *direction = Convert_Direction[direc];
yusuke_robocup 0:0c7c6eefafe4 203 *distance = dista;
yusuke_robocup 0:0c7c6eefafe4 204 }
yusuke_robocup 0:0c7c6eefafe4 205 }
yusuke_robocup 0:0c7c6eefafe4 206 }
yusuke_robocup 0:0c7c6eefafe4 207
yusuke_robocup 0:0c7c6eefafe4 208 int main ()
yusuke_robocup 0:0c7c6eefafe4 209 {
yusuke_robocup 0:0c7c6eefafe4 210 pc.baud(9600);
yusuke_robocup 0:0c7c6eefafe4 211
yusuke_robocup 0:0c7c6eefafe4 212 int derection = 0;
yusuke_robocup 0:0c7c6eefafe4 213 int distance = 0;
yusuke_robocup 0:0c7c6eefafe4 214
yusuke_robocup 0:0c7c6eefafe4 215 for(;;){
yusuke_robocup 0:0c7c6eefafe4 216 IR_Position(&derection,&distance);
yusuke_robocup 0:0c7c6eefafe4 217
yusuke_robocup 0:0c7c6eefafe4 218 pc.printf("derection:%d distance:%d\n",derection,distance);
yusuke_robocup 0:0c7c6eefafe4 219 }
yusuke_robocup 0:0c7c6eefafe4 220 }