一种颜色识别机器人 STM32代码

#include "sys.h"
int a[7],a_dian[7];
int abc1,abc2,abc3,abc4;
int timech=0,chao1,timesc,countsc,dj[9];
u16 pcolor[3]={0};


int bi1=515,bi2=2495,bi3=2500,bi4=1600,shou1=1020;
int bi1_last,bi2_last,bi3_last,bi4_last,shou1_last,a1,a2,a3,a4,a5,a6;
int sx1,sx2,sx3,sx4;
extern int colour;
int main(void)
{        int lk1,lk2,lk3,lkk2;
         u8 t,len,adx;        
         int i1,colour_zhi;
         a[1]=3;a[2]=0;a[3]=3;a[4]=0;a[5]=3;a[6]=0;
         a_dian[1]=0;a_dian[2]=0;a_dian[3]=0;a_dian[4]=0;a_dian[5]=0;a_dian[6]=0;
         my_init(); pcolor[0]=50,pcolor[1]=50,pcolor[2]=50;
                 while(0)
        {
                colour_zhi=colourjudge(pcolor),printf("%d  %d    %d     %d \r\n",colour_zhi,pcolor[0],pcolor[1],pcolor[2]);
               
        }
        duoji(515,2495,2500,1600,1020);
        duoji(515,2495,2500,1600,1420);
  duoji(515,2495,2500,1600,1020);         

        i1=100;


  while(1)
{
qi_shi();//Æe2½3ìDò
while(1)//ÑÕé«135
{

        
        
        
while(a[3])
{
        
        fang_zhi_jing_du();
                                        while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[3],0);
jia_qu_di_ji_kuai(a[3]);
zhong_xin_wei_zhi_tiao_jie(a[3],1);

        a[3]=a[3]-1;
        while(1)
        {
        colour_zhi=colourjudge(pcolor),printf("%d\r\n",colour_zhi);
        if((0<colour_zhi)&&(colour_zhi<6))break;
        }
//duoji(515,2495,1700,900,1760);        
duoji(515,2495,2500,1600,1760);
        
if(colour_zhi==3)
{if(a[3]==0)
        {zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
                fang_zhi_jing_du();
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[3]+1);
                fang_zhi_di_ji_kuai(a_dian[3]+1);
                a_dian[3]=a_dian[3]+1;
                dao_che(3);
        }
else        
{lu_xian_qie_huan_zhong_ji(3,6);
fang_zhi_jing_du();
while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[6]+1,0);
fang_zhi_di_ji_kuai(a[6]+1);
zhong_xin_wei_zhi_tiao_jie(a[6]+1,1);
lu_xian_qie_huan_zhong_ji(6,3);
a[6]=a[6]+1;}
}
if(colour_zhi==1)
{
lu_xian_qie_huan_zhong_ji(3,2);
        fang_zhi_jing_du();
while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[2]+1,0);
fang_zhi_di_ji_kuai(a[2]+1);
zhong_xin_wei_zhi_tiao_jie(a[2]+1,1);
lu_xian_qie_huan_zhong_ji(2,3);
a[2]=a[2]+1;
}
if(colour_zhi==5)
{
lu_xian_qie_huan_zhong_ji(3,4);//////////////////////////////////////////////
        fang_zhi_jing_du();
while(1)
{

if(KEY10==0)a1=0,a2=0;else a1=-20,a2=-20;
if((KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[4]+1,0);

if((a[4]+1)==1)
{lkk2=4;
        while(lkk2--)
fastforward_turn(-20,20);}
fang_zhi_di_ji_kuai(a[4]+1);
if((a[4]+1)==1)
{lkk2=4;
        while(lkk2--)
fastforward_turn(20,-20);}
zhong_xin_wei_zhi_tiao_jie(a[4]+1,1);
lu_xian_qie_huan_zhong_ji(4,3);
a[4]=a[4]+1;

}
}
if(a[6]!=0)
{
while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}

}
while(a[6]!=0)
{
        lu_xian_qie_huan_zhong_ji(3,6);
        fang_zhi_jing_du();
while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[6],0);
jia_qu_di_ji_kuai(a[6]);
zhong_xin_wei_zhi_tiao_jie(a[6],1);
        
a[6]=a[6]-1;        
lu_xian_qie_huan_zhong_ji(6,3);
zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
        fang_zhi_jing_du();
fang_zhi_nu_biao_dian_tiao_jie(a_dian[3]+1);
        fang_zhi_di_ji_kuai(a_dian[3]+1);
        a_dian[3]=a_dian[3]+1;
        dao_che(3);
}
lu_xian_qie_huan_zhong_ji(3,1);///////////////////////////////////////////μú¶t¶Ñ
while(a[1])
{
        fang_zhi_jing_du();
                                        while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[1],0);
        jia_qu_di_ji_kuai(a[1]);
zhong_xin_wei_zhi_tiao_jie(a[1],1);

        a[1]=a[1]-1;
        while(1)
        {colour_zhi=colourjudge(pcolor),printf("%d\r\n",colour_zhi);
        if((0<colour_zhi)&&(colour_zhi<6))break;}
//duoji(515,2495,1700,900,1670);        
//duoji(515,2495,2500,1600,1750);
duoji(515,2495,2500,1600,1760);        
if(colour_zhi==3)
{  lu_xian_qie_huan_zhong_ji(1,3);
         zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
        fang_zhi_jing_du();
fang_zhi_nu_biao_dian_tiao_jie(a_dian[3]+1);
                fang_zhi_di_ji_kuai(a_dian[3]+1);
                a_dian[3]=a_dian[3]+1;
                dao_che(3);
         lu_xian_qie_huan_zhong_ji(3,1);

}
if(colour_zhi==1)
{if(a[1]==0)
        {
         zou_dao_zhi_ding_fang_zhi_wei_zhi(1);        
                fang_zhi_jing_du();
                fang_zhi_nu_biao_dian_tiao_jie(a_dian[1]+1);
         fang_zhi_di_ji_kuai(a_dian[1]+1);        
                a_dian[1]=a_dian[1]+1;
                dao_che(1);
               
        }
        else
        {lu_xian_qie_huan_zhong_ji(1,2);
                fang_zhi_jing_du();
        while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[2]+1,0);
fang_zhi_di_ji_kuai(a[2]+1);
zhong_xin_wei_zhi_tiao_jie(a[2]+1,1);               
lu_xian_qie_huan_zhong_ji(2,1);
a[2]=a[2]+1;
        }
}
if(colour_zhi==5)
{
lu_xian_qie_huan_zhong_ji(1,4);
        fang_zhi_jing_du();
        while(1)
{

if(KEY10==0)a1=0,a2=0;else a1=-20,a2=-20;
if((KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[4]+1,0);
fang_zhi_di_ji_kuai(a[4]+1);
zhong_xin_wei_zhi_tiao_jie(a[4]+1,1);
lu_xian_qie_huan_zhong_ji(4,1);
a[4]=a[4]+1;
}
}
//zhong_xin_wei_zhi_tiao_jie(a[2]+1,0);
if(a[2]!=0)
{
                while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}


}
while(a[2]!=0)
{lu_xian_qie_huan_zhong_ji(1,2);
        fang_zhi_jing_du();
                while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}

        zhong_xin_wei_zhi_tiao_jie(a[2],0);

qian_jin_mai_chong(-10);
jia_qu_di_ji_kuai(a[2]);
qian_jin_mai_chong(10);

        zhong_xin_wei_zhi_tiao_jie(a[2],1);

a[2]=a[2]-1;        
lu_xian_qie_huan_zhong_ji(2,1);
zou_dao_zhi_ding_fang_zhi_wei_zhi(1);
        fang_zhi_jing_du();
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[1]+1);
        fang_zhi_di_ji_kuai(a_dian[1]+1);
        a_dian[1]=a_dian[1]+1;
        dao_che(1);
               
}
/////////////////////////////////
lu_xian_qie_huan_zhong_ji(1,5);/////////////μúsan¶Ñ
while(a[5])
{

        fang_zhi_jing_du();
                                        while(1)
{
if(KEY13==0)a2=0;else a2=-20;        
if(KEY10==0)a1=0;else a1=-20;
if((KEY13+KEY10)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[5],0);
        jia_qu_di_ji_kuai(a[5]);
zhong_xin_wei_zhi_tiao_jie(a[5],1);
        a[5]=a[5]-1;
        while(1)
        {colour_zhi=colourjudge(pcolor),printf("%d\r\n",colour_zhi);
        if((0<colour_zhi)&&(colour_zhi<6))break;}
//duoji(515,2495,1700,900,1670);        
//duoji(515,2495,2500,1600,1750);
        duoji(515,2495,2500,1600,1760);
if(colour_zhi==3)
{  lu_xian_qie_huan_zhong_ji(5,3);
        
         zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
        fang_zhi_jing_du();
fang_zhi_nu_biao_dian_tiao_jie(a_dian[3]+1);
                fang_zhi_di_ji_kuai(a_dian[3]+1);
                a_dian[3]=a_dian[3]+1;
                dao_che(3);
         lu_xian_qie_huan_zhong_ji(3,5);

}
if(colour_zhi==1)
{
        lu_xian_qie_huan_zhong_ji(5,1);
         zou_dao_zhi_ding_fang_zhi_wei_zhi(1);
   fang_zhi_jing_du();        
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[1]+1);
         fang_zhi_di_ji_kuai(a_dian[1]+1);        
                a_dian[1]=a_dian[1]+1;
                dao_che(1);
                lu_xian_qie_huan_zhong_ji(1,5);
}
if(colour_zhi==5)
{if(a[5]==0)
{
zou_dao_zhi_ding_fang_zhi_wei_zhi(5);        
        fang_zhi_jing_du();
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[5]+1);
fang_zhi_di_ji_kuai(a_dian[5]+1);        
                a_dian[5]=a_dian[5]+1;
                dao_che(5);
}        
else
{lu_xian_qie_huan_zhong_ji(5,2);
        fang_zhi_jing_du();
zhong_xin_wei_zhi_tiao_jie(a[2]+1,0);
        
fang_zhi_di_ji_kuai(a[2]+1);
zhong_xin_wei_zhi_tiao_jie(a[2]+1,1);               
lu_xian_qie_huan_zhong_ji(2,5);
a[2]=a[2]+1;}
}
}
if(a[2]!=0)
{
                while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}


}
while(a[2]!=0)
{        while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}
        lu_xian_qie_huan_zhong_ji(5,2);
        fang_zhi_jing_du();
        while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}
zhong_xin_wei_zhi_tiao_jie(a[2],0);        

jia_qu_di_ji_kuai(a[2]);
zhong_xin_wei_zhi_tiao_jie(a[2],1);        
a[2]=a[2]-1;        
lu_xian_qie_huan_zhong_ji(2,5);
zou_dao_zhi_ding_fang_zhi_wei_zhi(5);
        fang_zhi_jing_du();
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[5]+1);
        fang_zhi_di_ji_kuai(a_dian[5]+1);
        a_dian[5]=a_dian[5]+1;
        dao_che(5);

}



if(a[4]!=0)
{
                while(1)
{
if(KEY13==0)a2=0,a1=0;else a2=-20,a1=-20;        

if((KEY13)==0)break;
fastforward_turn(a1,a2);
}


}

while(a[4]!=0)
{lu_xian_qie_huan_zhong_ji(5,4);
        fang_zhi_jing_du();
        while(1)
{

if(KEY10==0)a1=0,a2=0;else a1=-20,a2=-20;
if((KEY10)==0)break;
fastforward_turn(a1,a2);
}
//zhong_xin_wei_zhi_tiao_jie(a[4],0);        
zhong_xin_wei_zhi_tiao_jie(a[4],0);               
jia_qu_di_ji_kuai(a[4]);
//zhong_xin_wei_zhi_tiao_jie(a[4],1);               
zhong_xin_wei_zhi_tiao_jie(a[4],1);        
qian_jin_mai_chong(10);
a[4]=a[4]-1;        
lu_xian_qie_huan_zhong_ji(4,5);
zou_dao_zhi_ding_fang_zhi_wei_zhi(5);
        fang_zhi_jing_du();
        fang_zhi_nu_biao_dian_tiao_jie(a_dian[5]+1);
        fang_zhi_di_ji_kuai(a_dian[5]+1);
        a_dian[5]=a_dian[5]+1;
        dao_che(5);
}
lu_xian_qie_huan_zhong_ji(5,3);
abc4=4;
while(abc4--)
{

abc2=-20,abc3=20;
fastforward_turn(abc2,abc3);        

}

while(1)
{
abc1=KEY25+KEY24+KEY23+KEY22;
if(KEY25==1)abc2=-40;else abc2=-70;
        if(KEY23==0)abc2=-100;
if(KEY24==1)abc3=-40;else abc3=-70;
        if(KEY22==0)abc3=-100;
if(abc1<=1)abc2=0,abc3=0;
fastforward_turn(abc2,abc3);        
if(abc1<=1)break;
}
abc4=40;
while(abc4--)
{

abc2=-150,abc3=-150;
fastforward_turn(abc2,abc3);        

}
fastforward_turn(0,0);        
while(1);



















//lu_xian_qie_huan_zhong_ji(3,colour_zhi);
//zou_dao_zhi_ding_fang_zhi_wei_zhi(colour_zhi);        
//fang_zhi_di_ji_kuai(1);
//dao_che(colour_zhi);
//if(colour_zhi!=3)
//lu_xian_qie_huan_zhong_ji(colour_zhi,3);        
}        
//printf("%d\r\n",colour_zhi);
//

        
//jia_qu_di_ji_kuai(3);
//colourjudge(pcolor);        
//lu_xian_qie_huan(3,2);
//zou_dao_zhi_ding_fang_zhi_wei_zhi(2);
//dao_che(2);
//lu_xian_qie_huan(2,1);
//zou_dao_zhi_ding_fang_zhi_wei_zhi(1);
//dao_che(1);        
//lu_xian_qie_huan(1,2);        
//zou_dao_zhi_ding_fang_zhi_wei_zhi(2);
//dao_che(2);        
//lu_xian_qie_huan(2,3);        
//zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
//dao_che(3);        
//lu_xian_qie_huan(3,4);        
//zou_dao_zhi_ding_fang_zhi_wei_zhi(4);
//dao_che(4);        
//lu_xian_qie_huan(4,5);
//zou_dao_zhi_ding_fang_zhi_wei_zhi(5);
//dao_che(5);        
//lu_xian_qie_huan(5,6);////////////////////////////////////////

//lu_xian_qie_huan(6,5);////////////////////////////////////////

//lu_xian_qie_huan(5,4);        
//zou_dao_zhi_ding_fang_zhi_wei_zhi(4);
//dao_che(4);        
//lu_xian_qie_huan(4,3);        
//zou_dao_zhi_ding_fang_zhi_wei_zhi(3);
//dao_che(3);        



        }
}