Diff: main.cpp

Revision:

3:04f6fe153dbc

Parent:

2:864b823b1735

Child:

4:795055e031c3

--- a/main.cpp	Sun Feb 28 15:43:11 2016 +0000
+++ b/main.cpp	Mon Feb 29 15:10:24 2016 +0000
@@ -5,17 +5,17 @@
 #include "enc_gyro.h"
 
 /*ここで切り替え（強制）*/
-/*
-#define SLOPE
-#define TURN
-#define RIVER 
+
+//#define SLOPE
+//#define TURN
+//#define RIVER 
 //#define RIVER2
 #define DOWNHILL 
-*/
+
 
 /*白との差*/
-#define bw 400  //blueとwhite
-#define yw 200  //yellow ちょっと厳しめにすること。
+#define bw 450  //blueとwhite
+#define yw 40  //yellow ちょっと厳しめにすること。
 #define rw 200  //red(orange)
 
 Serial pc(USBTX, USBRX); 
@@ -59,10 +59,10 @@
     Enc.attach(&distance, sp_t);
     servo.period_ms(20);      //サーボ周期
     servo.pulsewidth_us(naka);
-    //while(1){}
+  //  while(1){}
 /*キャリブレーション*/
     calibration();
-    
+/*    
 if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
     #define SLOPE
     #define TURN
@@ -70,20 +70,22 @@
     #define DOWNHILL 
     }  
     
-else if(switch_1==1&&switch_2==1&&switch_3==0){  //リバーから
+else if(switch_1==0&&switch_2==1&&switch_3==0){  //リバーから
     #define RIVER
     #define DOWNHILL 
     }  
     
-else if(switch_1==1&&switch_2==1&&switch_3==1){  //ダウンヒルから
+else if(switch_1==0&&switch_2==0&&switch_3==1){  //ダウンヒルから
+    kyori_reset();
     #define DOWNHILL 
     }   
-
+*/
 #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("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();
@@ -116,7 +118,7 @@
    // pc.printf("%d\r\n",fixed_rot_data);
     servo.pulsewidth_us(out);
     led4=1;
-    if(fixed_rot_data<-50)break;//前は６０°だった
+    if(fixed_rot_data<-60)break;//前は６０°だった
     }
     
 while(1){
@@ -132,6 +134,7 @@
 
 #ifdef RIVER
 /*0*/    
+       blue_led=1;
     while(1){   //ライントレース
         LF();
         out-=50;
@@ -143,24 +146,19 @@
         
         if(blue==1){    //青になったらリバースタート
             river_flag=1;
+            blue_led=0;
             out=naka-250;
             servo.pulsewidth_us(out);
             break;
         }
-        }
-        
-    while(1){
-        static int magari_flag;
-        magari_flag++;
-        if(magari_flag>30)break;
-        }    
+        } 
         
 /*1*/    
     while(1){    //初めに曲がる直前まで
      //gyro_keisan();
      
      LF();     //基本的にライントレース
-     out-=50;
+     //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;
     
@@ -222,13 +220,15 @@
          lf_downhill();
          pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,sa_dh);
          
-         static int riv;
+         /*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;
              }
+             */
+         if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||kyori_m90>T)break;    
          }      
         
      if(kyori_m90>T){   
@@ -259,7 +259,7 @@
 ////////////////////////////////////////////////////////////////////////////
 
 #ifdef DOWNHILL
-
+    //blue_led=1;
 while(1){
     static int q;
     q++;
@@ -272,6 +272,7 @@
         kyori_reset();
         blue_led=1;
         downhill=1;
+      //  pc.printf("aaa\r\n");
        break;
         }
     }
@@ -281,7 +282,7 @@
     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();
     
@@ -289,38 +290,25 @@
     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=naka+300;
-        /*led1=0;
-        led2=0;
-        led3=0;
-        led4=1;
-        */
+        out+=200;//naka+300;
         }
     else if(sa_dh<yw&&f<-2){
-        out=naka-300;
-       /* led1=1;
-        led2=0;
-        led3=0;
-        led4=0;*/
+        out-=200;//naka-300;
+       
         }
-        }
+       }
     
     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 if(kyori_dh>1700&&kyori_dh<2800){out+=125;led3=1;}
     else{
         led1=0;
         led2=0;
         led3=0;
         }
+        
     /*
     if(kyori_dh<600){
         out-=50;