ryo seki / Mbed 2 deprecated serialSend5_5

final_slave

Dependencies:   TextLCD mbed

IR.cpp@0:7d0227dcc289, 2013-06-19 (annotated)

Committer:
akudohune
Date:
Wed Jun 19 08:42:10 2013 +0000
Revision:
0:7d0227dcc289
slave

Who changed what in which revision?

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