Diff: main.cpp

Revision:

7:3c273b3b19b1

Parent:

6:c28aa7d26eba

Child:

8:340da692d865

--- a/main.cpp	Wed Mar 02 15:32:12 2016 +0000
+++ b/main.cpp	Thu Mar 03 13:20:50 2016 +0000
@@ -36,8 +36,7 @@
 int wr_flag,wl_flag;
 int lf_downhill(void);      //ダウンヒルのライントレース用。他のところでも使ってるけど。cdsの最大値、最小値、その差を出す。
 int sa_dh,max_dh,min_dh;    //lf_downhillで使う変数。
-void back_cds(void);        //白線の左右どちらに振れたのか見る。（ｃｄｓ）、上り、リバー用
-void back_cds_dh(void);     //同上、ダウンヒル用
+void back_cds(void);        //白線の左右どちらに振れたのか見る。（ｃｄｓ)
 int river_turn(void);       //リバーの最初の曲がりをcdsで線を読みとる
 
 int back_r_flag=0;
@@ -66,7 +65,7 @@
     calibration();
 
     /*ファイル*/
-    if(switch_3==1){
+    if(switch_1==1){
       if ( NULL == (fp = fopen( "/local/test.csv", "w" )) )error( "" );
       file=1;
     }
@@ -94,7 +93,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();
@@ -122,7 +121,7 @@
     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_3==0)fclose( fp );
+    if(file==1&&switch_1==0)fclose( fp );
     gyro_keisan();
     out=naka+325;//300;
     servo.pulsewidth_us(out);
@@ -132,7 +131,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_3==0)fclose( fp );
+    if(file==1&&switch_1==0)fclose( fp );
     judge_color();
     lf_downhill();
     led3=1;
@@ -262,30 +261,47 @@
 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);  
     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);   
     servo.pulsewidth_us(out);
     if(yellow==1&&q>30){  //黄色に入る
         kyori_reset();
-        blue_led=1;
         yellow_pin=1;
         downhill=1;
        break;
         }
     }  
+    
+    if(file==1)fprintf(fp,"downhill\n");
+    
 while(1){
+    /*PID調整*/
+    if(kyori_f<5){
+        zure1=35;//25;
+        zure2=65;//50;
+        zure3=125;//100;
+        zure4=200;//175;
+        zure5=300;
+        }
+    else{
+        zure3=150;
+        zure4=250;
+        zure5=350;
+        }    
+    
+    
     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 );
+    if(file==1&&switch_1==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);  
-    
-    //gyro_keisan();
+    //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);  
     
     if(kyori_dh>50){
     static int f;
@@ -303,60 +319,23 @@
     
     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;}//150
+    else if(kyori_dh>1700&&kyori_dh<2800){out+=250;led3=1;}
     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(blue_pin==1&&kyori_dh>2800){   //最後の青に入ったら。
+          static int end;
+          if(abs(kyori_f)<1)end++;
+          blue_led=0;
+          if(file==1)fclose( fp );
+          if(end>30)while(1){}    
          } 
-      */ 
-     /*
-     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);
     }
-
 #endif
 
    }
@@ -431,14 +410,12 @@
     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&&switch_3==0)fclose( fp );
+            if(file==1&&switch_1==0)fclose( fp );
             LF();
             gyro_keisan();
             lf_downhill();
             Find_White(); //いらないはず
-           // out=1100;
-            //pc.printf("hidari now %d\r\n",now);
-            out=naka-300;
+            out=naka-zure5-50;
             save_servo();
             servo.pulsewidth_us(out);
             judge_color();
@@ -464,13 +441,12 @@
     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&&switch_3==0)fclose( fp );
+            if(file==1&&switch_1==0)fclose( fp );
             LF();
             gyro_keisan();
             Find_White();
             lf_downhill();
-            //pc.printf("migi now=%d\r\n",now);
-            out=naka+300;;
+            out=naka+zure5+50;
             save_servo();
             servo.pulsewidth_us(out);//1900
             judge_color();
@@ -484,107 +460,6 @@
             }
         }
     }  
-    }
-
-////////////////////////////////////////////////////////////////////////
-void back_cds_dh(void){     //ｃｄｓでラインのどちらにずれたのか検知する。
-     Find_White();
-     
-     static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop; 
-     if(kyori_dh>50){
-     static int f;
-     if(sa_dh>yw+50){
-        f=now;
-        }
-     else if(sa_dh<yw&&f>2){
-        out=naka+300;
-        }
-     else if(sa_dh<yw&&f<-2){
-        out=naka-300;
-       }
-        }
-        
-     if(now==-3||now==-4){
-        back_cds_l=0;
-        back_cds_r=1;
-        }
-     if(back_cds_r==1&&(now==-5||now==-6)){   
-        back_cds_r2=1;
-        back_cds_r=0;
-       }
-        
-    if(back_cds_r2==1&&now!=-6&&now!=-5){//||(downhill==1&&sa_dh<yw)){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){   //変更
-        while(1){
-            LF();
-            gyro_keisan();
-            lf_downhill();
-            Find_White(); //いらないはず
-           // 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_r==1||white_mr==1||white_m==1||white_ml==1||white_l==1){ 
-                back_cds_r2=0;
-                flag_loop=0;
-                break;    
-                }
-                
-            if(downhill==1){    
-             static int yw_flag;    
-            if(sa_dh>yw)yw_flag++;
-            if(yw_flag>10){
-                yw_flag=0;
-                back_cds_r2=0;
-                flag_loop=0;
-                break;}     
-                }   
-            }
-        }
-        
-    if(now==3||now==4){
-        back_cds_l=1;
-        back_cds_r=0;
-        }
-        
-    if(back_cds_l==1&&(now==5||now==6)){
-        back_cds_l=0;
-        back_cds_l2=1;
-        }    
-        
-    if(back_cds_l2==1&&now!=6&&now!=5){//||(downhill==1&&sa_dh<yw)){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){
-        while(1){
-            LF();
-            gyro_keisan();
-            Find_White();
-            lf_downhill();
-            //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_l==1||white_ml==1||white_m==1||white_mr==1||white_r==1){ //調べて
-                back_cds_l2=0;
-                flag_loop=0;
-                break;
-                }
-                
-            if(downhill==1){ 
-              static int yw_flag;    
-            if(sa_dh>yw)yw_flag++;
-            if(yw_flag>10){
-                back_cds_l2=0;
-                flag_loop=0;
-                yw_flag=0;
-                break;}
-                }       
-            }
-        }
     }