Diff: main.cpp

Revision:

1:f9c953ddc87a

Parent:

0:77188ca200ce

Child:

2:864b823b1735

--- a/main.cpp	Wed Feb 24 04:23:37 2016 +0000
+++ b/main.cpp	Sun Feb 28 15:37:23 2016 +0000
@@ -4,16 +4,20 @@
 #include "color.h"
 #include "enc_gyro.h"
 
+
 /*ここで切り替え（強制）*/
+/*
 //#define SLOPE
 //#define TURN
 #define RIVER 
 //#define RIVER2
 #define DOWNHILL 
+*/
 
 /*白との差*/
-#define bw 550  //blueとwhite
+#define bw 400  //blueとwhite
 #define yw 200  //yellow ちょっと厳しめにすること。
+#define rw 200  //red(orange)
 
 Serial pc(USBTX, USBRX); 
 PwmOut servo(p25); 
@@ -31,7 +35,8 @@
 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で線を読みとる
 
 int back_r_flag=0;
@@ -40,7 +45,7 @@
 float yoko;                 //上りラインに対しての左右の振れ
 int kakudo;
 
-int downhill;               //ダウンヒルのとき１になる。
+
 
 int start_flag=1;           //fieldで使用
 int rg_flag_1,rg_flag_2,rg_count=0;
@@ -57,40 +62,39 @@
     Enc.attach(&distance, sp_t);
     servo.period_ms(20);      //サーボ周期
     servo.pulsewidth_us(naka);
-    
-    blue_led=1;
-    
+    //while(1){}
 /*キャリブレーション*/
     calibration();
     
 /*スイッチ切り替え*/
-/*    
-if(switch_1==0&&switch_2==0&&switch_3==0){  //最初から
+
+if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
     #define SLOPE
     #define TURN
     #define RIVER
     #define DOWNHILL 
     }  
     
-if(switch_1==1&&switch_2==0&&switch_3==0){  //リバーから
+else if(switch_1==1&&switch_2==1&&switch_3==0){  //リバーから
     #define RIVER
     #define DOWNHILL 
     }  
     
-if(switch_1==1&&switch_2==1&&switch_3==0){  //ダウンヒルから
+else if(switch_1==1&&switch_2==1&&switch_3==1){  //ダウンヒルから
     #define DOWNHILL 
     }           
-*/
+
 
 #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);
-    pc.printf("out=%d,dir=%d,sa=%f,now=%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",out,dir,sa,now,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12);       
+    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);
     field();
     LF();
     //back();
-    //back_cds();
+    back_cds();
     save_servo();
     servo.pulsewidth_us(out);
     
@@ -113,17 +117,19 @@
 #ifdef TURN
 
 while(1){    
-    //gyro_keisan();
-    out=1750;
+    gyro_keisan();
+    out=naka+250;
+   // pc.printf("%d\r\n",fixed_rot_data);
     servo.pulsewidth_us(out);
     led4=1;
-    if(fixed_rot_data<-60)break;
+    if(fixed_rot_data<-50)break;//前は６０°だった
     }
     
 while(1){
     judge_color();
+    lf_downhill();
     led3=1;
-    if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1)break;
+    if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1||sa_dh<rw)break;
     }    
  
 #endif
@@ -138,7 +144,7 @@
         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("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);
         
         if(blue==1){    //青になったらリバースタート
@@ -184,7 +190,7 @@
        gyro_keisan();
        led2=1;
        //pc.printf("2 theta=%d\r\n",fixed_rot_data);
-       out=gyro_pid(-90-phai,7,0,5);
+       out=gyro_pid(-90-phai,7,0,0);//705
        save_servo();
        servo.pulsewidth_us(out);
        if(river_flag==1){
@@ -201,7 +207,7 @@
     while(1){    //直進する
      led3=1;
      gyro_keisan();
-     pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
+     //pc.printf("%f,%f,theta=%d,phai=%f,out=%d\r\n",kyori_m90,Z,fixed_rot_data,phai,out);
      out=gyro_pid(-90-phai,6,0.1,0);
      save_servo();
      servo.pulsewidth_us(out);
@@ -220,10 +226,13 @@
          judge_color();
          LF();
          lf_downhill();
-         pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,out);
+         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(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||sa_dh>bw){ //白線検知
+         if(riv>3||sa_dh>bw){ //白線検知
              break;
              }
          }      
@@ -277,45 +286,27 @@
     LF();
     lf_downhill();
     judge_color();
-    back_cds();
+    back_cds_dh(); //ダウンヒル専用
     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();
     
-    if(kyori_dh>50){
-    static int f;
-    if(sa_dh>yw+50){
-        f=now;
-        led1=0;
-        led2=1;
-        led3=0;
-        led4=0;
-        }
-    else if(sa_dh<yw&&f>2){
-        out=1800;
+    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+=75;led3=1;}
+    else{
         led1=0;
         led2=0;
         led3=0;
-        led4=1;
         }
-    else if(sa_dh<yw&&f<-2){
-        out=1200;
-        led1=1;
-        led2=0;
-        led3=0;
-        led4=0;
-        }
-        }
-    
-    
-    
+    /*
     if(kyori_dh<600){
-        out-=100;
+        out-=50;
         }
        
     else if(kyori_dh>600&&kyori_dh<900){
         led3=1;
-        out-=120;
+        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;
@@ -334,7 +325,7 @@
          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){
@@ -424,7 +415,7 @@
         back_cds_r=0;
        }
         
-    if((back_cds_r2==1&&now!=-6&&now!=-5)||sa_dh<yw){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){   //変更
+    if(back_cds_r2==1&&now!=-6&&now!=-5){
         while(1){
             LF();
             gyro_keisan();
@@ -443,13 +434,97 @@
                 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){
+        while(1){
+            LF();
+            gyro_keisan();
+            Find_White();
+            lf_downhill();
+            pc.printf("migi now=%d\r\n",now);
+            out=2100;
+            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){ //調べて
+                back_cds_l2=0;
+                flag_loop=0;
+                break;
+                }       
+            }
+        }
+    }  
+    }
+
+
+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=1100;
+            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){ 
+                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;}        
+                break;}     
+                }   
             }
         }
         
@@ -463,7 +538,7 @@
         back_cds_l2=1;
         }    
         
-    if((back_cds_l2==1&&now!=6&&now!=5)||sa_dh<yw){//||(white_r==0&&white_l==0&&white_mr==0&&white_ml==0&&white_m==0)||sa_dh<yw){
+    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();
@@ -481,17 +556,21 @@
                 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;}       
+                break;}
+                }       
             }
         }
-    }  
-    
+    }      
+
+
 
 void back(void){    //エンコーダーとジャイロで左右を見分ける方法（使ってない）
     if(white_mr==1){