# 2023实体驾驶（虹口区青少年活动中心初中组B队）

**Repository Path**: HKQQSNHDZX/Lizan

## Basic Information

- **Project Name**: 2023实体驾驶（虹口区青少年活动中心初中组B队）
- **Description**: 目前较为稳定的解决方案
- **Primary Language**: C
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 2
- **Forks**: 0
- **Created**: 2023-12-02
- **Last Updated**: 2024-03-01

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 2023年实体无人驾驶
# 车辆样式
![输入图片说明](PHOTO/1image.png)
![输入图片说明](PHOTO/2image.png)
## 一号车夹子卡齿问题
### 安装
【K-MON小车（防止卡齿）】 https://www.bilibili.com/video/BV1uH4y1k7Le/?share_source=copy_web&vd_source=93f2701ff6e7363fc4f5676786b836db
### 讲解
【K-安装】 https://www.bilibili.com/video/BV1vg4y1o7MP/?share_source=copy_web&vd_source=93f2701ff6e7363fc4f5676786b836db
# 代码
## 一号车
```
float t1=0;//时间初始值
float t2=0;//时间终止值
int flag=0;//白线纠正的情况值
int wall=3;//释放物块时检测墙距离
int wu=8;//检测物块
int kp;//巡线功率差系数
int l_yellow;//黄色边线

//servo2 控制抓取物块爪子马达
//servo3 控制物块挡板马达
//servo4 控制倒物块马达

void run(int a,int b,int c,int d);
void xunxian(int speed, int value, float kp);//巡黄线直行（speed：速度； value：黄线值； kp：功率差系数）
void jiuzheng();//白线平行纠正

void right();//路口右转
void left();//路口左转
void straight();//路口直行
void back(int fang);//掉头
void up(int xian, int quwu, int deng);//巡线直行（xian：黄实线1黄虚线0； quwu：拿物块1，不拿0； deng：等红绿灯1，不等0）
void stop();//停车入库
    
int main(void)
{
up(1,0,0);
straight();
up(1,1,0);
right();
up(0,0,0);
right();
up(1,1,0);
right();
up(1,1,0);

straight();
up(1,1,0);
right();
up(1,1,0);
right();
up(0,0,0);
right();
up(1,1,0);
right();
up(1,0,0);

left();
back(1);


left();
up(1,0,0);
right();
up(1,0,0);
right();
up(0,0,1);
straight();
up(0,1,1);
straight();
up(0,1,1);
straight();
up(0,0,0);
right();
up(1,0,0);
right();
up(1,0,0);

left();
back(1);

right();
up(1,0,0);
left();
up(1,0,0);
right();
stop();


}

void up(int xian, int quwu, int deng){
    if(xian==1){
        kp=1.2;     //黄线实线，巡线的功率差系数
    }else{
        kp=1.3;     //黄线虚线，巡线的功率差系数
    }
    if(quwu==1){
        t1=seconds(1);
        l_yellow=getadc(1);
        while(seconds(1)-t1<0.5){  //确保爪子放下时不触碰红绿灯框架
            xunxian(200,300,kp);
        }
        run(0,0,0,0);
        servo(2,0);     //放下爪子
        wait(1.4);
        while(getadc(wu)>50){       //巡线直到爪子检测到物块
            xunxian(150,300,kp);
        }
        run(0,0,0,0);
        servo(2,3600);      //收回
        wait(1.4);
        servo(2,3200);      //松开爪子使物块掉落
        if(deng==1){
            while(getport(1)==1){
                run(0,0,0,0);
            }
        }
        while(l_yellow>100){
            xunxian(200,300,kp);
        }
    }else{
        t1=seconds(1);
        l_yellow=getadc(1);
        while(seconds(1)-t1<0.5){
            xunxian(200,300,kp);
        }
        if(deng==1){
            while(getport(1)==1){
                run(0,0,0,0);
            }
        }
        while(l_yellow>100){
            xunxian(200,300,kp);
        }
    }
    jiuzheng();
}

void right(){
    run(400,400,160,160);
    wait(0.8);
    //run(0,0,0,0);
}

void left(){
    run(200,200,200,200);
    wait(0.7);
    run(200,200,400,400);
    wait(1);
    run(200,200,200,200);
    wait(0.35);
    //run(0,0,0,0);
}

void straight(){
    run(250,250,250,250);
    wait(1.9);
    //run(0,0,0,0);
}

void stop(){
    l_yellow=getadc(1);
    while(l_yellow>50){
        xunxian(200,300,0.8);
    }
    while(getadc(5)>50){
        run(200,200,200,200);
    }
    run(100,100,100,100);
    wait(0.15);
    run(0,0,0,0);
}

void back(int fang){
    l_yellow=getadc(1);
    while(l_yellow>50){     //巡线纠正车身
        xunxian(150,300,0.6);
    }
    run(0,0,0,0);
    run(70,70,285,285);     //掉头
    wait(1.8);
    jiuzheng();     //纠正车身
    run(0,0,0,0);
    if(fang==1){
        while(getadc(wall)>50){     //后退检测墙距离
            run(-200,-200,-200,-200);
        }
        run(-150,-150,-150,-150);
        wait(0.38);
        run(0,0,0,0);
        servo(2,0);     //放下爪子
        servo(3,900);   //打开挡板
        wait(1.4);
        servo(4,1400);  //倒物块
        wait(0.8);
        servo(4,2900);  //收回
        wait(0.8);
        servo(3,1800);
        wait(0.5);
        while(getadc(wall)<50){
            run(200,200,200,200);
        }
        wait(0.3);
        l_yellow=getadc(1);
        while(l_yellow>50){
            xunxian(150,300,0.8);       //巡线前进
        }
        servo(2,3600);
        servo(3,0);     //收回挡板
        jiuzheng();
    }
}

void run(int a,int b,int c,int d)
{  
    float ii=1.2;
    motor(1,-a*ii); motor(2,-b*ii); motor(3,c*ii); motor(4,d*ii);
}



void jiuzheng(){
    while(getadc(6)>300 && getadc(7)>300){      //直行找白线，6号左侧灰度传感器，7号右侧灰度传感器
        run(100,100,100,100);
    }    
    run(0,0,0,0);
    wait(0.2);
    if(getadc(6)<300 && getadc(7)<300){     //判断左右灰度传感器是否同时在白线上
        flag=0;     //同时在白线，情况0（flag=0）
    }else{
        flag=1;     //不同时在白线，情况1（flag=1)
    }
    t1=seconds(1);      //记录纠正前时间
    if(getadc(7)>300){      //左侧灰度在白线上，右侧灰度不在白线上，车身右偏
        while(getadc(7)>300||getadc(6)>300){        //使两侧灰度纠正后在白线上
            while(getadc(7)>300){
                run(0,0,100,100);       //右侧前进
            }
            while(getadc(6)>300){       //纠正前车身太偏，右侧前进后导致左侧灰度前进离开白线
                run(-100,-100,0,0);     //左侧后退
                flag=2;     //纠正后，左侧灰度在白线靠前位置，右侧灰度在白线靠后位置，情况2（flag=2）
            }
        }
        t2=seconds(1);      //记录纠正后时间
        if(flag==1){        //纠正后，仍有小幅度右偏，情况1
            run(0,0,100,100);
            wait((t2-t1)*0.25);     //根据纠正时间，乘以系数0.25，对右侧进行补偿，纠正车身
        }else if(flag==2){
            run(0,0,100,100);
            wait(0.33);     //纠正后，两侧灰度位置基本固定，右侧补偿时间固定
        }
    }
    else if(getadc(6)>300){     ////右侧灰度在白线上，左侧灰度不在白线上，车身左偏
        while(getadc(7)>300||getadc(6)>300){        //使两侧灰度纠正后在白线上
            while(getadc(6)>300){       //左侧前进
                run(100,100,0,0);
            }
            while(getadc(7)>300){       //纠正前车身太偏，左侧前进后导致右侧灰度前进离开白线
                run(0,0,-100,-100);     //左侧后退
                flag=2;     //纠正后，左侧灰度在白线靠前位置，右侧灰度在白线靠后位置，情况2（flag=2）
            }
        }
        t2=seconds(1);      //记录纠正后时间
        if(flag==1){        //纠正后，仍有小幅度左偏，情况1
            run(100,100,0,0);
            wait((t2-t1)*0.25);     //根据纠正时间，乘以系数0.25，对左侧进行补偿，纠正车身
        }else if(flag==2){
            run(100,100,0,0);
            wait(0.33);     //纠正后，两侧灰度位置基本固定，右侧补偿时间固定
        }
    }
    run(0,0,0,0);
}

void xunxian(int speed, int value, float kp){
    l_yellow=getadc(1);
    run(speed+kp*(value-l_yellow),speed+kp*(value-l_yellow),speed+kp*(l_yellow-value),speed+kp*(l_yellow-value));
}












```
## 二号车

```
//2023年智能驾驶（KBOT麦轮）1号出发位
/************端口定义************/
#define gd1 getadc(9)
#define gd2 getadc(10)
#define gd3 getadc(11)
#define gd4 getadc(12)
#define gd_fr1 getadc(13)
#define gd_fr2 getadc(14)
#define irr getadc(1)
#define irf getadc(2)
#define senor_qr getadc(3)   //只有2号小车安装了二维码模块
#define irr_cz getadc(4)
#define Open_MV getadc(5)

#define sev_gd 1   //控制右前方地灰的舵机
#define sev_1 2    //机械臂舵机
#define sev_2 3    //机械爪舵机

/************子函数定义************/
void go_line(float x);  //小车沿右侧黄线行进，X为行进时间
void go_fr(void);       //小车沿右侧墙行进 
void go_fr_t(float x);  //小车沿墙走一段时间
void go_fr_s(float x);  //小车沿右侧墙低速走一段时间或到路口
void goto_intersection(void);   //前进到路口（检测白线）
void goto_barrier(int x);   //前进到前方护栏
void goto_null(int x);      //高架环线内行驶
void goto_capsule(void);    //高架上核心舱维修
void intersection_handling(int x);   //路口处理 
void intersection_correct(void);    //路口矫正 
void QR_code(void);
void LED_light(int x);  //亮灯 x为亮灯次数
void sev_rst(void);     //舵机初始化   
void sev_catch_up(int x);    //抓取物块
void sev_put_down(int x);    //放物块
void red_led_light(void);   //红绿灯路口检测
void Auto_brake(int x,int y);

void Up_Viaduct(void);
void Viaduct_capsule(int x);//2号位置出发
void Viaduct_capsule_4(int x);//4号位置出发
void Down_Viaduct(void);
void SpaceBase(void);
void LabCenter(void);
void QRCenter(void);
void AstronautTraining(int x);//航天员训练基地-1号停车库

void run(int a,int b,int c,int d);  //4轮独立控制
void run_f(int x, int y);  //直排轮控制模式

int irr_4=500,irr_6=450,irr_9=385,irr_12=300;
int irr_null=210;//高架路口检测，
int irf_8=490,irf_brake=460;
int gd_m1=750,gd_m2=750,gd_m3=550,gd_m4=530;//白线左至右为1，2，3，4********
int gdfr_m1=740,gdfr_m2=700;//无护栏检测黄色标线********
int gd_m3_b=780;
int i_brake,i_QR,i_SpaceBase;
int radian=1;
float t_brake;

//舵机参数
int s_gd_up=200,s_gd_down=1850;

int s_up=2700;     //机械臂舵机抬升参考值
int s_down=950;  //机械臂舵机放下参考值**********
int s_catch=850; //手抓舵机抓取参考值
int s_open=1500;  //手抓舵机打开参考值

int main(void)
{
    //舵机初始化，再按运行键启动
    wait(0.5);
    sev_rst();
    //servo(sev_2,s_open);wait(1);
   
    //出库出发---小车前进驶离车库
    go_fr_t(0.2);  
    goto_intersection();
    intersection_handling(3); 
    goto_intersection();
    intersection_correct();
    intersection_handling(33);
    //核心舱控制中心
    QRCenter();
    
    Up_Viaduct();
    
    Viaduct_capsule(i_QR);//2号出发
	//Viaduct_capsule_4(i_QR); //4号出发 
    Down_Viaduct();
    QRCenter();
    AstronautTraining(i_QR);
    
    
}




/************行进路线子函数************/
void SpaceBase(void)
{
    go_line(0.8);
    servo(sev_gd,s_gd_up);wait(1);
    //前进到投放去并左转
    run_f(240,250); while(irf<irf_8) ;  run_f(0,0);sound(500,100);
    run_f(-350,350);wait(0.35);
    run_f(0,0);sound(500,100);
    
    //投放物块
    sev_put_down(1);
    i_SpaceBase++;
    go_fr_s(220);  //wait(3);
    
    //左转前进到黄边线
    run_f(-350,350);wait(0.35);
    servo(sev_gd,s_gd_down);
    run(200,380,200,380);wait(0.45);run_f(0,0);sound(500,100); //wait(3);
    while(gd_fr2>gdfr_m2&&gd_fr1>gdfr_m1) { run(300,0,300,0); }  run_f(0,0);sound(500,100);  //wait(3);****
    
    //沿黄边线到白线路口，并校正
    go_line(gd_m2);
    intersection_correct();
}

void LabCenter(void)
{
    go_line(0.8);
    servo(sev_gd,s_gd_up);wait(1);
    //前进到投放去并左转
    run_f(240,250); while(irf<irf_8) ;  run_f(0,0);sound(500,100);
    run_f(-350,350);wait(0.35);
    run_f(0,0);sound(500,100);
    
    //投放物块
    //go_fr_s(0.3);
    sev_put_down(1);
    go_fr_s(220);  //wait(3);
    
    //左转前进到黄边线
    run_f(-350,350);wait(0.35);
    servo(sev_gd,s_gd_down);
    run(200,380,200,380);wait(0.45);run_f(0,0);sound(500,100); //wait(3);
    while(gd_fr2>gdfr_m2&&gd_fr1>gdfr_m1) { run(300,0,300,0); }  run_f(0,0);sound(500,100);  //wait(3);
    
    //沿黄边线到白线路口，并校正
    go_line(gd_m2);
    intersection_correct();
}

void QRCenter(void)
{
    go_line(1);run_f(0,0);sound(500,100);//wait(3);
    //前进--左偏，检测二维码
    run_f(-220,220);servo(sev_gd,s_gd_up);wait(0.15);  
    run_f(0,0);sound(500,300);//wait(3);
    i_QR=senor_qr;
    while(i_QR<50)
    {
        run_f(-250,250);wait(0.1);run_f(0,0);wait(0.1);//wait(3);
        i_QR=senor_qr;
        run_f(250,-250);wait(0.1);run_f(0,0);wait(0.1);//wait(3);
        i_QR=senor_qr;
        break;
    }

    //前进到底线并左转
    run_f(220,-220);wait(0.1); 
    run_f(250,250);wait(0.5); while(gd3<gd_m3_b) ;  run_f(0,0);sound(500,100); //wait(3);
    run_f(-250,-250); wait(0.2);  run_f(0,0);sound(500,100);
    run_f(-350,350);wait(0.37);
    run_f(0,0);sound(500,100);
    
    //二维码处闪灯
    QR_code();
    
    //前进出二维码中心
    run_f(250,250); while(gd3<gd_m3_b) ;  run_f(0,0);sound(500,100);
    run_f(-250,-250); wait(0.3);  run_f(0,0);sound(500,100);
    run_f(-350,350);wait(0.36);
    run_f(0,0);sound(500,100);
    

    servo(sev_gd,s_gd_down);
    run(250,350,250,350);wait(0.3);run_f(0,0);sound(500,100); //wait(3);
    while(gd_fr2>gdfr_m2&&gd_fr1>gdfr_m1) { run(300,0,300,0); }  run_f(0,0);sound(500,100);  //wait(3);
    
    //沿黄边线到白线路口，并校正
    go_line(gd_m2);
    intersection_correct();
}

void Up_Viaduct(void)   //上高架--核心舱控制中心路口白线位置上高架（已路口校正）
{
    float ttt;
        
    //右转到高架口
    run_f(350,350);wait(0.45);  //====按实际调整
    run_f(350,-350);wait(0.4);  run_f(0,0);sound(500,100);//wait(3);
    run_f(380,380);wait(0.3);
    run_f(0,0);sound(500,10);//wait(3);

    //前进到平台
    while(irf < irf_8)   
    {
        if(irr > irr_6) run_f(300,550);
        else if(irr > irr_9) run_f(450,450); 
        else if(irr > irr_12) run_f(500,400);
        else run_f(500,350); 
    }
    run_f(-350,350);wait(0.4);
    run_f(0,0);sound(500,200);//wait(3);
    
    //前进到高架路口
    go_fr_t(1);  
    //goto_barrier(3);  //wait(3);
    ttt=seconds(1)+1.5;
    while(seconds(1) < ttt)
    {
        if(irr > irr_6) run_f(150,550);
        else if(irr > irr_9) run_f(500,500); 
        else run_f(500,350); 
    }
    run_f(0,0);sound(800,150);  //wait(3);
    
    goto_null(1);  
    radian=3;
    go_fr_t(1);  sound(800,150);//sound(800,500);sound(1000,1000);sound(800,500);sound(800,500);sound(1000,1000);sound(800,500);//wait(5);
    radian=1;
}
void Viaduct_capsule(int x)   //2号出发高架任务
{
    if(x==1)
    {
        goto_null(3); goto_null(3);
        goto_capsule();
        goto_null(2);
    }
    else if(x==2)
    {
        goto_null(3); goto_null(3); goto_null(3);
        goto_capsule();
        goto_null(3); goto_null(3); goto_null(3);goto_null(2);
    }
    else if(x==3)
    {
        goto_capsule();
        goto_null(3); goto_null(3); goto_null(2);
    }
    else if(x==4)
    {
        goto_null(3); 
        goto_capsule();
        goto_null(3); goto_null(2);
    }  
}

void Viaduct_capsule_4(int x)   //4号出发高架任务
{
    if(x==1)
    {
        goto_capsule();
        goto_null(3); goto_null(3); goto_null(2);
    }
    else if(x==2)
    {
        goto_null(3); 
        goto_capsule();
        goto_null(3); goto_null(2);
    }
    else if(x==3)
    {
        goto_null(3); goto_null(3);
        goto_capsule();
        goto_null(2);
    }
    else if(x==4)
    {
        goto_null(3); goto_null(3); goto_null(3);
        goto_capsule();
        goto_null(3); goto_null(3); goto_null(3);goto_null(2);
    }  
}
void Down_Viaduct(void)   //下高架
{
    radian=2;
    go_fr_t(2);  //sound(800,50);wait(2);
    radian=1;
    goto_barrier(2);//wait(3);
    
    //下平台到路口
    go_fr_s(0.65);//run_f(0,0);wait(5);
    go_fr_s(250);
    
    //右转到航天员训练管理中心路口黄边线
    run_f(400,120);
    servo(sev_gd,s_gd_down);wait(0.8);  run_f(0,0);sound(500,100);
    while(gd_fr2>gdfr_m2&&gd_fr1>gdfr_m1) { run(250,-250,250,-250); } 
    run_f(0,0);sound(800,100);  //wait(3);
}

void AstronautTraining(int x)//航天员训练基地-1号停车库
{
    if(x==1||x==3)
    {
        intersection_handling(1); 
        go_fr_t(0.2); 
        sev_put_down(1); 
        goto_intersection(); 
        intersection_handling(3); 
        go_fr_t(0.25);//红灯位置
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2); 
        go_fr_t(0.35);//红灯位置
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(1); 
        go_fr_t(0.3);//红灯位置
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2); 
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2);
        goto_intersection(); 
        intersection_handling(3);
        goto_barrier(1);
    }
    if(x==2||x==4)
    {
        intersection_handling(3);
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2);
        go_fr_t(0.3); 
        sev_put_down(1); 
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(1); 
        go_fr_t(0.3);//红灯位置
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(1); 
        go_fr_t(0.3);//红灯位置
        red_led_light();
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2); 
        goto_intersection(); 
        intersection_correct();
        intersection_handling(2);
        goto_intersection(); 
        intersection_handling(3);
        goto_barrier(1);
        
    }

}


/************功能子函数************/
void go_line(float x)
{
    float tt;
    
    //使用该函数前灰度舵机需已放下servo(sev_gd,s_gd_down);
    tt=seconds(1)+x;
    while(seconds(1) < tt)
    {
        if(gd_fr1<gdfr_m1) run_f(120,250);
        else if(gd_fr2<gdfr_m2) run_f(200,250);
        else run_f(250,120);
    
        if(x==gd_m2&&gd2<gd_m2) break;
    }
    run_f(0,0);
    servo(sev_gd,s_gd_up);
}

void go_fr(void)     //右侧 沿墙走
{ 
    if(irr > irr_6) run_f(250,550);
    else if(irr > irr_9) run_f(450,450); 
    else if(irr > irr_12) run_f(500,300); 
    else 
    { 
        if(radian==1) run_f(500,235);   //>>>地面沿墙走弧度控制
        else run_f(500,200);            //>>>高架进出口沿墙走弧度控制
    } 
}

void go_fr_t(float x)  //沿墙行走一段时间
{
    float tt;
    
    tt=seconds(1)+x;
    while(seconds(1) < tt)
    {
        if(irf>irf_brake)
        {
            i_brake=0;
            run_f(0,0);while(irf>(irf_brake-20)) { wait(0.05);i_brake++; } 
            wait(1);
            tt=tt+1+i_brake*0.05+0.03;
        }
        else
            go_fr();
    }
    run_f(0,0);
}

void go_fr_s(float x)     // 沿右侧墙低速行进--使用在投放物块时
{ 
    float tt,ttt;
  
    tt=seconds(1)+x;
    ttt=seconds(1)+0.1;
    while(1)
    {
        if(irr > irr_4) run_f(150,300);
        else if(irr > irr_6) run_f(300,300); 
        else if(irr > irr_9) run_f(300,200); 
        else run_f(300,120);   
        
        if(seconds(1) > tt) break;
        if(x==1000&&irr_cz<350&&seconds(1)>ttt) break;
        else if(x>100&&x<1000&&irr<x&&seconds(1)>ttt) break;
    }
    run_f(0,0);
}



void goto_barrier(int x)//前进到前方护栏 1n直行（建议偏左）前进，0n沿墙前进；  n1到墙停，n2到墙左转90度  n3到墙左转30度（用于高架第二平台）  
{
    while(irf<irf_8)   
    {
        if(x<10) go_fr();
        else run_f(240,250);
        
        if(x==3&&irf>285) break;
    }
    run_f(0,0);sound(500,100);
    if(x==2)
    {
        run_f(-350,350);wait(0.38);
        run_f(0,0);sound(500,100);
    }
    else if(x==3)
    {
        run_f(-350,350);wait(0.1);
        go_fr_t(0.4);
    }
}

void goto_intersection(void)//前进到路口
{
    while(gd2>gd_m2 && gd3>gd_m3) 
    {
        if(irf>irf_brake)
        {
            run_f(0,0);while(irf>(irf_brake-20)) { wait(0.05); } 
            wait(1);
        }
        else
            go_fr();
    }
    run_f(0,0);
}

void goto_null(int x)  //高架环线内行驶   x=1进环线  x=2前进到路口(带防撞）  x=3前进到路口并过路口
{
    while(irr>irr_null) 
    {
        if(irf>irf_brake&&x>1)
        {
            run_f(0,0);while(irf>(irf_brake-20)) { wait(0.05); } 
            wait(1);
        }
        else  //---如进环线时卡墙，可调整radian变量改变小车弧度
            go_fr();
    }
    if(x==3)   //>>>高架环线内过路口
    {
        run_f(260,360);wait(0.8);  //run_f(0,0);  wait(3);
        go_fr_t(0.3);  //sound(800,300);
    }
    run_f(0,0);
}
void goto_capsule(void)  //高架上核心舱维修
{
    //放下机械臂
    servo(sev_1,s_down+300);wait(0.5);//调机械臂放下高度，300，越小越低************
    sound(800,500);//sound(1200,500);
    
    while(irr_cz>300) 
    {
        if(irf>irf_brake)
        {
            run_f(0,0);while(irf>(irf_brake-20)) { wait(0.05); } 
            wait(1);
        }
        else
            go_fr();
    }
    go_fr_s(0.5);
    run_f(0,0);wait(0.1);
    run_f(-200,-200);wait(0.1);run_f(0,0);wait(0.1);
    //收回机械臂
    servo(sev_1,s_up);wait(0.6);sound(800,500);
    run_f(0,0);
}

void intersection_handling(int x)//路口处理(1/2/3普通路口，11/22/33转至黄线路口）   x=1/11直行   x=2/22左转    x=3/33右转 
{
    float tt;
    
    if(x==1||x==11)
    {
        run_f(300,290);wait(0.1);
        run_f(392,400);    //>>>小车过路口时建议左偏一点，避免撞到右侧墙（但不能压线）
        t_brake=seconds(1)+1.25;    //>>>控制小车过路口的时间
        Auto_brake(392,400); 
        if(x==1&&irr<irr_12)
        {
            while(irr<irr_9) { run(300,0,300,0); } 
            run_f(0,0);//wait(1);
        }
        else if(x==11)
        {
            run_f(0,0);servo(sev_gd,s_gd_down);sound(800,600);
            while(gd_fr2>gdfr_m2&&gd_fr1>gdfr_m1) { run(250,-200,250,-200); } 
            run_f(0,0);sound(800,100);
        }
    }    
    else if(x==2||x==22)
    {
        run_f(300,290);wait(0.1);
        run_f(400,400);  
        t_brake=seconds(1)+0.55;  //>>>控制小车直行到左转位（通过小车转弯后位置进行调整，转弯后小车整体过虚黄线）
        Auto_brake(400,400);
        run_f(90,400); 
        t_brake=seconds(1)+0.68;  //>>>小车左转角度时间（建议80-85度）
        Auto_brake(90,400);
        run_f(400,400);  
        t_brake=seconds(1)+0.6;   //>>>控制小车车头直行过路口
        Auto_brake(400,400);
        if(x==2&&irr<irr_12)
        {
            while(irr<irr_9) { run(300,0,300,0); } 
            run_f(0,0);//wait(1);  
        }
        else if(x==22)
        {
            run_f(0,0);servo(sev_gd,s_gd_down);sound(800,600);
            while(gd_fr2>gdfr_m2) run(250,-200,250,-200);
            run_f(0,0);sound(800,100);
        }
    }
    else if(x==3)
    {
        go_fr_t(0.8);
    }
    else if(x==33)
    {
        servo(sev_gd,s_gd_down);
        run_f(500,198);wait(0.7);    run_f(0,0);sound(800,100);  //---调时间让小车右转过路口，小车平行于黄边线（不能碰掉头标志线）;
        while(gd_fr2>gdfr_m2) run(250,-200,250,-200);
        run_f(0,0);sound(800,100);
    }
    run_f(0,0);//sound(500,100);wait(2);
}

void intersection_correct(void)//路口矫正
{
    while(gd2<(gd_m2+20) || gd3<(gd_m3+20))
    {
        if(gd2>gd_m2 && gd3<gd_m3) run_f(-120,200);
        else if(gd2<gd_m2 && gd3>gd_m3) run_f(200,-120);
        else run_f(180,180);
    }
    while(gd1>(gd_m1-20) || gd4>(gd_m4-20))
    {
        if(gd1<gd_m1 && gd4>gd_m4) run_f(-120,200);
        else if(gd1>gd_m1 && gd4<gd_m4) run_f(200,-120);
        else run_f(180,180);
    }
    run_f(0,0);sound(500,100);  //sound(800,500);  wait(1);
}

void LED_light(int x)//亮灯 x为亮灯次数
{
    int i;
    
    run_f(0,0);
    for(i=0;i<x;i++)
    {
        setport(4,1);sound(800,500);
        setport(4,0);wait(0.5);
    }
    if(x==1) wait(2);
    else if(x==2) wait(1);
}

void red_led_light(void)//红绿灯路口检测
{
    int iii=0;
    //run_f(-200,200);wait(0.1);run_f(0,0);
    while(1)
    {
        if(Open_MV>180&&Open_MV<280) iii++;else iii=0; if(iii>3) break;  //建议检测绿灯
        run_f(0,0);wait(0.02);
    }
}

void sev_rst(void)//舵机初始化
{
    servo(sev_gd,s_gd_up);wait(0.3);
    servo(sev_2,s_catch);wait(0.3);
    servo(sev_1,s_up);
    sound(800,300); sound(1200,300); 
    cls();printf("按运行键启动");
    while(trigger()==0) { wait(0.05); }
    cls();
}

void sev_catch_up(int x)//抓取物块
{
    //前进到抓物块位置
    go_fr_s(1000);
    if(x==1) go_fr_s(0.05);
    else if(x==2) go_fr_s(0.1);
    run_f(0,0);sound(800,350);wait(3);
    /*
    //靠近平台
    while(irr<irr_6) { run(200,-200,200,-200); }   //---通过irr6测距大小控制小车离平台距离
    run_f(0,0);sound(800,100); //wait(5);*/
    
    servo(sev_1,s_down+1000);wait(0.5);
    servo(sev_2,s_open);wait(0.5);
    servo(sev_1,s_down); wait(0.5); sound(800,300);
    if(x==1) servo(sev_2,s_catch);
    if(x==2) servo(sev_2,s_catch+100);
    wait(1);sound(800,200);
    servo(sev_1,s_up);wait(0.5);
    
    /*wait(2);servo(sev_1,s_down+1000);wait(0.5);
    servo(sev_2,s_catch);wait(0.5);
    servo(sev_1,s_up); wait(0.3); sound(800,300);*/
    
}

void sev_put_down(int x)//放下物块
{
    //投放位置确认--多个投放时避免堆叠掉落
    //go_fr_s(x/10);

    //靠近平台
    while(irr<irr_6) { run(200,-200,200,-200); }   //---通过irr6测距
    run_f(0,0);sound(800,100);
    
    //投放物块--如多个都放开爪碰物块时可调整投放角度
    servo(sev_1,s_down+100);wait(0.5);
    servo(sev_1,s_down); wait(0.5); sound(800,300);
    servo(sev_2,s_open);wait(0.5);sound(800,500);
    //收回爪子
    servo(sev_1,s_down+1000);wait(0.5);
    servo(sev_2,s_catch);wait(0.5);
    servo(sev_1,s_up); wait(0.3); sound(800,300);
}


void QR_code(void)//二维码模块识别 
{
    int iii,n=1;
    
   /* run_f(0,0);wait(0.1);
    iii=senor_qr;
    while(iii<50)
    {
        if(n==1) { run_f(250,250);wait(0.1);run_f(0,0);wait(0.1);n=2;}
        else if(n==2) { run_f(-250,-250);wait(0.1);run_f(0,0);wait(0.1);n=3;}
        else if(n==3) { run_f(-250,-250);wait(0.1);run_f(0,0);wait(0.1);n=4;}
        else if(n==4) { run_f(250,250);wait(0.1);run_f(0,0);wait(0.1);n=1;}
        iii=senor_qr;
    }*/
	if((i_QR>=80&&i_QR<=155)||(i_QR>=345&&i_QR<=405))
	{
        i_QR=1;LED_light(1);	
	}
	else if((i_QR>150&&i_QR<=215)||(i_QR>=405&&i_QR<=465))
	{
        i_QR=2;LED_light(2);	
	}
	else if((i_QR>215&&i_QR<=275)||(i_QR>=470&&i_QR<=530))
	{
        i_QR=3;LED_light(3);	
	}
	else 
	{
        i_QR=4;LED_light(4);	
	}
} 

void Auto_brake(int x,int y)
{
    while(seconds(1) < t_brake)
        {
            if(irf>irf_brake)
            {
                run_f(0,0);i_brake=0;
                while(irf>(irf_brake-20)) { wait(0.05);i_brake++; } 
                wait(1);
                t_brake=t_brake+1+i_brake*0.05+0.03;
            }
            else 
                run_f(x,y);
        }
}

void run_f(int x, int y)
{
    run(x,x,y,y);
}

void run(int a,int b,int c,int d)
{
    motor(1,a); motor(2,b); motor(3,c); motor(4,d);
}











```
### 二号车更改
出发点更改（88-89行）/机器抓往下幅度（498行）
## 摄像头 openmv（未改）

```
float t1=0;//时间初始值
float t2=0;//时间终止值
int flag=0;//白线纠正的情况值
int wall=3;//释放物块时检测墙距离
int wu=8;//检测物块
int kp;//巡线功率差系数
int l_yellow;//黄色边线

//servo2 控制抓取物块爪子马达
//servo3 控制物块挡板马达
//servo4 控制倒物块马达

void run(int a,int b,int c,int d);
void xunxian(int speed, int value, float kp);//巡黄线直行（speed：速度； value：黄线值； kp：功率差系数）
void jiuzheng();//白线平行纠正

void right();//路口右转
void left();//路口左转
void straight();//路口直行
void back(int fang);//掉头
void up(int xian, int quwu, int deng);//巡线直行（xian：黄实线1黄虚线0； quwu：拿物块1，不拿0； deng：等红绿灯1，不等0）
void stop();//停车入库
    
int main(void)
{
up(1,0,0);
straight();
up(1,1,0);
right();
up(0,0,0);
right();
up(1,1,0);
right();
up(1,1,0);

straight();
up(1,1,0);
right();
up(1,1,0);
right();
up(0,0,0);
right();
up(1,1,0);
right();
up(1,0,0);

left();
back(1);


left();
up(1,0,0);
right();
up(1,0,0);
right();
up(0,0,1);
straight();
up(0,1,1);
straight();
up(0,1,1);
straight();
up(0,0,0);
right();
up(1,0,0);
right();
up(1,0,0);

left();
back(1);

right();
up(1,0,0);
left();
up(1,0,0);
right();
stop();


}

void up(int xian, int quwu, int deng){
    if(xian==1){
        kp=1.2;     //黄线实线，巡线的功率差系数
    }else{
        kp=1.3;     //黄线虚线，巡线的功率差系数
    }
    if(quwu==1){
        t1=seconds(1);
        l_yellow=getadc(1);
        while(seconds(1)-t1<0.5){  //确保爪子放下时不触碰红绿灯框架
            xunxian(200,300,kp);
        }
        run(0,0,0,0);
        servo(2,0);     //放下爪子
        wait(1.4);
        while(getadc(wu)>50){       //巡线直到爪子检测到物块
            xunxian(150,300,kp);
        }
        run(0,0,0,0);
        servo(2,3600);      //收回
        wait(1.4);
        servo(2,3200);      //松开爪子使物块掉落
        if(deng==1){
            while(getport(1)==1){
                run(0,0,0,0);
            }
        }
        while(l_yellow>100){
            xunxian(200,300,kp);
        }
    }else{
        t1=seconds(1);
        l_yellow=getadc(1);
        while(seconds(1)-t1<0.5){
            xunxian(200,300,kp);
        }
        if(deng==1){
            while(getport(1)==1){
                run(0,0,0,0);
            }
        }
        while(l_yellow>100){
            xunxian(200,300,kp);
        }
    }
    jiuzheng();
}

void right(){
    run(400,400,160,160);
    wait(0.8);
    //run(0,0,0,0);
}

void left(){
    run(200,200,200,200);
    wait(0.7);
    run(200,200,400,400);
    wait(1);
    run(200,200,200,200);
    wait(0.35);
    //run(0,0,0,0);
}

void straight(){
    run(250,250,250,250);
    wait(1.9);
    //run(0,0,0,0);
}

void stop(){
    l_yellow=getadc(1);
    while(l_yellow>50){
        xunxian(200,300,0.8);
    }
    while(getadc(5)>50){
        run(200,200,200,200);
    }
    run(100,100,100,100);
    wait(0.15);
    run(0,0,0,0);
}

void back(int fang){
    l_yellow=getadc(1);
    while(l_yellow>50){     //巡线纠正车身
        xunxian(150,300,0.6);
    }
    run(0,0,0,0);
    run(70,70,285,285);     //掉头
    wait(1.8);
    jiuzheng();     //纠正车身
    run(0,0,0,0);
    if(fang==1){
        while(getadc(wall)>50){     //后退检测墙距离
            run(-200,-200,-200,-200);
        }
        run(-150,-150,-150,-150);
        wait(0.38);
        run(0,0,0,0);
        servo(2,0);     //放下爪子
        servo(3,900);   //打开挡板
        wait(1.4);
        servo(4,1400);  //倒物块
        wait(0.8);
        servo(4,2900);  //收回
        wait(0.8);
        servo(3,1800);
        wait(0.5);
        while(getadc(wall)<50){
            run(200,200,200,200);
        }
        wait(0.3);
        l_yellow=getadc(1);
        while(l_yellow>50){
            xunxian(150,300,0.8);       //巡线前进
        }
        servo(2,3600);
        servo(3,0);     //收回挡板
        jiuzheng();
    }
}

void run(int a,int b,int c,int d)
{  
    float ii=1.2;
    motor(1,-a*ii); motor(2,-b*ii); motor(3,c*ii); motor(4,d*ii);
}



void jiuzheng(){
    while(getadc(6)>300 && getadc(7)>300){      //直行找白线，6号左侧灰度传感器，7号右侧灰度传感器
        run(100,100,100,100);
    }    
    run(0,0,0,0);
    wait(0.2);
    if(getadc(6)<300 && getadc(7)<300){     //判断左右灰度传感器是否同时在白线上
        flag=0;     //同时在白线，情况0（flag=0）
    }else{
        flag=1;     //不同时在白线，情况1（flag=1)
    }
    t1=seconds(1);      //记录纠正前时间
    if(getadc(7)>300){      //左侧灰度在白线上，右侧灰度不在白线上，车身右偏
        while(getadc(7)>300||getadc(6)>300){        //使两侧灰度纠正后在白线上
            while(getadc(7)>300){
                run(0,0,100,100);       //右侧前进
            }
            while(getadc(6)>300){       //纠正前车身太偏，右侧前进后导致左侧灰度前进离开白线
                run(-100,-100,0,0);     //左侧后退
                flag=2;     //纠正后，左侧灰度在白线靠前位置，右侧灰度在白线靠后位置，情况2（flag=2）
            }
        }
        t2=seconds(1);      //记录纠正后时间
        if(flag==1){        //纠正后，仍有小幅度右偏，情况1
            run(0,0,100,100);
            wait((t2-t1)*0.25);     //根据纠正时间，乘以系数0.25，对右侧进行补偿，纠正车身
        }else if(flag==2){
            run(0,0,100,100);
            wait(0.33);     //纠正后，两侧灰度位置基本固定，右侧补偿时间固定
        }
    }
    else if(getadc(6)>300){     ////右侧灰度在白线上，左侧灰度不在白线上，车身左偏
        while(getadc(7)>300||getadc(6)>300){        //使两侧灰度纠正后在白线上
            while(getadc(6)>300){       //左侧前进
                run(100,100,0,0);
            }
            while(getadc(7)>300){       //纠正前车身太偏，左侧前进后导致右侧灰度前进离开白线
                run(0,0,-100,-100);     //左侧后退
                flag=2;     //纠正后，左侧灰度在白线靠前位置，右侧灰度在白线靠后位置，情况2（flag=2）
            }
        }
        t2=seconds(1);      //记录纠正后时间
        if(flag==1){        //纠正后，仍有小幅度左偏，情况1
            run(100,100,0,0);
            wait((t2-t1)*0.25);     //根据纠正时间，乘以系数0.25，对左侧进行补偿，纠正车身
        }else if(flag==2){
            run(100,100,0,0);
            wait(0.33);     //纠正后，两侧灰度位置基本固定，右侧补偿时间固定
        }
    }
    run(0,0,0,0);
}

void xunxian(int speed, int value, float kp){
    l_yellow=getadc(1);
    run(speed+kp*(value-l_yellow),speed+kp*(value-l_yellow),speed+kp*(l_yellow-value),speed+kp*(l_yellow-value));
}












```
## 识别二维码

```
# 二维码

import sensor, image, time, json
from pyb import UART,Pin, Timer,DAC

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)  # 必须关闭此功能，以防止图像冲洗…
clock = time.clock()
uart = UART(3,115200)
IO1 = 0
AD1 = 0
AD2 = 0
def data_out():
    if AD1 == 1 :
        data.append(('%04d , %04d ,%04d' %(100,200,int(AD2))))
    elif AD1==2 :
        data.append(('%04d , %04d ,%04d' %(100,400,int(AD2))))
    elif AD1==3 :
        data.append(('%04d , %04d ,%04d' %(100,600,int(AD2))))
    elif AD1==4 :
        data.append(('%04d , %04d ,%04d' %(100,800,int(AD2))))



while(True):
    data = []
    clock.tick()
    img = sensor.snapshot()
    img.lens_corr(1.8) # 利用软件进行畸变矫正 1.8的强度参数对于2.8mm镜头来说是不错的。
                        #OpenMV标配自带的镜头是2.8mm焦距的鱼眼镜头，会存在桶形畸变，会影响二维码识别
    for code in img.find_qrcodes(): # img.find_qrcodes()只有一个参数: roi感兴趣区
        img.draw_rectangle(code.rect(), color = (255, 0, 0))
        message = code.payload()
        if message=='1' or message=='A':
            AD1=1
        if message=='2' or message=='B':
            AD1=2
        if message=='3' or message=='C':
            AD1=3
        if message=='4' or message=='D':
            AD1=4
        data_out()
        data_send = json.dumps(set(data))
        uart.write(data_send +'\n')
        print('you sned:',data_send)
        #print(AD1)
    #print(clock.fps())




```

# 规则
见附件
# 比赛中的注意事项
## 蓝色场地调程序（防撞）
4辆车同时跑
红方蓝方随机
## 准备清单核对
| 工具 | A,B车    |
|----|---------|
|    | 车的充电线   |
|    | 电脑手机    |
|    | 拖线板     |
|    | 身份证，参赛证 |
|    | 备用电源    |

## 检查车辆
检查硬件：
1. 底灰线，电源线红外测障的距离，摄像头有没有下好程序
2. 底灰测值尽量测小
3. 沿墙走
4. 抓物块
5. 路口如果单独一个路口出现问题，就设条件这个路口单独编写(原因可能是白线贴的是歪的)
## 胶带粘场地
## 赛中重启
跟裁判确认物块的位置（只能在1分钟之内，不然没有意义）物块得摆放在大概宽度1CM左右
如重启：
1得让裁判把大小方块放好
2二号车上宇航员重放
3实验基地归位（一般来说不会在做完此任务后重启）

# 已解决问题
1. 红路灯测最快值（4秒后）（绿灯10黄灯2红灯12）
# 未解决问题
1. 红路灯检测采用路灯和黄灯都通过，更为了图方便去将路灯和黄灯的阈值放在一起，导致后面发现在黄灯最后几秒回闯红灯也没有时间去改
2. 二号车程序没用原版的方法，选择使用往右上偏移，没有考虑到它的寻黄线会压线，导致罚秒。
3. 一号车的夹子看似稳定，但它对方块的方位有很高要求，并没有去识别物块在哪里，当大方块稍微远一点就夹不到了，有机会的话我觉得还可以再改一下夹子的抓取的硬件
# 阈值 
| 黄线    |   |
|-------|---|
| 白线    |   |
| 悬空值   |   |
| 红色出发  |   |
| 蓝色出发  |   |
| 青色识别区 |   |
| 黄色识别区 |   |