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@5:f90bd93f8558, 2016-03-02 (annotated)

Committer:
namikosaito
Date:
Wed Mar 02 05:18:58 2016 +0000
Revision:
5:f90bd93f8558
Parent:
4:795055e031c3
Child:
6:c28aa7d26eba
3_2???????????

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