Diff: main.cpp

Revision:

2:864b823b1735

Parent:

1:f9c953ddc87a

Child:

3:04f6fe153dbc

--- a/main.cpp	Sun Feb 28 15:37:23 2016 +0000
+++ b/main.cpp	Sun Feb 28 15:43:11 2016 +0000
@@ -4,11 +4,10 @@
 #include "color.h"
 #include "enc_gyro.h"
 
-
 /*ここで切り替え（強制）*/
 /*
-//#define SLOPE
-//#define TURN
+#define SLOPE
+#define TURN
 #define RIVER 
 //#define RIVER2
 #define DOWNHILL 
@@ -45,8 +44,6 @@
 float yoko;                 //上りラインに対しての左右の振れ
 int kakudo;
 
-
-
 int start_flag=1;           //fieldで使用
 int rg_flag_1,rg_flag_2,rg_count=0;
 int gr_flag_1,gr_flag_2,gr_count=0;
@@ -66,8 +63,6 @@
 /*キャリブレーション*/
     calibration();
     
-/*スイッチ切り替え*/
-
 if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
     #define SLOPE
     #define TURN
@@ -82,8 +77,7 @@
     
 else if(switch_1==1&&switch_2==1&&switch_3==1){  //ダウンヒルから
     #define DOWNHILL 
-    }           
-
+    }   
 
 #ifdef SLOPE
 while(1){
@@ -144,7 +138,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){    //青になったらリバースタート
@@ -286,11 +280,39 @@
     LF();
     lf_downhill();
     judge_color();
-    back_cds_dh(); //ダウンヒル専用
+    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();
     
+    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=naka+300;
+        /*led1=0;
+        led2=0;
+        led3=0;
+        led4=1;
+        */
+        }
+    else if(sa_dh<yw&&f<-2){
+        out=naka-300;
+       /* led1=1;
+        led2=0;
+        led3=0;
+        led4=0;*/
+        }
+        }
+    
     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;}