乐高四光感巡线思路解说

乐高四光感巡线思路解说
国际赛现场

视频
http://v.youku.com/v_show/id_XMzc3OTkzNjE4OA==.html

车头朝前光感从左到右编号分别是 1234
巡线原理
车辆初始摆放时，黑线在23光感之间

前进时的调整
23光感遇线进行快速前行时左右摆头的柔和调整
脱线时的调整
14光感遇线即认为车头脱线 ，1光感遇线需要刹车然后右转，直到2光感遇线，4号光感遇线需要刹车左转，直到3号光感遇线。

伪代码：

while(1){

        while (1)
        {
            // 前进时的调整 


            //获取4个光感的 光值 
            gg_1=getColorReflected(S1);
            gg_2=getColorReflected(S2);
            gg_3=getColorReflected(S3);
            gg_4=getColorReflected(S4);
            if(gg_2<gg_black){
                //2# hit
                if(gg_3<gg_black){
                    //2#3# hit
                }else{
                    //2# hit 3#no hit turnleft
                    LRM(50,100);                    
                }
            }else{
                if(gg_3<gg_black){
                    //3# hit 2#no hit
                    LRM(100,50);                    
                }else{
                    //2# no hit 3#no hit
                    LRM(100,100);                   
                }
            }
            a1=(gg_1<gg_black);
            b1=(gg_4<gg_black);
            if(a1||b1){
                break;
            }           
        }

        //脱线时的调整    
        if(a1){         
            //turn left
            LRM(0,100);
            do{
                gg_2=getColorReflected(S2);
            }while(!(gg_2<gg_black));
            //过调限制 
            LRM(100,0);
            delay(50);
        }else{          
            //turn right
            LRM(100,0);
            do{
                gg_3=getColorReflected(S3);
            }while(!(gg_3<gg_black));
            //过调限制 
            LRM(0,100);
            delay(50);
        }
    }

这样逻辑上没有错误，但车跑起来略显僵硬。
应该考虑以下调整优化的方向
一、23光感遇线前后的调整。统计左右光值的差距，计算出一个比例，作为左右轮转速调整的依据。
二、23光感遇线时的 快速调整。 2光感遇线使左轮速度快速的下降，3号光感遇线时，右轮轮速快速下降。
由于遇线时间短，可以用非线性的公式，让轮速快速的下降到一个合适的值。如每次循环让轮速设置为原轮速的80%。
如 sudu*=0.8;

三、14光感遇线前的调整，可以使用与一同样的调整方式。

四、14光感遇线时刹车制动的调整。根据线路图转弯的缓急可以调整是否加刹车（轮速为0，保持一段时间），
脱线回调时，条件是23光感遇线就停止回调，但存在过调的现象，可以加一个过调限制，保持短暂的时间。

五、最大速度问题。大长直线速度过快，由于车的惯性会导致车冲出离线太远，从而导致车寻不到线上，根据线路图设置最大速度。

调整左右轮速的依据是根据测得光值计算的结果，所以需要采用线性度比较接近的光感，或者对光感校对通过算法进行其线性度的补偿。

加速可以采用每侧2个中马达的的驱动方式。
伪代码

void LRM(float l,float r){
        setMotorSpeed(motorA, l);
        setMotorSpeed(motorB, l);
        setMotorSpeed(motorC, r*-1);
        setMotorSpeed(motorD, r*-1);
    } 

光感校对
先获取黑白的最小和最大值。min_gg_# 、max_gg_#
校对之后的值算法

gg_#=min_gg_#+(getColorReflected(S#)-min_gg_#)/(max_gg_#-min_gg_#)*(100-min_gg_#);

优化方向一
伪代码

void quick_goline(float xishu){
        float chazhi=(gg_left-gg_right)/gg_total*xishu;
        if(chazhi<0){//turn left
            gonglv_left=(1+chazhi)*gonglvMax;
            gonglv_right=gonglvMax;
            }else{//trun right
            gonglv_right=(1-chazhi)*gonglvMax;
            gonglv_left=gonglvMax;

        }
    }

方案测试
http://v.youku.com/v_show/id_XMzc3OTkzNjE5Mg==.html