master

分支 (1)

管理

管理

master

Lizan
/
车1改最新.c

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