File content as of revision 2:864b823b1735:

#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 400  //blueとwhite
#define yw 200  //yellow ちょっと厳しめにすること。
#define rw 200  //red(orange)

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);        //同上（ｃｄｓ）、上り、リバー用
void back_cds_dh(void);     //同上、ダウンヒル用
int river_turn(void);       //リバーの最初の曲がりをcdsで線を読みとる

int back_r_flag=0;
int back_l_flag=0;

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;

///////////////////////////////////////////////////////////////////////////////////////////////


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);
    //while(1){}
/*キャリブレーション*/
    calibration();
    
if(switch_1==1&&switch_2==0&&switch_3==0){  //最初から
    #define SLOPE
    #define TURN
    #define RIVER
    #define DOWNHILL 
    }  
    
else if(switch_1==1&&switch_2==1&&switch_3==0){  //リバーから
    #define RIVER
    #define DOWNHILL 
    }  
    
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_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();
    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=naka+250;
   // pc.printf("%d\r\n",fixed_rot_data);
    servo.pulsewidth_us(out);
    led4=1;
    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||sa_dh<rw)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,0);//705
       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,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(riv>3||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=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;}
    else{
        led1=0;
        led2=0;
        led3=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((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){
        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(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;}     
                }   
            }
        }
        
    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=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;
                }
                
            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;}
                }       
            }
        }
    }      



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++;
        }
    }