Diff: main.cpp

Revision:

4:795055e031c3

Parent:

3:04f6fe153dbc

Child:

5:f90bd93f8558

--- a/main.cpp	Mon Feb 29 15:10:24 2016 +0000
+++ b/main.cpp	Tue Mar 01 15:56:22 2016 +0000
@@ -6,18 +6,21 @@
 
 /*ここで切り替え（強制）*/
 
-//#define SLOPE
-//#define TURN
-//#define RIVER 
+#define SLOPE
+#define TURN
+#define RIVER 
 //#define RIVER2
 #define DOWNHILL 
 
 
 /*白との差*/
 #define bw 450  //blueとwhite
-#define yw 40  //yellow ちょっと厳しめにすること。
+#define yw 400  //yellow ちょっと厳しめにすること。
 #define rw 200  //red(orange)
 
+LocalFileSystem local("local");
+FILE *fp;
+int file;
 Serial pc(USBTX, USBRX); 
 PwmOut servo(p25); 
 PwmOut blue_led(p21);
@@ -33,8 +36,7 @@
 int wr_flag,wl_flag;
 int lf_downhill(void);      //ダウンヒルのライントレース用。他のところでも使ってるけど。cdsの最大値、最小値、その差を出す。
 int sa_dh,max_dh,min_dh;    //lf_downhillで使う変数。
-void back(void);            //白線の左右どちらに振れたのか見る。（エンコーダー＋ジャイロ）
-void back_cds(void);        //同上（ｃｄｓ）、上り、リバー用
+void back_cds(void);        //白線の左右どちらに振れたのか見る。（ｃｄｓ）、上り、リバー用
 void back_cds_dh(void);     //同上、ダウンヒル用
 int river_turn(void);       //リバーの最初の曲がりをcdsで線を読みとる
 
@@ -52,6 +54,12 @@
 
 
 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);   
@@ -81,12 +89,12 @@
     }   
 */
 #ifdef SLOPE
-   blue_led=1;
 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);
     //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);       
-    //pc.printf("s1=%d,s2=%d,s3=%d,c1=%d,d1=%d,e1=%d\r\n",s1,s2,s3,c1,d1,e1);
+    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 );
     field();
     LF();
     //back();
@@ -94,10 +102,10 @@
     save_servo();
     servo.pulsewidth_us(out);
     
-    if(rg_count==0&&gr_count==1)led1=1;
-    if(rg_count==1&&gr_count==1)led2=1;
-    if(rg_count==1&&gr_count==2)led3=1;
-    if(rg_count==2&&gr_count==2)led4=1;
+    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=1;}
+    if(rg_count==2&&gr_count==2){led4=1;blue_led=0;}
     if(rg_count==2&&gr_count==3){
         led1=0;
         led2=0;
@@ -111,17 +119,20 @@
 /////////////////////////////////////////////////////////////////////////////////////
 
 #ifdef TURN
-
+    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 );
     gyro_keisan();
-    out=naka+250;
-   // pc.printf("%d\r\n",fixed_rot_data);
+    out=naka+300;
     servo.pulsewidth_us(out);
     led4=1;
     if(fixed_rot_data<-60)break;//前は６０°だった
     }
     
 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 );
     judge_color();
     lf_downhill();
     led3=1;
@@ -139,7 +150,6 @@
         LF();
         out-=50;
         judge_color();
-        //gyro_keisan();
         servo.pulsewidth_us(out);
         //pc.printf("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);
@@ -158,7 +168,6 @@
      //gyro_keisan();
      
      LF();     //基本的にライントレース
-     //out-=50;
      if(fixed_rot_data<-60)out=gyro_pid(-45,5,0,0);//out=naka-150;   //最初は角度も利用
      //if(fixed_rot_data<-75)out=naka-250;
     
@@ -218,21 +227,12 @@
          judge_color();
          LF();
          lf_downhill();
-         pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,sa_dh);
-         
-         /*static int riv;
-         if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)riv++;
-         
-         if(kyori_m90>T)break;  //行き過ぎ
-         if(riv>3||sa_dh>bw){ //白線検知
-             break;
-             }
-             */
+         //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;    
          }      
         
      if(kyori_m90>T){   
-        servo.pulsewidth_us(1100);
+        servo.pulsewidth_us(naka-400);
         wait(0.5);
         }
         
@@ -259,11 +259,10 @@
 ////////////////////////////////////////////////////////////////////////////
 
 #ifdef DOWNHILL
-    //blue_led=1;
 while(1){
     static int q;
     q++;
-    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("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);  
     judge_color();
     LF();
     back_cds();
@@ -272,12 +271,12 @@
         kyori_reset();
         blue_led=1;
         downhill=1;
-      //  pc.printf("aaa\r\n");
        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 );
     LF();
     lf_downhill();
     judge_color();
@@ -427,19 +426,21 @@
         
     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 );
             LF();
             gyro_keisan();
             lf_downhill();
             Find_White(); //いらないはず
            // out=1100;
-            pc.printf("hidari now %d\r\n",now);
-            out=1100;
+            //pc.printf("hidari now %d\r\n",now);
+            out=naka-300;
             save_servo();
             servo.pulsewidth_us(out);
             judge_color();
             field();
-            if(now==-6)flag_loop=1;
-            if((flag_loop==1&&(now==-4||now==-5))||white_mr==1||white_m==1){ 
+            if(now==-6||now==-5)flag_loop=1;//6
+            if((flag_loop==1&&(now==-4||now==-3))||white_r==1||white_mr==1||white_m==1||white_ml==1||white_l==1){//45 
                 back_cds_r2=0;
                 flag_loop=0;
                 break;    
@@ -458,18 +459,20 @@
         
     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 );
             LF();
             gyro_keisan();
             Find_White();
             lf_downhill();
-            pc.printf("migi now=%d\r\n",now);
-            out=2100;
+            //pc.printf("migi now=%d\r\n",now);
+            out=naka+300;;
             save_servo();
             servo.pulsewidth_us(out);//1900
             judge_color();
             field();
-            if(now==6)flag_loop=1;
-            if((flag_loop==1&&(now==4||now==5))||white_ml==1||white_m==1){ //調べて
+            if(now==6||now==5)flag_loop=1;
+            if((flag_loop==1&&(now==4||now==3))||white_r==1||white_mr==1||white_m==1||white_ml==1||white_l==1){ //調べて
                 back_cds_l2=0;
                 flag_loop=0;
                 break;
@@ -479,7 +482,7 @@
     }  
     }
 
-
+////////////////////////////////////////////////////////////////////////
 void back_cds_dh(void){     //ｃｄｓでラインのどちらにずれたのか検知する。
      Find_White();
      
@@ -513,14 +516,14 @@
             lf_downhill();
             Find_White(); //いらないはず
            // out=1100;
-            pc.printf("hidari now %d\r\n",now);
-            out=1100;
+            //pc.printf("hidari now %d\r\n",now);
+            out=naka-300;
             save_servo();
             servo.pulsewidth_us(out);
             judge_color();
             field();
             if(now==-6)flag_loop=1;
-            if((flag_loop==1&&(now==-4||now==-5))||white_mr==1||white_m==1){ 
+            if((flag_loop==1&&(now==-4||now==-5))||white_r==1||white_mr==1||white_m==1||white_ml==1||white_l==1){ 
                 back_cds_r2=0;
                 flag_loop=0;
                 break;    
@@ -554,14 +557,14 @@
             gyro_keisan();
             Find_White();
             lf_downhill();
-            pc.printf("migi now=%d\r\n",now);
-            out=2100;
+            //pc.printf("migi now=%d\r\n",now);
+            out=naka+300;
             save_servo();
             servo.pulsewidth_us(out);//1900
             judge_color();
             field();
             if(now==6)flag_loop=1;
-            if((flag_loop==1&&(now==4||now==5))||white_ml==1||white_m==1){ //調べて
+            if((flag_loop==1&&(now==4||now==5))||white_l==1||white_ml==1||white_m==1||white_mr==1||white_r==1){ //調べて
                 back_cds_l2=0;
                 flag_loop=0;
                 break;
@@ -581,66 +584,6 @@
     }      
 
 
-
-void back(void){    //エンコーダーとジャイロで左右を見分ける方法（使ってない）
-    if(white_mr==1){
-        back_l_flag=0;
-        back_r_flag=1;
-        }
-    if(back_r_flag==1&&white_r==1){
-        while(1){
-           if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
-           if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
-           if(rg_count==1&&gr_count==1)yoko=kyori_45x;
-
-            
-            out=1300;
-            pc.printf("hidari %d\r\n",out);
-            servo.pulsewidth_us(out);
-            judge_color();
-            field();
-            if(white_ml==1||white_l==1||white_m==1){
-                back_r_flag=0;
-                break;
-                }
-            static int yoko_flag;
-            if(yoko<-5)yoko_flag=2;
-            if(yoko_flag==2&&yoko>0)break;
-            if(yoko>5)yoko_flag=3;
-            if(yoko_flag==3&&yoko<0)break;
-            }
-        }
-        
-    if(white_ml==1){
-        back_r_flag=0;
-        back_l_flag=1;
-        }
-    if(back_l_flag==1&&white_l==1){
-        while(1){
-
-           if((rg_count==0&&gr_count==0)||(rg_count==2&&gr_count==2))yoko=kyori_15x;
-           if((rg_count==0&&gr_count==1)||(rg_count==1&&gr_count==2))yoko=kyori_30x;
-           if(rg_count==1&&gr_count==1)yoko=kyori_45x;
-
-            out=1700;
-            pc.printf("migi %d\r\n",out);
-            servo.pulsewidth_us(out);
-            judge_color();
-            field();
-            if(white_mr==1||white_r==1||white_m==1){
-                back_r_flag=0;
-                break;
-                }
-            static int yoko_flag;
-            if(yoko<-5)yoko_flag=2;
-            if(yoko_flag==2&&yoko>0)break;
-            if(yoko>5)yoko_flag=3;
-            if(yoko_flag==3&&yoko<0)break;    
-        }    
-    }
-    }
-    
-
 void field(void){ //上りにおいてフィールドの色を判断し、数えていく。
      judge_color();