Diff: main.cpp

Revision:

6:c28aa7d26eba

Parent:

5:f90bd93f8558

Child:

7:3c273b3b19b1

--- a/main.cpp	Wed Mar 02 05:18:58 2016 +0000
+++ b/main.cpp	Wed Mar 02 15:32:12 2016 +0000
@@ -6,9 +6,9 @@
 
 /*ここで切り替え（強制）*/
 
-#define SLOPE
-#define TURN
-#define RIVER 
+//#define SLOPE
+//#define TURN
+//#define RIVER 
 //#define RIVER2
 #define DOWNHILL 
 
@@ -25,8 +25,6 @@
 PwmOut servo(p25); 
 PwmOut blue_led(p21);
 
-DigitalOut yellow_pin(p26); 
-
 void save_servo(void){    //サーボの振れすぎを防ぐ。
     if(out>2250)out=2250;
     else if(out<650)out=650;   
@@ -63,18 +61,15 @@
     Enc.attach(&distance, sp_t);
     servo.period_ms(20);      //サーボ周期
     servo.pulsewidth_us(naka);
-  //  while(1){}
+   //while(1){}
 /*キャリブレーション*/
     calibration();
-    
-/*ファイル*/
-    if(switch_1==1){
+
+    /*ファイル*/
+    if(switch_3==1){
       if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
       file=1;
     }
-        
-    
-    
 /*    
 if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
     #define SLOPE
@@ -99,7 +94,7 @@
     //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 );
+    if(file==1&&switch_3==0)fclose( fp );
     field();
     LF();
     //back();
@@ -107,9 +102,9 @@
     save_servo();
     servo.pulsewidth_us(out);
     
-    if(rg_count==0&&gr_count==1){led1=1;blue_led=0;}
+    if(rg_count==0&&gr_count==1){led1=1;blue_led=1;}
     if(rg_count==1&&gr_count==1){led2=1;blue_led=0;}
-    if(rg_count==1&&gr_count==2){led3=1;blue_led=0;}
+    if(rg_count==1&&gr_count==2){led3=1;blue_led=1;}
     if(rg_count==2&&gr_count==2){led4=1;blue_led=0;}
     if(rg_count==2&&gr_count==3){
         led1=0;
@@ -124,12 +119,12 @@
 /////////////////////////////////////////////////////////////////////////////////////
 
 #ifdef TURN
-   // blue_led=1;
+    blue_led=1;
 while(1){    
     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 );
+    if(file==1&&switch_3==0)fclose( fp );
     gyro_keisan();
-    out=naka+300;
+    out=naka+325;//300;
     servo.pulsewidth_us(out);
     led4=1;
     if(fixed_rot_data<-60)break;//前は６０°だった
@@ -137,7 +132,7 @@
     
 while(1){
     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 );
+    if(file==1&&switch_3==0)fclose( fp );
     judge_color();
     lf_downhill();
     led3=1;
@@ -150,7 +145,7 @@
 
 #ifdef RIVER
 /*0*/    
-       //blue_led=1;
+       blue_led=1;
     while(1){   //ライントレース
         LF();
         out-=50;
@@ -275,19 +270,20 @@
     if(yellow==1&&q>30){  //黄色に入る
         kyori_reset();
         blue_led=1;
+        yellow_pin=1;
         downhill=1;
-        yellow_pin=1;
        break;
         }
     }  
 while(1){
-    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);   
-    if(file==1&&switch_1==0)fclose( fp );
+    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&&switch_3==0)fclose( fp );
     LF();
     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_dh,sa_dh,fixed_rot_data,out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);  
+    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);  
     
     //gyro_keisan();
     
@@ -307,14 +303,56 @@
     
     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+=125;led3=1;}
+    else if(kyori_dh>1700&&kyori_dh<2800){out+=250;led3=1;}//150
     else{
         led1=0;
         led2=0;
         led3=0;
         }
         
-   
+ //   if(blue_pin==1){led1=1;wait(1);led1=0;wait(1);blue_led=0;  }  
+        
+    /*
+    if(kyori_dh<600){
+        out-=50;
+        }
+       
+    else if(kyori_dh>600&&kyori_dh<900){
+        led3=1;
+        out-=100;
+        if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){
+         out=1300;
+         out-=(yw-sa_dh)*0.15;
+        }
+        }
+        
+    else if(kyori_dh>1200&&kyori_dh<1700){ 
+        out+=50;
+        }   
+ 
+    else if(kyori_dh>1700&&kyori_dh<2700){
+        led3=1;
+        out+=180;
+        if((white_r==0&&white_mr==0&&white_l==0&&white_ml==0&&white_m==0)||sa_dh<yw){ 
+         out=1900;
+         out+=(yw-sa_dh)*0.15;
+         }     
+         } 
+      */ 
+     /*
+     if((kyori_dh>2000&&kyori_dh<2700&&(white_r==0&&white_mr==0&&white_m==0))||((kyori_dh>2200&&kyori_dh<2700)&&fixed_rot_data>15)){
+         while(1){
+             LF();
+             lf_downhill();
+             out=2100;
+             if(fixed_rot_data<0)out=2000;
+             if(fixed_rot_data<-15)out=1900;
+             if(fixed_rot_data<-30)out=1700;
+             servo.pulsewidth_us(out);
+             if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)||sa_dh>yw)break;
+                }
+                }    
+       */        
     save_servo();
     servo.pulsewidth_us(out);
     }
@@ -392,8 +430,8 @@
         
     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,%d\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,sa_dh);   
-            if(file==1&&switch_1==0)fclose( fp );
+            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&&switch_3==0)fclose( fp );
             LF();
             gyro_keisan();
             lf_downhill();
@@ -425,8 +463,8 @@
         
     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,%d\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh,sa_dh);   
-            if(file==1&&switch_1==0)fclose( fp );
+            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&&switch_3==0)fclose( fp );
             LF();
             gyro_keisan();
             Find_White();