nakao kanaki / Mbed 2 deprecated Eco_ziguzagu

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:f9c953ddc87a, 2016-02-28 (annotated)

Committer:
namikosaito
Date:
Sun Feb 28 15:37:23 2016 +0000
Revision:
1:f9c953ddc87a
Parent:
0:77188ca200ce
Child:
2:864b823b1735
2_29

Who changed what in which revision?

UserRevisionLine numberNew contents of line
namikosaito 0:77188ca200ce 1 #include "mbed.h"
namikosaito 0:77188ca200ce 2 #include "math.h"
namikosaito 0:77188ca200ce 3 #include "LF2.h"
namikosaito 0:77188ca200ce 4 #include "color.h"
namikosaito 0:77188ca200ce 5 #include "enc_gyro.h"
namikosaito 0:77188ca200ce 6
namikosaito 1:f9c953ddc87a 7
namikosaito 0:77188ca200ce 8 /*ここで切り替え（強制）*/
namikosaito 1:f9c953ddc87a 9 /*
namikosaito 0:77188ca200ce 10 //#define SLOPE
namikosaito 0:77188ca200ce 11 //#define TURN
namikosaito 0:77188ca200ce 12 #define RIVER
namikosaito 0:77188ca200ce 13 //#define RIVER2
namikosaito 0:77188ca200ce 14 #define DOWNHILL
namikosaito 1:f9c953ddc87a 15 */
namikosaito 0:77188ca200ce 16
namikosaito 0:77188ca200ce 17 /*白との差*/
namikosaito 1:f9c953ddc87a 18 #define bw 400 //blueとwhite
namikosaito 0:77188ca200ce 19 #define yw 200 //yellow ちょっと厳しめにすること。
namikosaito 1:f9c953ddc87a 20 #define rw 200 //red(orange)
namikosaito 0:77188ca200ce 21
namikosaito 0:77188ca200ce 22 Serial pc(USBTX, USBRX);
namikosaito 0:77188ca200ce 23 PwmOut servo(p25);
namikosaito 0:77188ca200ce 24 PwmOut blue_led(p21);
namikosaito 0:77188ca200ce 25
namikosaito 0:77188ca200ce 26 void save_servo(void){ //サーボの振れすぎを防ぐ。
namikosaito 0:77188ca200ce 27 if(out>2250)out=2250;
namikosaito 0:77188ca200ce 28 else if(out<650)out=650;
namikosaito 0:77188ca200ce 29 }
namikosaito 0:77188ca200ce 30
namikosaito 0:77188ca200ce 31
namikosaito 0:77188ca200ce 32 /*関数*/
namikosaito 0:77188ca200ce 33 void field(void); //フィールドの色を見る
namikosaito 0:77188ca200ce 34 int wr_flag,wl_flag;
namikosaito 0:77188ca200ce 35 int lf_downhill(void); //ダウンヒルのライントレース用。他のところでも使ってるけど。cdsの最大値、最小値、その差を出す。
namikosaito 0:77188ca200ce 36 int sa_dh,max_dh,min_dh; //lf_downhillで使う変数。
namikosaito 0:77188ca200ce 37 void back(void); //白線の左右どちらに振れたのか見る。（エンコーダー＋ジャイロ）
namikosaito 1:f9c953ddc87a 38 void back_cds(void); //同上（ｃｄｓ）、上り、リバー用
namikosaito 1:f9c953ddc87a 39 void back_cds_dh(void); //同上、ダウンヒル用
namikosaito 0:77188ca200ce 40 int river_turn(void); //リバーの最初の曲がりをcdsで線を読みとる
namikosaito 0:77188ca200ce 41
namikosaito 0:77188ca200ce 42 int back_r_flag=0;
namikosaito 0:77188ca200ce 43 int back_l_flag=0;
namikosaito 0:77188ca200ce 44
namikosaito 0:77188ca200ce 45 float yoko; //上りラインに対しての左右の振れ
namikosaito 0:77188ca200ce 46 int kakudo;
namikosaito 0:77188ca200ce 47
namikosaito 1:f9c953ddc87a 48
namikosaito 0:77188ca200ce 49
namikosaito 0:77188ca200ce 50 int start_flag=1; //fieldで使用
namikosaito 0:77188ca200ce 51 int rg_flag_1,rg_flag_2,rg_count=0;
namikosaito 0:77188ca200ce 52 int gr_flag_1,gr_flag_2,gr_count=0;
namikosaito 0:77188ca200ce 53
namikosaito 0:77188ca200ce 54 ///////////////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 55
namikosaito 0:77188ca200ce 56
namikosaito 0:77188ca200ce 57 int main(){
namikosaito 0:77188ca200ce 58 mcp.format(7,0);
namikosaito 0:77188ca200ce 59 mcp.frequency(1000000);
namikosaito 0:77188ca200ce 60 pc.baud(115200);
namikosaito 0:77188ca200ce 61 gyro.format(16,3);
namikosaito 0:77188ca200ce 62 Enc.attach(&distance, sp_t);
namikosaito 0:77188ca200ce 63 servo.period_ms(20); //サーボ周期
namikosaito 0:77188ca200ce 64 servo.pulsewidth_us(naka);
namikosaito 1:f9c953ddc87a 65 //while(1){}
namikosaito 0:77188ca200ce 66 /*キャリブレーション*/
namikosaito 0:77188ca200ce 67 calibration();
namikosaito 0:77188ca200ce 68
namikosaito 0:77188ca200ce 69 /*スイッチ切り替え*/
namikosaito 1:f9c953ddc87a 70
namikosaito 1:f9c953ddc87a 71 if(switch_1==1&&switch_2==0&&switch_3==0){ //最初から
namikosaito 0:77188ca200ce 72 #define SLOPE
namikosaito 0:77188ca200ce 73 #define TURN
namikosaito 0:77188ca200ce 74 #define RIVER
namikosaito 0:77188ca200ce 75 #define DOWNHILL
namikosaito 0:77188ca200ce 76 }
namikosaito 0:77188ca200ce 77
namikosaito 1:f9c953ddc87a 78 else if(switch_1==1&&switch_2==1&&switch_3==0){ //リバーから
namikosaito 0:77188ca200ce 79 #define RIVER
namikosaito 0:77188ca200ce 80 #define DOWNHILL
namikosaito 0:77188ca200ce 81 }
namikosaito 0:77188ca200ce 82
namikosaito 1:f9c953ddc87a 83 else if(switch_1==1&&switch_2==1&&switch_3==1){ //ダウンヒルから
namikosaito 0:77188ca200ce 84 #define DOWNHILL
namikosaito 0:77188ca200ce 85 }
namikosaito 1:f9c953ddc87a 86
namikosaito 0:77188ca200ce 87
namikosaito 0:77188ca200ce 88 #ifdef SLOPE
namikosaito 0:77188ca200ce 89 while(1){
namikosaito 1:f9c953ddc87a 90 //pc.printf("%f\r\n",kyori_nobori);
namikosaito 0:77188ca200ce 91 //pc.printf("data=%d red=%d blue=%d yellow=%d white_l=%d ml=%d m=%d mr=%d r=%d\r\n",data_1,red,blue,yellow,white_l,white_ml,white_m,white_mr,white_r);
namikosaito 1:f9c953ddc87a 92 pc.printf("out=%d,dir=%d,sa_dh=%d,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",out,dir,sa_dh,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
namikosaito 0:77188ca200ce 93 //pc.printf("s1=%d,s2=%d,s3=%d,c1=%d,d1=%d,e1=%d\r\n",s1,s2,s3,c1,d1,e1);
namikosaito 0:77188ca200ce 94 field();
namikosaito 0:77188ca200ce 95 LF();
namikosaito 0:77188ca200ce 96 //back();
namikosaito 1:f9c953ddc87a 97 back_cds();
namikosaito 0:77188ca200ce 98 save_servo();
namikosaito 0:77188ca200ce 99 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 100
namikosaito 0:77188ca200ce 101 if(rg_count==0&&gr_count==1)led1=1;
namikosaito 0:77188ca200ce 102 if(rg_count==1&&gr_count==1)led2=1;
namikosaito 0:77188ca200ce 103 if(rg_count==1&&gr_count==2)led3=1;
namikosaito 0:77188ca200ce 104 if(rg_count==2&&gr_count==2)led4=1;
namikosaito 0:77188ca200ce 105 if(rg_count==2&&gr_count==3){
namikosaito 0:77188ca200ce 106 led1=0;
namikosaito 0:77188ca200ce 107 led2=0;
namikosaito 0:77188ca200ce 108 led3=0;
namikosaito 0:77188ca200ce 109 led4=0;
namikosaito 0:77188ca200ce 110 break;
namikosaito 0:77188ca200ce 111 }
namikosaito 0:77188ca200ce 112 }
namikosaito 0:77188ca200ce 113 #endif
namikosaito 0:77188ca200ce 114
namikosaito 0:77188ca200ce 115 /////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 116
namikosaito 0:77188ca200ce 117 #ifdef TURN
namikosaito 0:77188ca200ce 118
namikosaito 0:77188ca200ce 119 while(1){
namikosaito 1:f9c953ddc87a 120 gyro_keisan();
namikosaito 1:f9c953ddc87a 121 out=naka+250;
namikosaito 1:f9c953ddc87a 122 // pc.printf("%d\r\n",fixed_rot_data);
namikosaito 0:77188ca200ce 123 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 124 led4=1;
namikosaito 1:f9c953ddc87a 125 if(fixed_rot_data<-50)break;//前は６０°だった
namikosaito 0:77188ca200ce 126 }
namikosaito 0:77188ca200ce 127
namikosaito 0:77188ca200ce 128 while(1){
namikosaito 0:77188ca200ce 129 judge_color();
namikosaito 1:f9c953ddc87a 130 lf_downhill();
namikosaito 0:77188ca200ce 131 led3=1;
namikosaito 1:f9c953ddc87a 132 if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1||sa_dh<rw)break;
namikosaito 0:77188ca200ce 133 }
namikosaito 0:77188ca200ce 134
namikosaito 0:77188ca200ce 135 #endif
namikosaito 0:77188ca200ce 136
namikosaito 0:77188ca200ce 137 ////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 138
namikosaito 0:77188ca200ce 139 #ifdef RIVER
namikosaito 0:77188ca200ce 140 /*0*/
namikosaito 0:77188ca200ce 141 while(1){ //ライントレース
namikosaito 0:77188ca200ce 142 LF();
namikosaito 0:77188ca200ce 143 out-=50;
namikosaito 0:77188ca200ce 144 judge_color();
namikosaito 0:77188ca200ce 145 //gyro_keisan();
namikosaito 0:77188ca200ce 146 servo.pulsewidth_us(out);
namikosaito 1:f9c953ddc87a 147 pc.printf("kyori_m45=%f,theta=%d linetrace\r\n",kyori_m45,fixed_rot_data);
namikosaito 0:77188ca200ce 148 //pc.printf("out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",out,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
namikosaito 0:77188ca200ce 149
namikosaito 0:77188ca200ce 150 if(blue==1){ //青になったらリバースタート
namikosaito 0:77188ca200ce 151 river_flag=1;
namikosaito 0:77188ca200ce 152 out=naka-250;
namikosaito 0:77188ca200ce 153 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 154 break;
namikosaito 0:77188ca200ce 155 }
namikosaito 0:77188ca200ce 156 }
namikosaito 0:77188ca200ce 157
namikosaito 0:77188ca200ce 158 while(1){
namikosaito 0:77188ca200ce 159 static int magari_flag;
namikosaito 0:77188ca200ce 160 magari_flag++;
namikosaito 0:77188ca200ce 161 if(magari_flag>30)break;
namikosaito 0:77188ca200ce 162 }
namikosaito 0:77188ca200ce 163
namikosaito 0:77188ca200ce 164 /*1*/
namikosaito 0:77188ca200ce 165 while(1){ //初めに曲がる直前まで
namikosaito 0:77188ca200ce 166 //gyro_keisan();
namikosaito 0:77188ca200ce 167
namikosaito 0:77188ca200ce 168 LF(); //基本的にライントレース
namikosaito 0:77188ca200ce 169 out-=50;
namikosaito 0:77188ca200ce 170 if(fixed_rot_data<-60)out=gyro_pid(-45,5,0,0);//out=naka-150; //最初は角度も利用
namikosaito 0:77188ca200ce 171 //if(fixed_rot_data<-75)out=naka-250;
namikosaito 0:77188ca200ce 172
namikosaito 0:77188ca200ce 173 led1=1;
namikosaito 0:77188ca200ce 174 //pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
namikosaito 0:77188ca200ce 175 if(river_flag==1){
namikosaito 0:77188ca200ce 176 kyori_reset();
namikosaito 0:77188ca200ce 177 river_flag=0;
namikosaito 0:77188ca200ce 178 }
namikosaito 0:77188ca200ce 179 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 180 if((river_turn()==1&&kyori_m45>Runder)||kyori>400||kyori_m45>R){ //Rｍｍ進んだらorｃｄｓで直角検知
namikosaito 0:77188ca200ce 181 river_theta=fixed_rot_data;
namikosaito 0:77188ca200ce 182 river_flag=1;
namikosaito 0:77188ca200ce 183 break;
namikosaito 0:77188ca200ce 184 }}
namikosaito 0:77188ca200ce 185
namikosaito 0:77188ca200ce 186 river_keisan(); //角度と距離の補正（いらないかも・・）
namikosaito 0:77188ca200ce 187
namikosaito 0:77188ca200ce 188 /*2*/
namikosaito 0:77188ca200ce 189 while(1){ //向きを９０度に合わせる。
namikosaito 0:77188ca200ce 190 gyro_keisan();
namikosaito 0:77188ca200ce 191 led2=1;
namikosaito 0:77188ca200ce 192 //pc.printf("2 theta=%d\r\n",fixed_rot_data);
namikosaito 1:f9c953ddc87a 193 out=gyro_pid(-90-phai,7,0,0);//705
namikosaito 0:77188ca200ce 194 save_servo();
namikosaito 0:77188ca200ce 195 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 196 if(river_flag==1){
namikosaito 0:77188ca200ce 197 kyori_reset();
namikosaito 0:77188ca200ce 198 river_flag=0;
namikosaito 0:77188ca200ce 199 }
namikosaito 0:77188ca200ce 200 if(fixed_rot_data<-85-phai){ //ジャイロの角度が合ったら
namikosaito 0:77188ca200ce 201 out=naka;
namikosaito 0:77188ca200ce 202 break;
namikosaito 0:77188ca200ce 203 }
namikosaito 0:77188ca200ce 204 }
namikosaito 0:77188ca200ce 205
namikosaito 0:77188ca200ce 206 /*3*/
namikosaito 0:77188ca200ce 207 while(1){ //直進する
namikosaito 0:77188ca200ce 208 led3=1;
namikosaito 0:77188ca200ce 209 gyro_keisan();
namikosaito 1:f9c953ddc87a 210 //pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
namikosaito 0:77188ca200ce 211 out=gyro_pid(-90-phai,6,0.1,0);
namikosaito 0:77188ca200ce 212 save_servo();
namikosaito 0:77188ca200ce 213 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 214 if(kyori_m90>Z){ //Zmm進んだら
namikosaito 0:77188ca200ce 215 out=naka-200; //前は100
namikosaito 0:77188ca200ce 216 white_r=0;
namikosaito 0:77188ca200ce 217 white_l=0;
namikosaito 0:77188ca200ce 218 break;
namikosaito 0:77188ca200ce 219 }
namikosaito 0:77188ca200ce 220 }
namikosaito 0:77188ca200ce 221
namikosaito 0:77188ca200ce 222 /*4*/
namikosaito 0:77188ca200ce 223 while(1){ //白線を見つけるまでそのまま
namikosaito 0:77188ca200ce 224 gyro_keisan();
namikosaito 0:77188ca200ce 225 led4=1;
namikosaito 0:77188ca200ce 226 judge_color();
namikosaito 0:77188ca200ce 227 LF();
namikosaito 0:77188ca200ce 228 lf_downhill();
namikosaito 1:f9c953ddc87a 229 pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,sa_dh);
namikosaito 1:f9c953ddc87a 230
namikosaito 1:f9c953ddc87a 231 static int riv;
namikosaito 1:f9c953ddc87a 232 if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)riv++;
namikosaito 0:77188ca200ce 233
namikosaito 0:77188ca200ce 234 if(kyori_m90>T)break; //行き過ぎ
namikosaito 1:f9c953ddc87a 235 if(riv>3||sa_dh>bw){ //白線検知
namikosaito 0:77188ca200ce 236 break;
namikosaito 0:77188ca200ce 237 }
namikosaito 0:77188ca200ce 238 }
namikosaito 0:77188ca200ce 239
namikosaito 0:77188ca200ce 240 if(kyori_m90>T){
namikosaito 0:77188ca200ce 241 servo.pulsewidth_us(1100);
namikosaito 0:77188ca200ce 242 wait(0.5);
namikosaito 0:77188ca200ce 243 }
namikosaito 0:77188ca200ce 244
namikosaito 0:77188ca200ce 245 /*5*/
namikosaito 0:77188ca200ce 246 while(1){ //ライントレースに戻る
namikosaito 0:77188ca200ce 247 gyro_keisan();
namikosaito 0:77188ca200ce 248 LF();
namikosaito 0:77188ca200ce 249 back_cds();
namikosaito 0:77188ca200ce 250 out-=50;
namikosaito 0:77188ca200ce 251 if(fixed_rot_data<-45)out-=50;
namikosaito 0:77188ca200ce 252 if(fixed_rot_data<-75)out-=100;
namikosaito 0:77188ca200ce 253
namikosaito 0:77188ca200ce 254 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 255 judge_color();
namikosaito 0:77188ca200ce 256 led1=0;
namikosaito 0:77188ca200ce 257 led2=0;
namikosaito 0:77188ca200ce 258 led3=0;
namikosaito 0:77188ca200ce 259 led4=0;
namikosaito 0:77188ca200ce 260 if(red==1){kyori_reset();break;}
namikosaito 0:77188ca200ce 261 }
namikosaito 0:77188ca200ce 262
namikosaito 0:77188ca200ce 263 #endif
namikosaito 0:77188ca200ce 264
namikosaito 0:77188ca200ce 265 ////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 266
namikosaito 0:77188ca200ce 267 #ifdef DOWNHILL
namikosaito 0:77188ca200ce 268
namikosaito 0:77188ca200ce 269 while(1){
namikosaito 0:77188ca200ce 270 static int q;
namikosaito 0:77188ca200ce 271 q++;
namikosaito 0:77188ca200ce 272 pc.printf("a kyori_dh=%f, theta=%d, out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",kyori_dh,fixed_rot_data,out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
namikosaito 0:77188ca200ce 273 judge_color();
namikosaito 0:77188ca200ce 274 LF();
namikosaito 0:77188ca200ce 275 back_cds();
namikosaito 0:77188ca200ce 276 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 277 if(yellow==1&&q>30){ //黄色に入る
namikosaito 0:77188ca200ce 278 kyori_reset();
namikosaito 0:77188ca200ce 279 blue_led=1;
namikosaito 0:77188ca200ce 280 downhill=1;
namikosaito 0:77188ca200ce 281 break;
namikosaito 0:77188ca200ce 282 }
namikosaito 0:77188ca200ce 283 }
namikosaito 0:77188ca200ce 284
namikosaito 0:77188ca200ce 285 while(1){
namikosaito 0:77188ca200ce 286 LF();
namikosaito 0:77188ca200ce 287 lf_downhill();
namikosaito 0:77188ca200ce 288 judge_color();
namikosaito 1:f9c953ddc87a 289 back_cds_dh(); //ダウンヒル専用
namikosaito 0:77188ca200ce 290 pc.printf("kyori_dh=%f,sa_dh=%d, theta=%d, out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",kyori_dh,sa_dh,fixed_rot_data,out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
namikosaito 0:77188ca200ce 291
namikosaito 0:77188ca200ce 292 //gyro_keisan();
namikosaito 0:77188ca200ce 293
namikosaito 1:f9c953ddc87a 294 if(kyori_dh>600&&kyori_dh<900){out-=75;led1=1;}
namikosaito 1:f9c953ddc87a 295 else if(kyori_dh>1200&&kyori_dh<1700){out+=25;led2=1;}
namikosaito 1:f9c953ddc87a 296 else if(kyori_dh>1700&&kyori_dh<2800){out+=75;led3=1;}
namikosaito 1:f9c953ddc87a 297 else{
namikosaito 0:77188ca200ce 298 led1=0;
namikosaito 0:77188ca200ce 299 led2=0;
namikosaito 0:77188ca200ce 300 led3=0;
namikosaito 0:77188ca200ce 301 }
namikosaito 1:f9c953ddc87a 302 /*
namikosaito 0:77188ca200ce 303 if(kyori_dh<600){
namikosaito 1:f9c953ddc87a 304 out-=50;
namikosaito 0:77188ca200ce 305 }
namikosaito 0:77188ca200ce 306
namikosaito 0:77188ca200ce 307 else if(kyori_dh>600&&kyori_dh<900){
namikosaito 0:77188ca200ce 308 led3=1;
namikosaito 1:f9c953ddc87a 309 out-=100;
namikosaito 0:77188ca200ce 310 if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){
namikosaito 0:77188ca200ce 311 out=1300;
namikosaito 0:77188ca200ce 312 out-=(yw-sa_dh)*0.15;
namikosaito 0:77188ca200ce 313 }
namikosaito 0:77188ca200ce 314 }
namikosaito 0:77188ca200ce 315
namikosaito 0:77188ca200ce 316 else if(kyori_dh>1200&&kyori_dh<1700){
namikosaito 0:77188ca200ce 317 out+=50;
namikosaito 0:77188ca200ce 318 }
namikosaito 0:77188ca200ce 319
namikosaito 0:77188ca200ce 320 else if(kyori_dh>1700&&kyori_dh<2700){
namikosaito 0:77188ca200ce 321 led3=1;
namikosaito 0:77188ca200ce 322 out+=180;
namikosaito 0:77188ca200ce 323 if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){
namikosaito 0:77188ca200ce 324 out=1900;
namikosaito 0:77188ca200ce 325 out+=(yw-sa_dh)*0.15;
namikosaito 0:77188ca200ce 326 }
namikosaito 0:77188ca200ce 327 }
namikosaito 1:f9c953ddc87a 328 */
namikosaito 0:77188ca200ce 329 /*
namikosaito 0:77188ca200ce 330 if((kyori_dh>2000&&kyori_dh<2700&&(white_r==0&&white_mr==0&&white_m==0))||((kyori_dh>2200&&kyori_dh<2700)&&fixed_rot_data>15)){
namikosaito 0:77188ca200ce 331 while(1){
namikosaito 0:77188ca200ce 332 LF();
namikosaito 0:77188ca200ce 333 lf_downhill();
namikosaito 0:77188ca200ce 334 out=2100;
namikosaito 0:77188ca200ce 335 if(fixed_rot_data<0)out=2000;
namikosaito 0:77188ca200ce 336 if(fixed_rot_data<-15)out=1900;
namikosaito 0:77188ca200ce 337 if(fixed_rot_data<-30)out=1700;
namikosaito 0:77188ca200ce 338 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 339 if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)||sa_dh>yw)break;
namikosaito 0:77188ca200ce 340 }
namikosaito 0:77188ca200ce 341 }
namikosaito 0:77188ca200ce 342 */
namikosaito 0:77188ca200ce 343 save_servo();
namikosaito 0:77188ca200ce 344 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 345 }
namikosaito 0:77188ca200ce 346
namikosaito 0:77188ca200ce 347 #endif
namikosaito 0:77188ca200ce 348
namikosaito 0:77188ca200ce 349 }
namikosaito 0:77188ca200ce 350
namikosaito 0:77188ca200ce 351
namikosaito 0:77188ca200ce 352
namikosaito 0:77188ca200ce 353
namikosaito 0:77188ca200ce 354 //////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 355 int river_turn(void){
namikosaito 0:77188ca200ce 356 static int ritu,ritu_flag;
namikosaito 0:77188ca200ce 357 ritu=0;
namikosaito 0:77188ca200ce 358 ritu_flag=0;
namikosaito 0:77188ca200ce 359 if(s1-s12>bw&&s1-s11>bw)ritu++;
namikosaito 0:77188ca200ce 360 if(s2-s12>bw&&s2-s11>bw)ritu++;
namikosaito 0:77188ca200ce 361 if(s3-s12>bw&&s3-s11>bw)ritu++;
namikosaito 0:77188ca200ce 362 if(s4-s12>bw&&s4-s11>bw)ritu++;
namikosaito 0:77188ca200ce 363 if(s5-s12>bw&&s5-s11>bw)ritu++;
namikosaito 0:77188ca200ce 364
namikosaito 0:77188ca200ce 365 if(ritu>3)ritu_flag=1;
namikosaito 0:77188ca200ce 366 //pc.printf("ritu=%d",ritu);
namikosaito 0:77188ca200ce 367
namikosaito 0:77188ca200ce 368 return ritu_flag;
namikosaito 0:77188ca200ce 369 }
namikosaito 0:77188ca200ce 370
namikosaito 0:77188ca200ce 371 int lf_downhill(void){
namikosaito 0:77188ca200ce 372 /*cdsの値の最大*/
namikosaito 0:77188ca200ce 373 max_dh=s1;
namikosaito 0:77188ca200ce 374 if(max_dh<s2)max_dh=s2;
namikosaito 0:77188ca200ce 375 if(max_dh<s3)max_dh=s3;
namikosaito 0:77188ca200ce 376 if(max_dh<s4)max_dh=s4;
namikosaito 0:77188ca200ce 377 if(max_dh<s5)max_dh=s5;
namikosaito 0:77188ca200ce 378 if(max_dh<s6)max_dh=s6;
namikosaito 0:77188ca200ce 379 if(max_dh<s7)max_dh=s7;
namikosaito 0:77188ca200ce 380 if(max_dh<s8)max_dh=s8;
namikosaito 0:77188ca200ce 381 if(max_dh<s9)max_dh=s9;
namikosaito 0:77188ca200ce 382 if(max_dh<s10)max_dh=s10;
namikosaito 0:77188ca200ce 383 if(max_dh<s11)max_dh=s11;
namikosaito 0:77188ca200ce 384 if(max_dh<s12)max_dh=s12;
namikosaito 0:77188ca200ce 385
namikosaito 0:77188ca200ce 386 /*cdsの値の最小*/
namikosaito 0:77188ca200ce 387 min_dh=s1;
namikosaito 0:77188ca200ce 388 if(min_dh>s2)min_dh=s2;
namikosaito 0:77188ca200ce 389 if(min_dh>s3)min_dh=s3;
namikosaito 0:77188ca200ce 390 if(min_dh>s4)min_dh=s4;
namikosaito 0:77188ca200ce 391 if(min_dh>s5)min_dh=s5;
namikosaito 0:77188ca200ce 392 if(min_dh>s6)min_dh=s6;
namikosaito 0:77188ca200ce 393 if(min_dh>s7)min_dh=s7;
namikosaito 0:77188ca200ce 394 if(min_dh>s8)min_dh=s8;
namikosaito 0:77188ca200ce 395 if(min_dh>s9)min_dh=s9;
namikosaito 0:77188ca200ce 396 if(min_dh>s10)min_dh=s10;
namikosaito 0:77188ca200ce 397 if(min_dh>s11)min_dh=s11;
namikosaito 0:77188ca200ce 398 if(min_dh>s12)min_dh=s12;
namikosaito 0:77188ca200ce 399
namikosaito 0:77188ca200ce 400 sa_dh=max_dh-min_dh;
namikosaito 0:77188ca200ce 401 return sa_dh;
namikosaito 0:77188ca200ce 402 }
namikosaito 0:77188ca200ce 403
namikosaito 0:77188ca200ce 404 void back_cds(void){ //ｃｄｓでラインのどちらにずれたのか検知する。
namikosaito 0:77188ca200ce 405 Find_White();
namikosaito 0:77188ca200ce 406
namikosaito 0:77188ca200ce 407 static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop;
namikosaito 0:77188ca200ce 408
namikosaito 0:77188ca200ce 409 if(now==-3||now==-4){
namikosaito 0:77188ca200ce 410 back_cds_l=0;
namikosaito 0:77188ca200ce 411 back_cds_r=1;
namikosaito 0:77188ca200ce 412 }
namikosaito 0:77188ca200ce 413 if(back_cds_r==1&&(now==-5||now==-6)){
namikosaito 0:77188ca200ce 414 back_cds_r2=1;
namikosaito 0:77188ca200ce 415 back_cds_r=0;
namikosaito 0:77188ca200ce 416 }
namikosaito 0:77188ca200ce 417
namikosaito 1:f9c953ddc87a 418 if(back_cds_r2==1&&now!=-6&&now!=-5){
namikosaito 0:77188ca200ce 419 while(1){
namikosaito 0:77188ca200ce 420 LF();
namikosaito 0:77188ca200ce 421 gyro_keisan();
namikosaito 0:77188ca200ce 422 lf_downhill();
namikosaito 0:77188ca200ce 423 Find_White(); //いらないはず
namikosaito 0:77188ca200ce 424 // out=1100;
namikosaito 0:77188ca200ce 425 pc.printf("hidari now %d\r\n",now);
namikosaito 0:77188ca200ce 426 out=1100;
namikosaito 0:77188ca200ce 427 save_servo();
namikosaito 0:77188ca200ce 428 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 429 judge_color();
namikosaito 0:77188ca200ce 430 field();
namikosaito 0:77188ca200ce 431 if(now==-6)flag_loop=1;
namikosaito 0:77188ca200ce 432 if((flag_loop==1&&(now==-4||now==-5))||white_mr==1||white_m==1){
namikosaito 0:77188ca200ce 433 back_cds_r2=0;
namikosaito 0:77188ca200ce 434 flag_loop=0;
namikosaito 0:77188ca200ce 435 break;
namikosaito 0:77188ca200ce 436 }
namikosaito 1:f9c953ddc87a 437 }
namikosaito 1:f9c953ddc87a 438
namikosaito 1:f9c953ddc87a 439 if(now==3||now==4){
namikosaito 1:f9c953ddc87a 440 back_cds_l=1;
namikosaito 1:f9c953ddc87a 441 back_cds_r=0;
namikosaito 1:f9c953ddc87a 442 }
namikosaito 1:f9c953ddc87a 443
namikosaito 1:f9c953ddc87a 444 if(back_cds_l==1&&(now==5||now==6)){
namikosaito 1:f9c953ddc87a 445 back_cds_l=0;
namikosaito 1:f9c953ddc87a 446 back_cds_l2=1;
namikosaito 1:f9c953ddc87a 447 }
namikosaito 1:f9c953ddc87a 448
namikosaito 1:f9c953ddc87a 449 if(back_cds_l2==1&&now!=6&&now!=5){
namikosaito 1:f9c953ddc87a 450 while(1){
namikosaito 1:f9c953ddc87a 451 LF();
namikosaito 1:f9c953ddc87a 452 gyro_keisan();
namikosaito 1:f9c953ddc87a 453 Find_White();
namikosaito 1:f9c953ddc87a 454 lf_downhill();
namikosaito 1:f9c953ddc87a 455 pc.printf("migi now=%d\r\n",now);
namikosaito 1:f9c953ddc87a 456 out=2100;
namikosaito 1:f9c953ddc87a 457 save_servo();
namikosaito 1:f9c953ddc87a 458 servo.pulsewidth_us(out);//1900
namikosaito 1:f9c953ddc87a 459 judge_color();
namikosaito 1:f9c953ddc87a 460 field();
namikosaito 1:f9c953ddc87a 461 if(now==6)flag_loop=1;
namikosaito 1:f9c953ddc87a 462 if((flag_loop==1&&(now==4||now==5))||white_ml==1||white_m==1){ //調べて
namikosaito 1:f9c953ddc87a 463 back_cds_l2=0;
namikosaito 1:f9c953ddc87a 464 flag_loop=0;
namikosaito 1:f9c953ddc87a 465 break;
namikosaito 1:f9c953ddc87a 466 }
namikosaito 1:f9c953ddc87a 467 }
namikosaito 1:f9c953ddc87a 468 }
namikosaito 1:f9c953ddc87a 469 }
namikosaito 1:f9c953ddc87a 470 }
namikosaito 1:f9c953ddc87a 471
namikosaito 1:f9c953ddc87a 472
namikosaito 1:f9c953ddc87a 473 void back_cds_dh(void){ //ｃｄｓでラインのどちらにずれたのか検知する。
namikosaito 1:f9c953ddc87a 474 Find_White();
namikosaito 1:f9c953ddc87a 475
namikosaito 1:f9c953ddc87a 476 static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop;
namikosaito 1:f9c953ddc87a 477 if(kyori_dh>50){
namikosaito 1:f9c953ddc87a 478 static int f;
namikosaito 1:f9c953ddc87a 479 if(sa_dh>yw+50){
namikosaito 1:f9c953ddc87a 480 f=now;
namikosaito 1:f9c953ddc87a 481 }
namikosaito 1:f9c953ddc87a 482 else if(sa_dh<yw&&f>2){
namikosaito 1:f9c953ddc87a 483 out=naka+300;
namikosaito 1:f9c953ddc87a 484 }
namikosaito 1:f9c953ddc87a 485 else if(sa_dh<yw&&f<-2){
namikosaito 1:f9c953ddc87a 486 out=naka-300;
namikosaito 1:f9c953ddc87a 487 }
namikosaito 1:f9c953ddc87a 488 }
namikosaito 1:f9c953ddc87a 489
namikosaito 1:f9c953ddc87a 490 if(now==-3||now==-4){
namikosaito 1:f9c953ddc87a 491 back_cds_l=0;
namikosaito 1:f9c953ddc87a 492 back_cds_r=1;
namikosaito 1:f9c953ddc87a 493 }
namikosaito 1:f9c953ddc87a 494 if(back_cds_r==1&&(now==-5||now==-6)){
namikosaito 1:f9c953ddc87a 495 back_cds_r2=1;
namikosaito 1:f9c953ddc87a 496 back_cds_r=0;
namikosaito 1:f9c953ddc87a 497 }
namikosaito 1:f9c953ddc87a 498
namikosaito 1:f9c953ddc87a 499 if(back_cds_r2==1&&now!=-6&&now!=-5){//||(downhill==1&&sa_dh<yw)){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){ //変更
namikosaito 1:f9c953ddc87a 500 while(1){
namikosaito 1:f9c953ddc87a 501 LF();
namikosaito 1:f9c953ddc87a 502 gyro_keisan();
namikosaito 1:f9c953ddc87a 503 lf_downhill();
namikosaito 1:f9c953ddc87a 504 Find_White(); //いらないはず
namikosaito 1:f9c953ddc87a 505 // out=1100;
namikosaito 1:f9c953ddc87a 506 pc.printf("hidari now %d\r\n",now);
namikosaito 1:f9c953ddc87a 507 out=1100;
namikosaito 1:f9c953ddc87a 508 save_servo();
namikosaito 1:f9c953ddc87a 509 servo.pulsewidth_us(out);
namikosaito 1:f9c953ddc87a 510 judge_color();
namikosaito 1:f9c953ddc87a 511 field();
namikosaito 1:f9c953ddc87a 512 if(now==-6)flag_loop=1;
namikosaito 1:f9c953ddc87a 513 if((flag_loop==1&&(now==-4||now==-5))||white_mr==1||white_m==1){
namikosaito 1:f9c953ddc87a 514 back_cds_r2=0;
namikosaito 1:f9c953ddc87a 515 flag_loop=0;
namikosaito 1:f9c953ddc87a 516 break;
namikosaito 1:f9c953ddc87a 517 }
namikosaito 1:f9c953ddc87a 518
namikosaito 1:f9c953ddc87a 519 if(downhill==1){
namikosaito 0:77188ca200ce 520 static int yw_flag;
namikosaito 0:77188ca200ce 521 if(sa_dh>yw)yw_flag++;
namikosaito 0:77188ca200ce 522 if(yw_flag>10){
namikosaito 0:77188ca200ce 523 yw_flag=0;
namikosaito 0:77188ca200ce 524 back_cds_r2=0;
namikosaito 0:77188ca200ce 525 flag_loop=0;
namikosaito 1:f9c953ddc87a 526 break;}
namikosaito 1:f9c953ddc87a 527 }
namikosaito 0:77188ca200ce 528 }
namikosaito 0:77188ca200ce 529 }
namikosaito 0:77188ca200ce 530
namikosaito 0:77188ca200ce 531 if(now==3||now==4){
namikosaito 0:77188ca200ce 532 back_cds_l=1;
namikosaito 0:77188ca200ce 533 back_cds_r=0;
namikosaito 0:77188ca200ce 534 }
namikosaito 0:77188ca200ce 535
namikosaito 0:77188ca200ce 536 if(back_cds_l==1&&(now==5||now==6)){
namikosaito 0:77188ca200ce 537 back_cds_l=0;
namikosaito 0:77188ca200ce 538 back_cds_l2=1;
namikosaito 0:77188ca200ce 539 }
namikosaito 0:77188ca200ce 540
namikosaito 1:f9c953ddc87a 541 if(back_cds_l2==1&&now!=6&&now!=5){//||(downhill==1&&sa_dh<yw)){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){
namikosaito 0:77188ca200ce 542 while(1){
namikosaito 0:77188ca200ce 543 LF();
namikosaito 0:77188ca200ce 544 gyro_keisan();
namikosaito 0:77188ca200ce 545 Find_White();
namikosaito 0:77188ca200ce 546 lf_downhill();
namikosaito 0:77188ca200ce 547 pc.printf("migi now=%d\r\n",now);
namikosaito 0:77188ca200ce 548 out=2100;
namikosaito 0:77188ca200ce 549 save_servo();
namikosaito 0:77188ca200ce 550 servo.pulsewidth_us(out);//1900
namikosaito 0:77188ca200ce 551 judge_color();
namikosaito 0:77188ca200ce 552 field();
namikosaito 0:77188ca200ce 553 if(now==6)flag_loop=1;
namikosaito 0:77188ca200ce 554 if((flag_loop==1&&(now==4||now==5))||white_ml==1||white_m==1){ //調べて
namikosaito 0:77188ca200ce 555 back_cds_l2=0;
namikosaito 0:77188ca200ce 556 flag_loop=0;
namikosaito 0:77188ca200ce 557 break;
namikosaito 0:77188ca200ce 558 }
namikosaito 1:f9c953ddc87a 559
namikosaito 1:f9c953ddc87a 560 if(downhill==1){
namikosaito 0:77188ca200ce 561 static int yw_flag;
namikosaito 0:77188ca200ce 562 if(sa_dh>yw)yw_flag++;
namikosaito 0:77188ca200ce 563 if(yw_flag>10){
namikosaito 0:77188ca200ce 564 back_cds_l2=0;
namikosaito 0:77188ca200ce 565 flag_loop=0;
namikosaito 0:77188ca200ce 566 yw_flag=0;
namikosaito 1:f9c953ddc87a 567 break;}
namikosaito 1:f9c953ddc87a 568 }
namikosaito 0:77188ca200ce 569 }
namikosaito 0:77188ca200ce 570 }
namikosaito 1:f9c953ddc87a 571 }
namikosaito 1:f9c953ddc87a 572
namikosaito 1:f9c953ddc87a 573
namikosaito 0:77188ca200ce 574
namikosaito 0:77188ca200ce 575 void back(void){ //エンコーダーとジャイロで左右を見分ける方法（使ってない）
namikosaito 0:77188ca200ce 576 if(white_mr==1){
namikosaito 0:77188ca200ce 577 back_l_flag=0;
namikosaito 0:77188ca200ce 578 back_r_flag=1;
namikosaito 0:77188ca200ce 579 }
namikosaito 0:77188ca200ce 580 if(back_r_flag==1&&white_r==1){
namikosaito 0:77188ca200ce 581 while(1){
namikosaito 0:77188ca200ce 582 if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
namikosaito 0:77188ca200ce 583 if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
namikosaito 0:77188ca200ce 584 if(rg_count==1&&gr_count==1)yoko=kyori_45x;
namikosaito 0:77188ca200ce 585
namikosaito 0:77188ca200ce 586
namikosaito 0:77188ca200ce 587 out=1300;
namikosaito 0:77188ca200ce 588 pc.printf("hidari %d\r\n",out);
namikosaito 0:77188ca200ce 589 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 590 judge_color();
namikosaito 0:77188ca200ce 591 field();
namikosaito 0:77188ca200ce 592 if(white_ml==1||white_l==1||white_m==1){
namikosaito 0:77188ca200ce 593 back_r_flag=0;
namikosaito 0:77188ca200ce 594 break;
namikosaito 0:77188ca200ce 595 }
namikosaito 0:77188ca200ce 596 static int yoko_flag;
namikosaito 0:77188ca200ce 597 if(yoko<-5)yoko_flag=2;
namikosaito 0:77188ca200ce 598 if(yoko_flag==2&&yoko>0)break;
namikosaito 0:77188ca200ce 599 if(yoko>5)yoko_flag=3;
namikosaito 0:77188ca200ce 600 if(yoko_flag==3&&yoko<0)break;
namikosaito 0:77188ca200ce 601 }
namikosaito 0:77188ca200ce 602 }
namikosaito 0:77188ca200ce 603
namikosaito 0:77188ca200ce 604 if(white_ml==1){
namikosaito 0:77188ca200ce 605 back_r_flag=0;
namikosaito 0:77188ca200ce 606 back_l_flag=1;
namikosaito 0:77188ca200ce 607 }
namikosaito 0:77188ca200ce 608 if(back_l_flag==1&&white_l==1){
namikosaito 0:77188ca200ce 609 while(1){
namikosaito 0:77188ca200ce 610
namikosaito 0:77188ca200ce 611 if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
namikosaito 0:77188ca200ce 612 if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
namikosaito 0:77188ca200ce 613 if(rg_count==1&&gr_count==1)yoko=kyori_45x;
namikosaito 0:77188ca200ce 614
namikosaito 0:77188ca200ce 615 out=1700;
namikosaito 0:77188ca200ce 616 pc.printf("migi %d\r\n",out);
namikosaito 0:77188ca200ce 617 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 618 judge_color();
namikosaito 0:77188ca200ce 619 field();
namikosaito 0:77188ca200ce 620 if(white_mr==1||white_r==1||white_m==1){
namikosaito 0:77188ca200ce 621 back_r_flag=0;
namikosaito 0:77188ca200ce 622 break;
namikosaito 0:77188ca200ce 623 }
namikosaito 0:77188ca200ce 624 static int yoko_flag;
namikosaito 0:77188ca200ce 625 if(yoko<-5)yoko_flag=2;
namikosaito 0:77188ca200ce 626 if(yoko_flag==2&&yoko>0)break;
namikosaito 0:77188ca200ce 627 if(yoko>5)yoko_flag=3;
namikosaito 0:77188ca200ce 628 if(yoko_flag==3&&yoko<0)break;
namikosaito 0:77188ca200ce 629 }
namikosaito 0:77188ca200ce 630 }
namikosaito 0:77188ca200ce 631 }
namikosaito 0:77188ca200ce 632
namikosaito 0:77188ca200ce 633
namikosaito 0:77188ca200ce 634 void field(void){ //上りにおいてフィールドの色を判断し、数えていく。
namikosaito 0:77188ca200ce 635 judge_color();
namikosaito 0:77188ca200ce 636
namikosaito 0:77188ca200ce 637 if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)&&downhill==0)kyori_reset();
namikosaito 0:77188ca200ce 638 if((gr_flag_1==1&&red==1&&kyori_nobori>500)||(start_flag==1&&red==1)){
namikosaito 0:77188ca200ce 639 gr_flag_2=1;
namikosaito 0:77188ca200ce 640 }
namikosaito 0:77188ca200ce 641 if(gr_flag_2==1){
namikosaito 0:77188ca200ce 642 kyori_nobori_reset();
namikosaito 0:77188ca200ce 643 start_flag=0;
namikosaito 0:77188ca200ce 644 gr_flag_1=0;
namikosaito 0:77188ca200ce 645 gr_flag_2=0;
namikosaito 0:77188ca200ce 646 rg_flag_1=1;
namikosaito 0:77188ca200ce 647 gr_count++;
namikosaito 0:77188ca200ce 648 }
namikosaito 0:77188ca200ce 649 if(blue==1&&rg_flag_1==1&&kyori_nobori>500){
namikosaito 0:77188ca200ce 650 rg_flag_2=1;
namikosaito 0:77188ca200ce 651 }
namikosaito 0:77188ca200ce 652 if(rg_flag_2==1){
namikosaito 0:77188ca200ce 653 kyori_nobori_reset();
namikosaito 0:77188ca200ce 654 rg_flag_1=0;
namikosaito 0:77188ca200ce 655 rg_flag_2=0;
namikosaito 0:77188ca200ce 656 gr_flag_1=1;
namikosaito 0:77188ca200ce 657 rg_count++;
namikosaito 0:77188ca200ce 658 }
namikosaito 0:77188ca200ce 659 }