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@11:bf37710355ff, 2016-03-23 (annotated)

Committer:
kanakin
Date:
Wed Mar 23 16:31:20 2016 +0000
Revision:
11:bf37710355ff
Parent:
10:82f5a6e95dc2 
ziguzagu

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
kanakin 11:bf37710355ff 12 #define RIVER2
kanakin 11:bf37710355ff 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
kanakin 11:bf37710355ff 54 int sikii=2000; //cdsの青と白の閾値
kanakin 11:bf37710355ff 55 int ko=0;
kanakin 11:bf37710355ff 56
kanakin 11:bf37710355ff 57 int alpha;
kanakin 11:bf37710355ff 58 int out_alpha;
kanakin 11:bf37710355ff 59
kanakin 11:bf37710355ff 60
kanakin 11:bf37710355ff 61 //int max_dasi(void){
kanakin 11:bf37710355ff 62 /*cdsの値の最大*/
kanakin 11:bf37710355ff 63 /* max_dh=1;
kanakin 11:bf37710355ff 64 if(max_dh<s2)max_dh=2;
kanakin 11:bf37710355ff 65 if(max_dh<s3)max_dh=3;
kanakin 11:bf37710355ff 66 if(max_dh<s4)max_dh=4;
kanakin 11:bf37710355ff 67 if(max_dh<s5)max_dh=5;
kanakin 11:bf37710355ff 68 if(max_dh<s6)max_dh=6;
kanakin 11:bf37710355ff 69 if(max_dh<s7)max_dh=7;
kanakin 11:bf37710355ff 70 if(max_dh<s8)max_dh=8;
kanakin 11:bf37710355ff 71 if(max_dh<s9)max_dh=9;
kanakin 11:bf37710355ff 72 if(max_dh<s10)max_dh=10;
kanakin 11:bf37710355ff 73 if(max_dh<s11)max_dh=11;
kanakin 11:bf37710355ff 74 if(max_dh<s12)max_dh=12;
kanakin 11:bf37710355ff 75 return max_dh;
kanakin 11:bf37710355ff 76 }*/
kanakin 11:bf37710355ff 77
kanakin 11:bf37710355ff 78 //ジャイロ使わないとき
kanakin 11:bf37710355ff 79 /*int hidari_magari1(void){
kanakin 11:bf37710355ff 80 gyro_keisan();
kanakin 11:bf37710355ff 81 kosuu();
kanakin 11:bf37710355ff 82 if(ko=){
kanakin 11:bf37710355ff 83 if(s1>sikii){
kanakin 11:bf37710355ff 84 if(s2>sikii)}
kanakin 11:bf37710355ff 85 }
kanakin 11:bf37710355ff 86 */
kanakin 11:bf37710355ff 87 //ジャイロ使うとき
kanakin 11:bf37710355ff 88 void hidari_magari(){ //最大値がある側とは逆側の端のcdsが白になったら曲がる
kanakin 11:bf37710355ff 89
kanakin 11:bf37710355ff 90 if(now==-1||now==-2||now==-3||now==-4||now==-5||now==-6){
kanakin 11:bf37710355ff 91 alpha=-45-fixed_rot_data;
kanakin 11:bf37710355ff 92 out_alpha=alpha*5;//出力5で1度.１度で出力５
kanakin 11:bf37710355ff 93 out=naka-out_alpha+90*5+100;
kanakin 11:bf37710355ff 94 }
kanakin 11:bf37710355ff 95 if(now==1||now==2||now==3||now==4||now==5||now==6){
kanakin 11:bf37710355ff 96 alpha=-45-fixed_rot_data;
kanakin 11:bf37710355ff 97 out_alpha=alpha*5;
kanakin 11:bf37710355ff 98 out=naka-out_alpha+90*5+100;
kanakin 11:bf37710355ff 99 }
kanakin 11:bf37710355ff 100 }
kanakin 11:bf37710355ff 101
kanakin 11:bf37710355ff 102 void migi_magari(){
kanakin 11:bf37710355ff 103 if(now==-1||now==-2||now==-3||now==-4||now==-5||now==-6){
kanakin 11:bf37710355ff 104 alpha=-135-fixed_rot_data;
kanakin 11:bf37710355ff 105 out_alpha=alpha*5;
kanakin 11:bf37710355ff 106 out=naka-out_alpha-90*5-150;
kanakin 11:bf37710355ff 107 }
kanakin 11:bf37710355ff 108 if(now==1||now==2||now==3||now==4||now==5||now==6){
kanakin 11:bf37710355ff 109 alpha=-135-fixed_rot_data;
kanakin 11:bf37710355ff 110 out_alpha=alpha*5;
kanakin 11:bf37710355ff 111 out=naka-out_alpha-90*5-150;
kanakin 11:bf37710355ff 112 }
kanakin 11:bf37710355ff 113 }
kanakin 11:bf37710355ff 114
kanakin 11:bf37710355ff 115
kanakin 11:bf37710355ff 116 int kosuu(){
kanakin 11:bf37710355ff 117 int ko;
kanakin 11:bf37710355ff 118 if(s3>sikii&&s4>sikii&&s5>sikii)ko=3;
kanakin 11:bf37710355ff 119 else if(s4>sikii&&s5>sikii&&s6>sikii)ko=4;
kanakin 11:bf37710355ff 120 else if(s5>sikii&&s6>sikii&&s7>sikii)ko=5;
kanakin 11:bf37710355ff 121 else if(s6>sikii&&s7>sikii&&s8>sikii)ko=6;
kanakin 11:bf37710355ff 122 else if(s7>sikii&&s8>sikii&&s9>sikii)ko=7;
kanakin 11:bf37710355ff 123 else if(s8>sikii&&s9>sikii&&s10>sikii)ko=8;
kanakin 11:bf37710355ff 124 return ko;
kanakin 11:bf37710355ff 125 }
kanakin 11:bf37710355ff 126
namikosaito 0:77188ca200ce 127 ///////////////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 128
namikosaito 0:77188ca200ce 129
namikosaito 0:77188ca200ce 130 int main(){
namikosaito 0:77188ca200ce 131 mcp.format(7,0);
namikosaito 0:77188ca200ce 132 mcp.frequency(1000000);
namikosaito 0:77188ca200ce 133 pc.baud(115200);
namikosaito 0:77188ca200ce 134 gyro.format(16,3);
namikosaito 0:77188ca200ce 135 Enc.attach(&distance, sp_t);
namikosaito 0:77188ca200ce 136 servo.period_ms(20); //サーボ周期
namikosaito 0:77188ca200ce 137 servo.pulsewidth_us(naka);
namikosaito 8:340da692d865 138
namikosaito 0:77188ca200ce 139 /*キャリブレーション*/
namikosaito 0:77188ca200ce 140 calibration();
kanakin 11:bf37710355ff 141 //while(1){if(push_switch==1)break;}
namikosaito 8:340da692d865 142
namikosaito 6:c28aa7d26eba 143 /*ファイル*/
kanakin 11:bf37710355ff 144 /* if(switch_1==0){
namikosaito 10:82f5a6e95dc2 145 //if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
namikosaito 9:dbfcdf1b20c1 146 file=1;
kanakin 11:bf37710355ff 147 }*/
namikosaito 3:04f6fe153dbc 148 /*
namikosaito 9:dbfcdf1b20c1 149 if(switch_1==0&&switch_2==1&&switch_3==1){ //最初から
namikosaito 0:77188ca200ce 150 #define SLOPE
namikosaito 0:77188ca200ce 151 #define TURN
namikosaito 0:77188ca200ce 152 #define RIVER
namikosaito 0:77188ca200ce 153 #define DOWNHILL
namikosaito 0:77188ca200ce 154 }
namikosaito 0:77188ca200ce 155
namikosaito 9:dbfcdf1b20c1 156 else if(switch_1==1&&switch_2==0&&switch_3==1){ //リバーから
namikosaito 0:77188ca200ce 157 #define RIVER
namikosaito 0:77188ca200ce 158 #define DOWNHILL
namikosaito 0:77188ca200ce 159 }
namikosaito 0:77188ca200ce 160
namikosaito 9:dbfcdf1b20c1 161 else if(switch_1==1&&switch_2==1&&switch_3==0){ //ダウンヒルから
namikosaito 3:04f6fe153dbc 162 kyori_reset();
namikosaito 0:77188ca200ce 163 #define DOWNHILL
namikosaito 2:864b823b1735 164 }
namikosaito 3:04f6fe153dbc 165 */
namikosaito 0:77188ca200ce 166 #ifdef SLOPE
namikosaito 9:dbfcdf1b20c1 167 yellow_pin=0;
namikosaito 0:77188ca200ce 168 while(1){
namikosaito 9:dbfcdf1b20c1 169 //pc.printf("%f,%d\r\n",kyori_nobori,fixed_rot_data);
namikosaito 8:340da692d865 170 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 171 //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 172 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 173 if(file==1&&switch_1==0)fclose( fp );
namikosaito 0:77188ca200ce 174 field();
namikosaito 0:77188ca200ce 175 LF();
namikosaito 1:f9c953ddc87a 176 back_cds();
namikosaito 0:77188ca200ce 177 save_servo();
namikosaito 0:77188ca200ce 178 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 179
namikosaito 6:c28aa7d26eba 180 if(rg_count==0&&gr_count==1){led1=1;blue_led=1;}
namikosaito 4:795055e031c3 181 if(rg_count==1&&gr_count==1){led2=1;blue_led=0;}
namikosaito 6:c28aa7d26eba 182 if(rg_count==1&&gr_count==2){led3=1;blue_led=1;}
namikosaito 4:795055e031c3 183 if(rg_count==2&&gr_count==2){led4=1;blue_led=0;}
namikosaito 0:77188ca200ce 184 if(rg_count==2&&gr_count==3){
namikosaito 0:77188ca200ce 185 led1=0;
namikosaito 0:77188ca200ce 186 led2=0;
namikosaito 0:77188ca200ce 187 led3=0;
namikosaito 0:77188ca200ce 188 led4=0;
namikosaito 0:77188ca200ce 189 break;
namikosaito 0:77188ca200ce 190 }
namikosaito 0:77188ca200ce 191 }
namikosaito 0:77188ca200ce 192 #endif
namikosaito 0:77188ca200ce 193
namikosaito 0:77188ca200ce 194 /////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 195
namikosaito 0:77188ca200ce 196 #ifdef TURN
namikosaito 6:c28aa7d26eba 197 blue_led=1;
namikosaito 0:77188ca200ce 198 while(1){
namikosaito 5:f90bd93f8558 199 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 200 if(file==1&&switch_1==1)fclose( fp );
namikosaito 1:f9c953ddc87a 201 gyro_keisan();
namikosaito 6:c28aa7d26eba 202 out=naka+325;//300;
namikosaito 0:77188ca200ce 203 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 204 led4=1;
namikosaito 3:04f6fe153dbc 205 if(fixed_rot_data<-60)break;//前は６０°だった
namikosaito 0:77188ca200ce 206 }
namikosaito 0:77188ca200ce 207
namikosaito 0:77188ca200ce 208 while(1){
namikosaito 5:f90bd93f8558 209 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 210 if(file==1&&switch_1==1)fclose( fp );
namikosaito 0:77188ca200ce 211 judge_color();
namikosaito 1:f9c953ddc87a 212 lf_downhill();
namikosaito 0:77188ca200ce 213 led3=1;
namikosaito 1:f9c953ddc87a 214 if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1||sa_dh<rw)break;
namikosaito 0:77188ca200ce 215 }
namikosaito 0:77188ca200ce 216
namikosaito 0:77188ca200ce 217 #endif
namikosaito 0:77188ca200ce 218
namikosaito 0:77188ca200ce 219 ////////////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 220
namikosaito 0:77188ca200ce 221 #ifdef RIVER
namikosaito 0:77188ca200ce 222 /*0*/
namikosaito 6:c28aa7d26eba 223 blue_led=1;
namikosaito 9:dbfcdf1b20c1 224 yellow_pin=0;
namikosaito 0:77188ca200ce 225 while(1){ //ライントレース
namikosaito 10:82f5a6e95dc2 226 judge_color();
namikosaito 10:82f5a6e95dc2 227 static int river_start;
namikosaito 10:82f5a6e95dc2 228 river_start++;
namikosaito 10:82f5a6e95dc2 229 if(river_start<10)blue=0;
namikosaito 0:77188ca200ce 230 LF();
namikosaito 9:dbfcdf1b20c1 231 //out-=50;
namikosaito 0:77188ca200ce 232 servo.pulsewidth_us(out);
namikosaito 10:82f5a6e95dc2 233 /*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 234 if(file==1&&switch_1==1)fclose( fp );
namikosaito 0:77188ca200ce 235 //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 236
namikosaito 0:77188ca200ce 237 if(blue==1){ //青になったらリバースタート
namikosaito 0:77188ca200ce 238 river_flag=1;
namikosaito 3:04f6fe153dbc 239 blue_led=0;
namikosaito 0:77188ca200ce 240 out=naka-250;
namikosaito 0:77188ca200ce 241 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 242 break;
namikosaito 0:77188ca200ce 243 }
namikosaito 3:04f6fe153dbc 244 }
namikosaito 0:77188ca200ce 245
namikosaito 0:77188ca200ce 246 /*1*/
namikosaito 0:77188ca200ce 247 while(1){ //初めに曲がる直前まで
namikosaito 0:77188ca200ce 248 //gyro_keisan();
namikosaito 0:77188ca200ce 249
namikosaito 0:77188ca200ce 250 LF(); //基本的にライントレース
namikosaito 9:dbfcdf1b20c1 251 if(fixed_rot_data<-60)out=gyro_pid(-45,6,0,0);//500 //最初は角度も利用
namikosaito 0:77188ca200ce 252 //if(fixed_rot_data<-75)out=naka-250;
namikosaito 0:77188ca200ce 253
namikosaito 0:77188ca200ce 254 led1=1;
namikosaito 10:82f5a6e95dc2 255 /*if(file==1)/printf(fp,*/pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
namikosaito 10:82f5a6e95dc2 256 if(file==1&&switch_1==1)fclose( fp );
namikosaito 0:77188ca200ce 257 if(river_flag==1){
namikosaito 0:77188ca200ce 258 kyori_reset();
namikosaito 0:77188ca200ce 259 river_flag=0;
namikosaito 0:77188ca200ce 260 }
namikosaito 0:77188ca200ce 261 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 262 if((river_turn()==1&&kyori_m45>Runder)||kyori>400||kyori_m45>R){ //Rｍｍ進んだらorｃｄｓで直角検知
namikosaito 0:77188ca200ce 263 river_theta=fixed_rot_data;
namikosaito 0:77188ca200ce 264 river_flag=1;
namikosaito 0:77188ca200ce 265 break;
namikosaito 0:77188ca200ce 266 }}
namikosaito 0:77188ca200ce 267
namikosaito 0:77188ca200ce 268 river_keisan(); //角度と距離の補正（いらないかも・・）
namikosaito 0:77188ca200ce 269
namikosaito 0:77188ca200ce 270 /*2*/
namikosaito 0:77188ca200ce 271 while(1){ //向きを９０度に合わせる。
namikosaito 0:77188ca200ce 272 gyro_keisan();
namikosaito 0:77188ca200ce 273 led2=1;
namikosaito 10:82f5a6e95dc2 274 /*if(file==1)fprintf(fp,*/pc.printf("2 theta=%d\r\n",fixed_rot_data);
namikosaito 10:82f5a6e95dc2 275 if(file==1&&switch_1==1)fclose( fp );
namikosaito 9:dbfcdf1b20c1 276 out=gyro_pid(-90-phai,6,0,2);//705
namikosaito 0:77188ca200ce 277 save_servo();
namikosaito 0:77188ca200ce 278 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 279 if(river_flag==1){
namikosaito 0:77188ca200ce 280 kyori_reset();
namikosaito 0:77188ca200ce 281 river_flag=0;
namikosaito 0:77188ca200ce 282 }
namikosaito 0:77188ca200ce 283 if(fixed_rot_data<-85-phai){ //ジャイロの角度が合ったら
namikosaito 0:77188ca200ce 284 out=naka;
namikosaito 0:77188ca200ce 285 break;
namikosaito 0:77188ca200ce 286 }
namikosaito 0:77188ca200ce 287 }
namikosaito 0:77188ca200ce 288
namikosaito 0:77188ca200ce 289 /*3*/
namikosaito 0:77188ca200ce 290 while(1){ //直進する
namikosaito 0:77188ca200ce 291 led3=1;
namikosaito 0:77188ca200ce 292 gyro_keisan();
namikosaito 10:82f5a6e95dc2 293 judge_color();
namikosaito 10:82f5a6e95dc2 294 /*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 295 if(file==1&&switch_1==1)fclose( fp );
namikosaito 10:82f5a6e95dc2 296 out=gyro_pid(-90-phai,5,0.1,0);//6 0.1 0
namikosaito 0:77188ca200ce 297 save_servo();
namikosaito 0:77188ca200ce 298 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 299 if(kyori_m90>Z){ //Zmm進んだら
namikosaito 0:77188ca200ce 300 out=naka-200; //前は100
namikosaito 0:77188ca200ce 301 white_r=0;
namikosaito 10:82f5a6e95dc2 302 white_mr=0;
namikosaito 0:77188ca200ce 303 break;
namikosaito 0:77188ca200ce 304 }
namikosaito 0:77188ca200ce 305 }
namikosaito 0:77188ca200ce 306
namikosaito 0:77188ca200ce 307 /*4*/
namikosaito 0:77188ca200ce 308 while(1){ //白線を見つけるまでそのまま
namikosaito 0:77188ca200ce 309 gyro_keisan();
namikosaito 0:77188ca200ce 310 led4=1;
namikosaito 0:77188ca200ce 311 judge_color();
namikosaito 0:77188ca200ce 312 LF();
namikosaito 0:77188ca200ce 313 lf_downhill();
namikosaito 10:82f5a6e95dc2 314 /*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 315 if(file==1&&switch_1==1)fclose( fp );
namikosaito 10:82f5a6e95dc2 316 /*static int haku;
namikosaito 10:82f5a6e95dc2 317 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 318 if(haku>1)break;*/
namikosaito 10:82f5a6e95dc2 319 if(white_r==1||white_mr==1||kyori_m90>T||(sa_dh>bw&&river_fin==1))break;
namikosaito 0:77188ca200ce 320 }
namikosaito 0:77188ca200ce 321
namikosaito 0:77188ca200ce 322 if(kyori_m90>T){
namikosaito 4:795055e031c3 323 servo.pulsewidth_us(naka-400);
namikosaito 0:77188ca200ce 324 wait(0.5);
namikosaito 0:77188ca200ce 325 }
namikosaito 0:77188ca200ce 326
namikosaito 0:77188ca200ce 327 /*5*/
namikosaito 0:77188ca200ce 328 while(1){ //ライントレースに戻る
namikosaito 0:77188ca200ce 329 gyro_keisan();
namikosaito 0:77188ca200ce 330 LF();
namikosaito 0:77188ca200ce 331 back_cds();
namikosaito 0:77188ca200ce 332 out-=50;
namikosaito 0:77188ca200ce 333 if(fixed_rot_data<-45)out-=50;
namikosaito 0:77188ca200ce 334 if(fixed_rot_data<-75)out-=100;
namikosaito 0:77188ca200ce 335
namikosaito 0:77188ca200ce 336 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 337 judge_color();
namikosaito 0:77188ca200ce 338 led1=0;
namikosaito 0:77188ca200ce 339 led2=0;
namikosaito 0:77188ca200ce 340 led3=0;
namikosaito 0:77188ca200ce 341 led4=0;
namikosaito 0:77188ca200ce 342 if(red==1){kyori_reset();break;}
namikosaito 0:77188ca200ce 343 }
namikosaito 0:77188ca200ce 344
namikosaito 0:77188ca200ce 345 #endif
kanakin 11:bf37710355ff 346 ///////////////////////////////////////////////////////////////////////////////////
kanakin 11:bf37710355ff 347 #ifdef RIVER2
kanakin 11:bf37710355ff 348
kanakin 11:bf37710355ff 349 /* blue_led=1;
kanakin 11:bf37710355ff 350 yellow_pin=0;
kanakin 11:bf37710355ff 351 while(1){ //ライントレース
kanakin 11:bf37710355ff 352 judge_color();
kanakin 11:bf37710355ff 353 static int river_start;
kanakin 11:bf37710355ff 354 river_start++;
kanakin 11:bf37710355ff 355 if(river_start<10)blue=0;
kanakin 11:bf37710355ff 356 LF();
kanakin 11:bf37710355ff 357 //out-=50;
kanakin 11:bf37710355ff 358 servo.pulsewidth_us(out);*/
kanakin 11:bf37710355ff 359 /*if(file==1)fprintf(fp,*///pc.printf("blue=%d, river kyori_m45=%f,theta=%d linetrace\r\n",blue,kyori_m45,fixed_rot_data);
kanakin 11:bf37710355ff 360 /* if(file==1&&switch_1==1)fclose( fp );
kanakin 11:bf37710355ff 361 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);
kanakin 11:bf37710355ff 362
kanakin 11:bf37710355ff 363 if(blue==1){ //青になったらリバースタート
kanakin 11:bf37710355ff 364 river_flag=1;
kanakin 11:bf37710355ff 365 blue_led=0;
kanakin 11:bf37710355ff 366 out=naka-250;
kanakin 11:bf37710355ff 367 servo.pulsewidth_us(out);
kanakin 11:bf37710355ff 368 led1=1;
kanakin 11:bf37710355ff 369 break;
kanakin 11:bf37710355ff 370 }
kanakin 11:bf37710355ff 371 }
kanakin 11:bf37710355ff 372 */
kanakin 11:bf37710355ff 373 /*1*/
kanakin 11:bf37710355ff 374 //while(1){
kanakin 11:bf37710355ff 375 gyro_keisan();
kanakin 11:bf37710355ff 376 // LF();
kanakin 11:bf37710355ff 377 pc.printf("%d,||out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
kanakin 11:bf37710355ff 378 //}
kanakin 11:bf37710355ff 379 wait(10);
kanakin 11:bf37710355ff 380 while(1){
kanakin 11:bf37710355ff 381 gyro_keisan();
kanakin 11:bf37710355ff 382 LF();
kanakin 11:bf37710355ff 383 pc.printf("%d,s1=%d\n\r",fixed_rot_data,s1);
kanakin 11:bf37710355ff 384 //kosuu();
kanakin 11:bf37710355ff 385 //max_dasi();
kanakin 11:bf37710355ff 386 //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);
kanakin 11:bf37710355ff 387 //migi_magari();
kanakin 11:bf37710355ff 388 while(s12>sikii){
kanakin 11:bf37710355ff 389 hidari_magari();
kanakin 11:bf37710355ff 390 servo.pulsewidth_us(out);
kanakin 11:bf37710355ff 391 //pc.printf("%d||%d\n\r",out,fixed_rot_data);
kanakin 11:bf37710355ff 392 pc.printf("%d||out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
kanakin 11:bf37710355ff 393 if(fixed_rot_data>-140&&fixed_rot_data<-130){break;}}
kanakin 11:bf37710355ff 394 if(fixed_rot_data>-140&&fixed_rot_data<-130){led1=1;break;}
kanakin 11:bf37710355ff 395
kanakin 11:bf37710355ff 396 }
kanakin 11:bf37710355ff 397 /*2*/
kanakin 11:bf37710355ff 398 while(1){
kanakin 11:bf37710355ff 399 gyro_keisan();
kanakin 11:bf37710355ff 400 LF();
kanakin 11:bf37710355ff 401 pc.printf("%d,s10=%d,s11=%d,s12=%d\n\r",fixed_rot_data,s10,s11,s12);
kanakin 11:bf37710355ff 402 // max_dasi();
kanakin 11:bf37710355ff 403 //hidari_magari();
kanakin 11:bf37710355ff 404 //kosuu();
kanakin 11:bf37710355ff 405 while(s1>sikii){
kanakin 11:bf37710355ff 406 migi_magari();
kanakin 11:bf37710355ff 407 servo.pulsewidth_us(out);
kanakin 11:bf37710355ff 408 //pc.printf("%d||%d\n\r",out,fixed_rot_data);
kanakin 11:bf37710355ff 409 pc.printf("%d||out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
kanakin 11:bf37710355ff 410 if(fixed_rot_data>-60&&fixed_rot_data<-35){break;}}
kanakin 11:bf37710355ff 411 if(fixed_rot_data>-60&&fixed_rot_data<-35){led2=1;break;}
kanakin 11:bf37710355ff 412 }
kanakin 11:bf37710355ff 413 /*3*/
kanakin 11:bf37710355ff 414 while(1){
kanakin 11:bf37710355ff 415 gyro_keisan();
kanakin 11:bf37710355ff 416 LF();
kanakin 11:bf37710355ff 417 pc.printf("%d,s1=%d\n\r",fixed_rot_data,s1);
kanakin 11:bf37710355ff 418 //max_dasi();
kanakin 11:bf37710355ff 419 //kosuu();
kanakin 11:bf37710355ff 420 while(s12>sikii){
kanakin 11:bf37710355ff 421 hidari_magari();
kanakin 11:bf37710355ff 422 servo.pulsewidth_us(out);
kanakin 11:bf37710355ff 423 //pc.printf("%d||%d\n\r",out,fixed_rot_data);
kanakin 11:bf37710355ff 424 pc.printf("%d||out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
kanakin 11:bf37710355ff 425 if(fixed_rot_data>-145&&fixed_rot_data<-125){break;}}
kanakin 11:bf37710355ff 426 if(fixed_rot_data>-145&&fixed_rot_data<-125){
kanakin 11:bf37710355ff 427
kanakin 11:bf37710355ff 428 led3=1;break;}
kanakin 11:bf37710355ff 429 }
kanakin 11:bf37710355ff 430 /*4*/
kanakin 11:bf37710355ff 431 while(1){
kanakin 11:bf37710355ff 432 gyro_keisan();
kanakin 11:bf37710355ff 433 LF();
kanakin 11:bf37710355ff 434 pc.printf("%d,s10=%d,s11=%d,s12=%d\n\r",fixed_rot_data,s10,s11,s12);
kanakin 11:bf37710355ff 435 //max_dasi();
kanakin 11:bf37710355ff 436 while(s1>sikii){
kanakin 11:bf37710355ff 437 migi_magari();
kanakin 11:bf37710355ff 438 servo.pulsewidth_us(out);
kanakin 11:bf37710355ff 439 //pc.printf("%d||%d\n\r",out,fixed_rot_data);
kanakin 11:bf37710355ff 440 pc.printf("%d||out=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);
kanakin 11:bf37710355ff 441 if(fixed_rot_data>-55&&fixed_rot_data<-35){break;}}
kanakin 11:bf37710355ff 442 if(fixed_rot_data>-55&&fixed_rot_data<-35){led4=1;break;}
kanakin 11:bf37710355ff 443 }
kanakin 11:bf37710355ff 444 /*5*/
kanakin 11:bf37710355ff 445 while(1){
kanakin 11:bf37710355ff 446 gyro_keisan();
kanakin 11:bf37710355ff 447 LF();
kanakin 11:bf37710355ff 448 judge_color();
kanakin 11:bf37710355ff 449
kanakin 11:bf37710355ff 450 if(red==1){kyori_reset(); led1=0;
kanakin 11:bf37710355ff 451 led2=0;
kanakin 11:bf37710355ff 452 led3=0;
kanakin 11:bf37710355ff 453 led4=0;
kanakin 11:bf37710355ff 454 break;}
kanakin 11:bf37710355ff 455 }
kanakin 11:bf37710355ff 456 #endif
namikosaito 0:77188ca200ce 457
namikosaito 0:77188ca200ce 458 ////////////////////////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 459
namikosaito 0:77188ca200ce 460 #ifdef DOWNHILL
namikosaito 9:dbfcdf1b20c1 461 yellow_pin=0;
namikosaito 0:77188ca200ce 462 while(1){
namikosaito 0:77188ca200ce 463 static int q;
namikosaito 0:77188ca200ce 464 q++;
namikosaito 7:3c273b3b19b1 465 blue_led=1;
namikosaito 10:82f5a6e95dc2 466 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 467 judge_color();
namikosaito 0:77188ca200ce 468 LF();
namikosaito 0:77188ca200ce 469 back_cds();
namikosaito 8:340da692d865 470 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 471 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 472 if(yellow==1&&q>30){ //黄色に入る
namikosaito 0:77188ca200ce 473 kyori_reset();
namikosaito 6:c28aa7d26eba 474 yellow_pin=1;
namikosaito 0:77188ca200ce 475 downhill=1;
namikosaito 0:77188ca200ce 476 break;
namikosaito 0:77188ca200ce 477 }
namikosaito 4:795055e031c3 478 }
namikosaito 7:3c273b3b19b1 479
namikosaito 7:3c273b3b19b1 480 if(file==1)fprintf(fp,"downhill\n");
namikosaito 7:3c273b3b19b1 481
namikosaito 0:77188ca200ce 482 while(1){
namikosaito 7:3c273b3b19b1 483 /*PID調整*/
namikosaito 8:340da692d865 484 if(kyori_f<10){
namikosaito 7:3c273b3b19b1 485 zure1=35;//25;
namikosaito 7:3c273b3b19b1 486 zure2=65;//50;
namikosaito 7:3c273b3b19b1 487 zure3=125;//100;
namikosaito 7:3c273b3b19b1 488 zure4=200;//175;
namikosaito 7:3c273b3b19b1 489 zure5=300;
namikosaito 7:3c273b3b19b1 490 }
namikosaito 8:340da692d865 491 else/* if(kyori_f<10)*/{
namikosaito 7:3c273b3b19b1 492 zure3=150;
namikosaito 7:3c273b3b19b1 493 zure4=250;
namikosaito 7:3c273b3b19b1 494 zure5=350;
namikosaito 8:340da692d865 495 }
namikosaito 8:340da692d865 496 /*
namikosaito 8:340da692d865 497 else if(kyori_f<15){
namikosaito 8:340da692d865 498 zure4=275;
namikosaito 8:340da692d865 499 zure5=375;
namikosaito 8:340da692d865 500 }
namikosaito 8:340da692d865 501 else{
namikosaito 8:340da692d865 502 zure5=400;
namikosaito 8:340da692d865 503 }
namikosaito 8:340da692d865 504 */
namikosaito 7:3c273b3b19b1 505
namikosaito 7:3c273b3b19b1 506
namikosaito 6:c28aa7d26eba 507 yellow_pin=1;
namikosaito 8:340da692d865 508 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 509 if(file==1&&switch_1==1)fclose( fp );
namikosaito 0:77188ca200ce 510 LF();
namikosaito 8:340da692d865 511 gyro_keisan();
namikosaito 0:77188ca200ce 512 lf_downhill();
namikosaito 0:77188ca200ce 513 judge_color();
namikosaito 2:864b823b1735 514 back_cds();
namikosaito 10:82f5a6e95dc2 515 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 516
namikosaito 2:864b823b1735 517 if(kyori_dh>50){
namikosaito 2:864b823b1735 518 static int f;
namikosaito 2:864b823b1735 519 if(sa_dh>yw+50){
namikosaito 2:864b823b1735 520 f=now;
namikosaito 2:864b823b1735 521 }
namikosaito 2:864b823b1735 522 else if(sa_dh<yw&&f>2){
namikosaito 3:04f6fe153dbc 523 out+=200;//naka+300;
namikosaito 2:864b823b1735 524 }
namikosaito 2:864b823b1735 525 else if(sa_dh<yw&&f<-2){
namikosaito 3:04f6fe153dbc 526 out-=200;//naka-300;
namikosaito 3:04f6fe153dbc 527
namikosaito 2:864b823b1735 528 }
namikosaito 3:04f6fe153dbc 529 }
namikosaito 10:82f5a6e95dc2 530 if(kyori_dh>450&&kyori_dh<600&&kyori_f>27)out-=50;
namikosaito 9:dbfcdf1b20c1 531 if(kyori_dh>600&&kyori_dh<900){out-=75;led1=1;
namikosaito 10:82f5a6e95dc2 532 if(kyori_f>25)out-=15;
namikosaito 10:82f5a6e95dc2 533 if(kyori_f>30)out-=25;
namikosaito 9:dbfcdf1b20c1 534 }
namikosaito 1:f9c953ddc87a 535 else if(kyori_dh>1200&&kyori_dh<1700){out+=25;led2=1;}
namikosaito 10:82f5a6e95dc2 536 else if(kyori_dh>1700&&kyori_dh<2700){out+=225;led3=1;
namikosaito 10:82f5a6e95dc2 537 if(kyori_f>25)out+=40;
namikosaito 10:82f5a6e95dc2 538 if(kyori_f>30)out+=50;
namikosaito 9:dbfcdf1b20c1 539 }
namikosaito 9:dbfcdf1b20c1 540 else if(kyori_dh>2800&&kyori_dh<3300){out-=50;led4=1;
namikosaito 10:82f5a6e95dc2 541 if(kyori_f>25)out-=15;
namikosaito 10:82f5a6e95dc2 542 if(kyori_f>30)out-=25;
namikosaito 9:dbfcdf1b20c1 543 }
namikosaito 1:f9c953ddc87a 544 else{
namikosaito 0:77188ca200ce 545 led1=0;
namikosaito 0:77188ca200ce 546 led2=0;
namikosaito 0:77188ca200ce 547 led3=0;
namikosaito 0:77188ca200ce 548 }
namikosaito 3:04f6fe153dbc 549
namikosaito 7:3c273b3b19b1 550 if(blue_pin==1&&kyori_dh>2800){ //最後の青に入ったら。
namikosaito 7:3c273b3b19b1 551 static int end;
namikosaito 8:340da692d865 552 led1=1;
namikosaito 8:340da692d865 553 led2=1;
namikosaito 8:340da692d865 554 led3=1;
namikosaito 8:340da692d865 555 led4=1;
namikosaito 7:3c273b3b19b1 556 blue_led=0;
namikosaito 7:3c273b3b19b1 557 if(file==1)fclose( fp );
namikosaito 9:dbfcdf1b20c1 558
namikosaito 9:dbfcdf1b20c1 559 if(out<naka-200)out=naka-200; //曲がりすぎを防ぐ
namikosaito 9:dbfcdf1b20c1 560 else if(out>naka+200)out=naka+200;
namikosaito 9:dbfcdf1b20c1 561
namikosaito 9:dbfcdf1b20c1 562 if(abs(kyori_f)<1)end++;
namikosaito 7:3c273b3b19b1 563 if(end>30)while(1){}
namikosaito 6:c28aa7d26eba 564 }
namikosaito 0:77188ca200ce 565 save_servo();
namikosaito 0:77188ca200ce 566 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 567 }
namikosaito 0:77188ca200ce 568 #endif
namikosaito 0:77188ca200ce 569
namikosaito 0:77188ca200ce 570 }
namikosaito 0:77188ca200ce 571
namikosaito 0:77188ca200ce 572
namikosaito 0:77188ca200ce 573
namikosaito 0:77188ca200ce 574
namikosaito 0:77188ca200ce 575 //////////////////////////////////////////////////////////
namikosaito 0:77188ca200ce 576 int river_turn(void){
namikosaito 0:77188ca200ce 577 static int ritu,ritu_flag;
namikosaito 0:77188ca200ce 578 ritu=0;
namikosaito 0:77188ca200ce 579 ritu_flag=0;
namikosaito 0:77188ca200ce 580 if(s1-s12>bw&&s1-s11>bw)ritu++;
namikosaito 0:77188ca200ce 581 if(s2-s12>bw&&s2-s11>bw)ritu++;
namikosaito 0:77188ca200ce 582 if(s3-s12>bw&&s3-s11>bw)ritu++;
namikosaito 0:77188ca200ce 583 if(s4-s12>bw&&s4-s11>bw)ritu++;
namikosaito 0:77188ca200ce 584 if(s5-s12>bw&&s5-s11>bw)ritu++;
namikosaito 0:77188ca200ce 585
namikosaito 0:77188ca200ce 586 if(ritu>3)ritu_flag=1;
namikosaito 0:77188ca200ce 587 //pc.printf("ritu=%d",ritu);
namikosaito 0:77188ca200ce 588
namikosaito 0:77188ca200ce 589 return ritu_flag;
namikosaito 0:77188ca200ce 590 }
namikosaito 0:77188ca200ce 591
namikosaito 0:77188ca200ce 592 int lf_downhill(void){
namikosaito 0:77188ca200ce 593 /*cdsの値の最大*/
namikosaito 10:82f5a6e95dc2 594 river_fin=0;
namikosaito 0:77188ca200ce 595 max_dh=s1;
namikosaito 0:77188ca200ce 596 if(max_dh<s2)max_dh=s2;
namikosaito 0:77188ca200ce 597 if(max_dh<s3)max_dh=s3;
namikosaito 0:77188ca200ce 598 if(max_dh<s4)max_dh=s4;
namikosaito 0:77188ca200ce 599 if(max_dh<s5)max_dh=s5;
namikosaito 0:77188ca200ce 600 if(max_dh<s6)max_dh=s6;
namikosaito 0:77188ca200ce 601 if(max_dh<s7)max_dh=s7;
namikosaito 0:77188ca200ce 602 if(max_dh<s8)max_dh=s8;
namikosaito 0:77188ca200ce 603 if(max_dh<s9)max_dh=s9;
namikosaito 10:82f5a6e95dc2 604 if(max_dh<s10){max_dh=s10;river_fin=1;}
namikosaito 10:82f5a6e95dc2 605 if(max_dh<s11){max_dh=s11;river_fin=1;}
namikosaito 10:82f5a6e95dc2 606 if(max_dh<s12){max_dh=s12;river_fin=1;}
namikosaito 0:77188ca200ce 607
namikosaito 0:77188ca200ce 608 /*cdsの値の最小*/
namikosaito 0:77188ca200ce 609 min_dh=s1;
namikosaito 0:77188ca200ce 610 if(min_dh>s2)min_dh=s2;
namikosaito 0:77188ca200ce 611 if(min_dh>s3)min_dh=s3;
namikosaito 0:77188ca200ce 612 if(min_dh>s4)min_dh=s4;
namikosaito 0:77188ca200ce 613 if(min_dh>s5)min_dh=s5;
namikosaito 0:77188ca200ce 614 if(min_dh>s6)min_dh=s6;
namikosaito 0:77188ca200ce 615 if(min_dh>s7)min_dh=s7;
namikosaito 0:77188ca200ce 616 if(min_dh>s8)min_dh=s8;
namikosaito 0:77188ca200ce 617 if(min_dh>s9)min_dh=s9;
namikosaito 0:77188ca200ce 618 if(min_dh>s10)min_dh=s10;
namikosaito 0:77188ca200ce 619 if(min_dh>s11)min_dh=s11;
namikosaito 0:77188ca200ce 620 if(min_dh>s12)min_dh=s12;
namikosaito 0:77188ca200ce 621
namikosaito 0:77188ca200ce 622 sa_dh=max_dh-min_dh;
namikosaito 0:77188ca200ce 623 return sa_dh;
namikosaito 0:77188ca200ce 624 }
namikosaito 0:77188ca200ce 625
namikosaito 0:77188ca200ce 626 void back_cds(void){ //ｃｄｓでラインのどちらにずれたのか検知する。
namikosaito 0:77188ca200ce 627 Find_White();
namikosaito 0:77188ca200ce 628
namikosaito 0:77188ca200ce 629 static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop;
namikosaito 0:77188ca200ce 630
namikosaito 0:77188ca200ce 631 if(now==-3||now==-4){
namikosaito 0:77188ca200ce 632 back_cds_l=0;
namikosaito 0:77188ca200ce 633 back_cds_r=1;
namikosaito 0:77188ca200ce 634 }
namikosaito 0:77188ca200ce 635 if(back_cds_r==1&&(now==-5||now==-6)){
namikosaito 0:77188ca200ce 636 back_cds_r2=1;
namikosaito 0:77188ca200ce 637 back_cds_r=0;
namikosaito 0:77188ca200ce 638 }
namikosaito 0:77188ca200ce 639
namikosaito 1:f9c953ddc87a 640 if(back_cds_r2==1&&now!=-6&&now!=-5){
namikosaito 0:77188ca200ce 641 while(1){
namikosaito 8:340da692d865 642 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 643 if(file==1&&switch_1==0)fclose( fp );
namikosaito 0:77188ca200ce 644 LF();
namikosaito 0:77188ca200ce 645 gyro_keisan();
namikosaito 0:77188ca200ce 646 lf_downhill();
namikosaito 0:77188ca200ce 647 Find_White(); //いらないはず
namikosaito 7:3c273b3b19b1 648 out=naka-zure5-50;
namikosaito 0:77188ca200ce 649 save_servo();
namikosaito 0:77188ca200ce 650 servo.pulsewidth_us(out);
namikosaito 0:77188ca200ce 651 judge_color();
namikosaito 0:77188ca200ce 652 field();
namikosaito 4:795055e031c3 653 if(now==-6||now==-5)flag_loop=1;//6
namikosaito 4:795055e031c3 654 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 655 back_cds_r2=0;
namikosaito 0:77188ca200ce 656 flag_loop=0;
namikosaito 0:77188ca200ce 657 break;
namikosaito 0:77188ca200ce 658 }
namikosaito 1:f9c953ddc87a 659 }
namikosaito 1:f9c953ddc87a 660
namikosaito 1:f9c953ddc87a 661 if(now==3||now==4){
namikosaito 1:f9c953ddc87a 662 back_cds_l=1;
namikosaito 1:f9c953ddc87a 663 back_cds_r=0;
namikosaito 1:f9c953ddc87a 664 }
namikosaito 1:f9c953ddc87a 665
namikosaito 1:f9c953ddc87a 666 if(back_cds_l==1&&(now==5||now==6)){
namikosaito 1:f9c953ddc87a 667 back_cds_l=0;
namikosaito 1:f9c953ddc87a 668 back_cds_l2=1;
namikosaito 1:f9c953ddc87a 669 }
namikosaito 1:f9c953ddc87a 670
namikosaito 1:f9c953ddc87a 671 if(back_cds_l2==1&&now!=6&&now!=5){
namikosaito 1:f9c953ddc87a 672 while(1){
namikosaito 8:340da692d865 673 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 674 if(file==1&&switch_1==0)fclose( fp );
namikosaito 1:f9c953ddc87a 675 LF();
namikosaito 1:f9c953ddc87a 676 gyro_keisan();
namikosaito 1:f9c953ddc87a 677 Find_White();
namikosaito 1:f9c953ddc87a 678 lf_downhill();
namikosaito 7:3c273b3b19b1 679 out=naka+zure5+50;
namikosaito 1:f9c953ddc87a 680 save_servo();
namikosaito 1:f9c953ddc87a 681 servo.pulsewidth_us(out);//1900
namikosaito 1:f9c953ddc87a 682 judge_color();
namikosaito 1:f9c953ddc87a 683 field();
namikosaito 4:795055e031c3 684 if(now==6||now==5)flag_loop=1;
namikosaito 4:795055e031c3 685 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 686 back_cds_l2=0;
namikosaito 1:f9c953ddc87a 687 flag_loop=0;
namikosaito 1:f9c953ddc87a 688 break;
namikosaito 1:f9c953ddc87a 689 }
namikosaito 1:f9c953ddc87a 690 }
namikosaito 1:f9c953ddc87a 691 }
namikosaito 1:f9c953ddc87a 692 }
namikosaito 1:f9c953ddc87a 693 }
namikosaito 1:f9c953ddc87a 694
namikosaito 1:f9c953ddc87a 695
namikosaito 0:77188ca200ce 696 void field(void){ //上りにおいてフィールドの色を判断し、数えていく。
namikosaito 0:77188ca200ce 697 judge_color();
namikosaito 0:77188ca200ce 698
namikosaito 0:77188ca200ce 699 if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)&&downhill==0)kyori_reset();
namikosaito 0:77188ca200ce 700 if((gr_flag_1==1&&red==1&&kyori_nobori>500)||(start_flag==1&&red==1)){
namikosaito 0:77188ca200ce 701 gr_flag_2=1;
namikosaito 0:77188ca200ce 702 }
namikosaito 0:77188ca200ce 703 if(gr_flag_2==1){
namikosaito 0:77188ca200ce 704 kyori_nobori_reset();
namikosaito 0:77188ca200ce 705 start_flag=0;
namikosaito 0:77188ca200ce 706 gr_flag_1=0;
namikosaito 0:77188ca200ce 707 gr_flag_2=0;
namikosaito 0:77188ca200ce 708 rg_flag_1=1;
namikosaito 0:77188ca200ce 709 gr_count++;
namikosaito 0:77188ca200ce 710 }
namikosaito 0:77188ca200ce 711 if(blue==1&&rg_flag_1==1&&kyori_nobori>500){
namikosaito 0:77188ca200ce 712 rg_flag_2=1;
namikosaito 0:77188ca200ce 713 }
namikosaito 0:77188ca200ce 714 if(rg_flag_2==1){
namikosaito 0:77188ca200ce 715 kyori_nobori_reset();
namikosaito 0:77188ca200ce 716 rg_flag_1=0;
namikosaito 0:77188ca200ce 717 rg_flag_2=0;
namikosaito 0:77188ca200ce 718 gr_flag_1=1;
namikosaito 0:77188ca200ce 719 rg_count++;
namikosaito 0:77188ca200ce 720 }
namikosaito 0:77188ca200ce 721 }