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@10:82f5a6e95dc2, 2016-03-09 (annotated)

Committer:
namikosaito
Date:
Wed Mar 09 15:44:32 2016 +0000
Revision:
10:82f5a6e95dc2
Parent:
9:dbfcdf1b20c1
Child:
11:bf37710355ff
3_10?????????????; ????????????????????????????????????

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