Diff: main.cpp

Revision:

0:77188ca200ce

Child:

1:f9c953ddc87a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 24 04:23:37 2016 +0000
@@ -0,0 +1,580 @@
+#include "mbed.h"
+#include "math.h"
+#include "LF2.h"
+#include "color.h"
+#include "enc_gyro.h"
+
+/*ここで切り替え（強制）*/
+//#define SLOPE
+//#define TURN
+#define RIVER 
+//#define RIVER2
+#define DOWNHILL 
+
+/*白との差*/
+#define bw 550  //blueとwhite
+#define yw 200  //yellow ちょっと厳しめにすること。
+
+Serial pc(USBTX, USBRX); 
+PwmOut servo(p25); 
+PwmOut blue_led(p21);
+
+void save_servo(void){    //サーボの振れすぎを防ぐ。
+    if(out>2250)out=2250;
+    else if(out<650)out=650;   
+    }
+
+
+/*関数*/
+void field(void);           //フィールドの色を見る
+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);        //同上（ｃｄｓ）
+int river_turn(void);       //リバーの最初の曲がりをcdsで線を読みとる
+
+int back_r_flag=0;
+int back_l_flag=0;
+
+float yoko;                 //上りラインに対しての左右の振れ
+int kakudo;
+
+int downhill;               //ダウンヒルのとき１になる。
+
+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;
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+
+
+int main(){
+    mcp.format(7,0);
+    mcp.frequency(1000000);
+    pc.baud(115200);   
+    gyro.format(16,3);
+    Enc.attach(&distance, sp_t);
+    servo.period_ms(20);      //サーボ周期
+    servo.pulsewidth_us(naka);
+    
+    blue_led=1;
+    
+/*キャリブレーション*/
+    calibration();
+    
+/*スイッチ切り替え*/
+/*    
+if(switch_1==0&&switch_2==0&&switch_3==0){  //最初から
+    #define SLOPE
+    #define TURN
+    #define RIVER
+    #define DOWNHILL 
+    }  
+    
+if(switch_1==1&&switch_2==0&&switch_3==0){  //リバーから
+    #define RIVER
+    #define DOWNHILL 
+    }  
+    
+if(switch_1==1&&switch_2==1&&switch_3==0){  //ダウンヒルから
+    #define DOWNHILL 
+    }           
+*/
+
+#ifdef SLOPE
+while(1){
+    //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("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();
+    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==2&&gr_count==3){
+        led1=0;
+        led2=0;
+        led3=0;
+        led4=0;
+        break;
+        }
+    }
+#endif   
+
+/////////////////////////////////////////////////////////////////////////////////////
+
+#ifdef TURN
+
+while(1){    
+    //gyro_keisan();
+    out=1750;
+    servo.pulsewidth_us(out);
+    led4=1;
+    if(fixed_rot_data<-60)break;
+    }
+    
+while(1){
+    judge_color();
+    led3=1;
+    if(white_l==1||white_r==1||white_mr==1||white_ml==1||white_m==1)break;
+    }    
+ 
+#endif
+
+////////////////////////////////////////////////////////////////////////////////////
+
+#ifdef RIVER
+/*0*/    
+    while(1){   //ライントレース
+        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);
+        
+        if(blue==1){    //青になったらリバースタート
+            river_flag=1;
+            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;
+     if(fixed_rot_data<-60)out=gyro_pid(-45,5,0,0);//out=naka-150;   //最初は角度も利用
+     //if(fixed_rot_data<-75)out=naka-250;
+    
+     led1=1;
+     //pc.printf("kyori_m45=%f,theta=%d\r\n",kyori_m45,fixed_rot_data);
+     if(river_flag==1){   
+        kyori_reset();
+        river_flag=0; 
+        }
+     servo.pulsewidth_us(out);
+     if((river_turn()==1&&kyori_m45>Runder)||kyori>400||kyori_m45>R){  //Rｍｍ進んだらorｃｄｓで直角検知
+         river_theta=fixed_rot_data;
+         river_flag=1;
+         break;
+         }}
+     
+     river_keisan(); //角度と距離の補正（いらないかも・・）
+     
+/*2*/
+    while(1){     //向きを９０度に合わせる。
+       gyro_keisan();
+       led2=1;
+       //pc.printf("2 theta=%d\r\n",fixed_rot_data);
+       out=gyro_pid(-90-phai,7,0,5);
+       save_servo();
+       servo.pulsewidth_us(out);
+       if(river_flag==1){
+        kyori_reset();
+        river_flag=0; 
+        }
+       if(fixed_rot_data<-85-phai){  //ジャイロの角度が合ったら
+           out=naka;
+           break;
+           }
+       }
+       
+/*3*/         
+    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);
+     out=gyro_pid(-90-phai,6,0.1,0);
+     save_servo();
+     servo.pulsewidth_us(out);
+     if(kyori_m90>Z){   //Zmm進んだら
+         out=naka-200; //前は100
+         white_r=0;
+         white_l=0;
+         break;
+         }    
+     }
+     
+/*4*/     
+     while(1){   //白線を見つけるまでそのまま
+         gyro_keisan();
+         led4=1;   
+         judge_color();
+         LF();
+         lf_downhill();
+         pc.printf("%f,%d,%d\r\n",kyori_m90,Ec_count,out);
+         
+         if(kyori_m90>T)break;  //行き過ぎ
+         if(white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1||sa_dh>bw){ //白線検知
+             break;
+             }
+         }      
+        
+     if(kyori_m90>T){   
+        servo.pulsewidth_us(1100);
+        wait(0.5);
+        }
+        
+/*5*/
+while(1){     //ライントレースに戻る
+         gyro_keisan();
+         LF(); 
+         back_cds();   
+         out-=50;
+         if(fixed_rot_data<-45)out-=50;
+         if(fixed_rot_data<-75)out-=100;
+             
+         servo.pulsewidth_us(out);
+         judge_color();
+         led1=0;
+         led2=0;
+         led3=0;
+         led4=0;   
+         if(red==1){kyori_reset();break;}
+         }
+
+#endif
+
+////////////////////////////////////////////////////////////////////////////
+
+#ifdef DOWNHILL
+
+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);  
+    judge_color();
+    LF();
+    back_cds();
+    servo.pulsewidth_us(out);
+    if(yellow==1&&q>30){  //黄色に入る
+        kyori_reset();
+        blue_led=1;
+        downhill=1;
+       break;
+        }
+    }
+
+while(1){
+    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();
+    
+    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;
+        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;
+        }
+       
+    else if(kyori_dh>600&&kyori_dh<900){
+        led3=1;
+        out-=120;
+        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((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
+
+   }
+   
+   
+
+
+//////////////////////////////////////////////////////////
+int river_turn(void){
+    static int ritu,ritu_flag;
+    ritu=0;
+    ritu_flag=0;
+    if(s1-s12>bw&&s1-s11>bw)ritu++;
+    if(s2-s12>bw&&s2-s11>bw)ritu++;
+    if(s3-s12>bw&&s3-s11>bw)ritu++;
+    if(s4-s12>bw&&s4-s11>bw)ritu++;
+    if(s5-s12>bw&&s5-s11>bw)ritu++;
+    
+    if(ritu>3)ritu_flag=1;
+    //pc.printf("ritu=%d",ritu);
+    
+    return ritu_flag;
+    }    
+
+int lf_downhill(void){
+    /*cdsの値の最大*/
+    max_dh=s1;
+    if(max_dh<s2)max_dh=s2;
+    if(max_dh<s3)max_dh=s3;
+    if(max_dh<s4)max_dh=s4;
+    if(max_dh<s5)max_dh=s5;
+    if(max_dh<s6)max_dh=s6;
+    if(max_dh<s7)max_dh=s7;
+    if(max_dh<s8)max_dh=s8;
+    if(max_dh<s9)max_dh=s9;
+    if(max_dh<s10)max_dh=s10;
+    if(max_dh<s11)max_dh=s11;
+    if(max_dh<s12)max_dh=s12;
+   
+   /*cdsの値の最小*/ 
+    min_dh=s1;
+    if(min_dh>s2)min_dh=s2;
+    if(min_dh>s3)min_dh=s3;
+    if(min_dh>s4)min_dh=s4;
+    if(min_dh>s5)min_dh=s5;
+    if(min_dh>s6)min_dh=s6;
+    if(min_dh>s7)min_dh=s7;
+    if(min_dh>s8)min_dh=s8;
+    if(min_dh>s9)min_dh=s9;
+    if(min_dh>s10)min_dh=s10;
+    if(min_dh>s11)min_dh=s11;
+    if(min_dh>s12)min_dh=s12;
+    
+    sa_dh=max_dh-min_dh;
+    return sa_dh;
+    }
+
+void back_cds(void){     //ｃｄｓでラインのどちらにずれたのか検知する。
+     Find_White();
+     
+     static int back_cds_l,back_cds_l2,back_cds_r,back_cds_r2,flag_loop; 
+     
+     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)||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;    
+                }
+             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)||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=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;
+                }
+              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;}       
+            }
+        }
+    }  
+    
+
+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();
+     
+     if((white_r==1||white_l==1||white_mr==1||white_ml==1||white_m==1)&&downhill==0)kyori_reset();
+     if((gr_flag_1==1&&red==1&&kyori_nobori>500)||(start_flag==1&&red==1)){
+        gr_flag_2=1;
+        }
+    if(gr_flag_2==1){   
+        kyori_nobori_reset();
+        start_flag=0;
+        gr_flag_1=0;
+        gr_flag_2=0;
+        rg_flag_1=1;
+        gr_count++;
+        }
+    if(blue==1&&rg_flag_1==1&&kyori_nobori>500){
+        rg_flag_2=1;
+        }
+    if(rg_flag_2==1){
+        kyori_nobori_reset();
+        rg_flag_1=0;
+        rg_flag_2=0;
+        gr_flag_1=1;
+        rg_count++;
+        }
+    }
\ No newline at end of file