Diff: main.cpp

Revision:

5:f90bd93f8558

Parent:

4:795055e031c3

Child:

6:c28aa7d26eba

--- a/main.cpp	Tue Mar 01 15:56:22 2016 +0000
+++ b/main.cpp	Wed Mar 02 05:18:58 2016 +0000
@@ -25,6 +25,8 @@
 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;   
@@ -54,12 +56,6 @@
 
 
 int main(){
-    /*ファイル*/
-    if(switch_3==1){
-      if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
-      file=1;
-    }
-    
     mcp.format(7,0);
     mcp.frequency(1000000);
     pc.baud(115200);   
@@ -70,6 +66,15 @@
   //  while(1){}
 /*キャリブレーション*/
     calibration();
+    
+/*ファイル*/
+    if(switch_1==1){
+      if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
+      file=1;
+    }
+        
+    
+    
 /*    
 if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
     #define SLOPE
@@ -94,7 +99,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_3==0)fclose( fp );
+    if(file==1&&switch_1==0)fclose( fp );
     field();
     LF();
     //back();
@@ -102,9 +107,9 @@
     save_servo();
     servo.pulsewidth_us(out);
     
-    if(rg_count==0&&gr_count==1){led1=1;blue_led=1;}
+    if(rg_count==0&&gr_count==1){led1=1;blue_led=0;}
     if(rg_count==1&&gr_count==1){led2=1;blue_led=0;}
-    if(rg_count==1&&gr_count==2){led3=1;blue_led=1;}
+    if(rg_count==1&&gr_count==2){led3=1;blue_led=0;}
     if(rg_count==2&&gr_count==2){led4=1;blue_led=0;}
     if(rg_count==2&&gr_count==3){
         led1=0;
@@ -119,10 +124,10 @@
 /////////////////////////////////////////////////////////////////////////////////////
 
 #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,%d\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);   
-    if(file==1&&switch_3==0)fclose( fp );
+    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 );
     gyro_keisan();
     out=naka+300;
     servo.pulsewidth_us(out);
@@ -131,8 +136,8 @@
     }
     
 while(1){
-    if(file==1)fprintf(fp,"%d,%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_3==0)fclose( fp );
+    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 );
     judge_color();
     lf_downhill();
     led3=1;
@@ -145,7 +150,7 @@
 
 #ifdef RIVER
 /*0*/    
-       blue_led=1;
+       //blue_led=1;
     while(1){   //ライントレース
         LF();
         out-=50;
@@ -271,12 +276,13 @@
         kyori_reset();
         blue_led=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\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,kyori_dh);   
-    if(file==1&&switch_3==0)fclose( fp );
+    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 );
     LF();
     lf_downhill();
     judge_color();
@@ -308,47 +314,7 @@
         led3=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);
     }
@@ -426,8 +392,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\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);   
-            if(file==1&&switch_3==0)fclose( fp );
+            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 );
             LF();
             gyro_keisan();
             lf_downhill();
@@ -459,8 +425,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\n",out,now,fixed_rot_data,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);   
-            if(file==1&&switch_3==0)fclose( fp );
+            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 );
             LF();
             gyro_keisan();
             Find_White();