Diff: main.cpp

Revision:

8:340da692d865

Parent:

7:3c273b3b19b1

Child:

9:dbfcdf1b20c1

--- a/main.cpp	Thu Mar 03 13:20:50 2016 +0000
+++ b/main.cpp	Mon Mar 07 07:12:21 2016 +0000
@@ -6,7 +6,7 @@
 
 /*ここで切り替え（強制）*/
 
-//#define SLOPE
+#define SLOPE
 //#define TURN
 //#define RIVER 
 //#define RIVER2
@@ -60,14 +60,15 @@
     Enc.attach(&distance, sp_t);
     servo.period_ms(20);      //サーボ周期
     servo.pulsewidth_us(naka);
-   //while(1){}
+    
 /*キャリブレーション*/
     calibration();
-
+    while(1){if(push_switch==1)break;}
+    
     /*ファイル*/
     if(switch_1==1){
-      if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
-      file=1;
+      //if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
+      //file=1;
     }
 /*    
 if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
@@ -90,13 +91,13 @@
 #ifdef SLOPE
 while(1){
     //pc.printf("%f\r\n",kyori_nobori);
-    //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);
+    judge_color();
+    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);   
     if(file==1&&switch_1==0)fclose( fp );
     field();
     LF();
-    //back();
     back_cds();
     save_servo();
     servo.pulsewidth_us(out);
@@ -266,7 +267,7 @@
     judge_color();
     LF();
     back_cds();
-    if(file==1)fprintf(fp,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,kyori);   
+    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);   
     servo.pulsewidth_us(out);
     if(yellow==1&&q>30){  //黄色に入る
         kyori_reset();
@@ -280,28 +281,38 @@
     
 while(1){
     /*PID調整*/
-    if(kyori_f<5){
+    if(kyori_f<10){
         zure1=35;//25;
         zure2=65;//50;
         zure3=125;//100;
         zure4=200;//175;
         zure5=300;
         }
-    else{
+    else/* if(kyori_f<10)*/{
         zure3=150;
         zure4=250;
         zure5=350;
-        }    
+        }
+ /*           
+    else if(kyori_f<15){
+        zure4=275;
+        zure5=375;
+    }     
+    else{
+        zure5=400;
+        }
+    */
     
     
     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\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,kyori);   
+    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 );
     LF();
+    gyro_keisan();
     lf_downhill();
     judge_color();
     back_cds();
-    //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_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;
@@ -319,7 +330,8 @@
     
     if(kyori_dh>600&&kyori_dh<900){out-=75;led1=1;}
     else if(kyori_dh>1200&&kyori_dh<1700){out+=25;led2=1;}
-    else if(kyori_dh>1700&&kyori_dh<2800){out+=250;led3=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{
         led1=0;
         led2=0;
@@ -328,6 +340,10 @@
         
     if(blue_pin==1&&kyori_dh>2800){   //最後の青に入ったら。
           static int end;
+          led1=1;
+          led2=1;
+          led3=1;
+          led4=1;
           if(abs(kyori_f)<1)end++;
           blue_led=0;
           if(file==1)fclose( fp );
@@ -409,7 +425,7 @@
         
     if(back_cds_r2==1&&now!=-6&&now!=-5){
         while(1){
-            if(file==1)fprintf(fp,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,kyori);   
+            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 );
             LF();
             gyro_keisan();
@@ -440,7 +456,7 @@
         
     if(back_cds_l2==1&&now!=6&&now!=5){
         while(1){
-            if(file==1)fprintf(fp,"%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,kyori);   
+            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 );
             LF();
             gyro_keisan();