nakao kanaki
/
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.
Diff: main.cpp
- Revision:
- 9:dbfcdf1b20c1
- Parent:
- 8:340da692d865
- Child:
- 10:82f5a6e95dc2
--- a/main.cpp Mon Mar 07 07:12:21 2016 +0000
+++ b/main.cpp Tue Mar 08 15:04:10 2016 +0000
@@ -6,9 +6,9 @@
/*ここで切り替え（強制）*/
-#define SLOPE
-//#define TURN
-//#define RIVER
+//#define SLOPE
+#define TURN
+#define RIVER
//#define RIVER2
#define DOWNHILL
@@ -66,32 +66,32 @@
while(1){if(push_switch==1)break;}
/*ファイル*/
- if(switch_1==1){
- //if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
- //file=1;
+ if(switch_1==0){
+ if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
+ file=1;
}
/*
-if(switch_1==1&&switch_2==0&&switch_3==0){ //最初から
+if(switch_1==0&&switch_2==1&&switch_3==1){ //最初から
#define SLOPE
#define TURN
#define RIVER
#define DOWNHILL
}
-else if(switch_1==0&&switch_2==1&&switch_3==0){ //リバーから
+else if(switch_1==1&&switch_2==0&&switch_3==1){ //リバーから
#define RIVER
#define DOWNHILL
}
-else if(switch_1==0&&switch_2==0&&switch_3==1){ //ダウンヒルから
+else if(switch_1==1&&switch_2==1&&switch_3==0){ //ダウンヒルから
kyori_reset();
#define DOWNHILL
}
*/
#ifdef SLOPE
+ yellow_pin=0;
while(1){
- //pc.printf("%f\r\n",kyori_nobori);
- judge_color();
+ //pc.printf("%f,%d\r\n",kyori_nobori,fixed_rot_data);
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);
//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);
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);
@@ -146,12 +146,13 @@
#ifdef RIVER
/*0*/
blue_led=1;
+ yellow_pin=0;
while(1){ //ライントレース
LF();
- out-=50;
+ //out-=50;
judge_color();
servo.pulsewidth_us(out);
- //pc.printf("kyori_m45=%f,theta=%d linetrace\r\n",kyori_m45,fixed_rot_data);
+ pc.printf("river kyori_m45=%f,theta=%d linetrace\r\n",kyori_m45,fixed_rot_data);
//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);
if(blue==1){ //青になったらリバースタート
@@ -168,11 +169,11 @@
//gyro_keisan();
LF(); //基本的にライントレース
- if(fixed_rot_data<-60)out=gyro_pid(-45,5,0,0);//out=naka-150; //最初は角度も利用
+ if(fixed_rot_data<-60)out=gyro_pid(-45,6,0,0);//500 //最初は角度も利用
//if(fixed_rot_data<-75)out=naka-250;
led1=1;
- //pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
+ pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
if(river_flag==1){
kyori_reset();
river_flag=0;
@@ -190,8 +191,8 @@
while(1){ //向きを９０度に合わせる。
gyro_keisan();
led2=1;
- //pc.printf("2 theta=%d\r\n",fixed_rot_data);
- out=gyro_pid(-90-phai,7,0,0);//705
+ pc.printf("2 theta=%d\r\n",fixed_rot_data);
+ out=gyro_pid(-90-phai,6,0,2);//705
save_servo();
servo.pulsewidth_us(out);
if(river_flag==1){
@@ -208,8 +209,8 @@
while(1){ //直進する
led3=1;
gyro_keisan();
- //pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
- out=gyro_pid(-90-phai,6,0.1,0);
+ pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
+ out=gyro_pid(-90-phai,5,0,0);//6 0.1 0
save_servo();
servo.pulsewidth_us(out);
if(kyori_m90>Z){ //Zmm進んだら
@@ -228,7 +229,9 @@
LF();
lf_downhill();
//pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,sa_dh);
- if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||kyori_m90>T)break;
+ static int haku;
+ if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||kyori_m90>T)haku++;
+ if(haku>5)break;
}
if(kyori_m90>T){
@@ -259,11 +262,12 @@
////////////////////////////////////////////////////////////////////////////
#ifdef DOWNHILL
+ yellow_pin=0;
while(1){
static int q;
q++;
blue_led=1;
- // 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);
+ //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);
judge_color();
LF();
back_cds();
@@ -306,13 +310,13 @@
yellow_pin=1;
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);
- if(file==1&&switch_1==0)fclose( fp );
+ if(file==1&&switch_1==1)fclose( fp );
LF();
gyro_keisan();
lf_downhill();
judge_color();
back_cds();
- //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);
+ // 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);
if(kyori_dh>50){
static int f;
@@ -328,10 +332,16 @@
}
}
- if(kyori_dh>600&&kyori_dh<900){out-=75;led1=1;}
+ if(kyori_dh>600&&kyori_dh<900){out-=75;led1=1;
+ if(kyori_f>23)out-=25;
+ }
else if(kyori_dh>1200&&kyori_dh<1700){out+=25;led2=1;}
- else if(kyori_dh>1700&&kyori_dh<2700){out+=250;led3=1;}
- else if(kyori_dh>2800&&kyori_dh<3300){out-=50;led4=1;}
+ else if(kyori_dh>1700&&kyori_dh<2700){out+=250;led3=1;
+ if(kyori_f>23)out+=25;
+ }
+ else if(kyori_dh>2800&&kyori_dh<3300){out-=50;led4=1;
+ if(kyori_f>23)out-=25;
+ }
else{
led1=0;
led2=0;
@@ -344,9 +354,13 @@
led2=1;
led3=1;
led4=1;
- if(abs(kyori_f)<1)end++;
blue_led=0;
if(file==1)fclose( fp );
+
+ if(out<naka-200)out=naka-200; //曲がりすぎを防ぐ
+ else if(out>naka+200)out=naka+200;
+
+ if(abs(kyori_f)<1)end++;
if(end>30)while(1){}
}
save_servo();