EV3 三光感巡线简易实现原理

点击查看视频效果

读取三个光感的黑白范围（前文有光感校对示例原理）

rd_max_1
rd_max_2
rd_max_3
rd_min_1
rd_min_2
rd_min_3


每个的中值


rd_zz_1
rd_zz_2
rd_zz_3


rd_zz_#=(rd_max_#-rd_min_#)/2+rd_min_#






读传感器当前实际值


rd_sj_1
rd_sj_2
rd_sj_3




在线状态(bool)0:在线 1:脱线


is_online_1
is_online_2
is_online_3
is_online_#=rd_sj_#>rd_zz_#




归一化


rd_gy_1
rd_gy_2
rd_gy_3


rd_gy_# =rd_sj_# /(rd_max_# -rd_min_# )






三光感数据到左右轮作用的转换




总量rd_gy_1+rd_gy_2+rd_gy_3+1   //之所以+1是防止除以0无意义


左边量rd_gy_1+rd_gy_2+0.5
右边量rd_gy_1+rd_gy_2+0.5




zy_left=左边量/总量
zy_right=右边量/总量






分配功率
ab为放大系数
左功率=zy_left*a+(zy_left-zy_right)*b;
右功率=zy_right*a+(zy_right-zy_left)*b;




记录光感在线和脱线的状态
000全在线
101中光感在线
110右光感在线
011左光感在线
111脱线
....
脱线前一次的状态为我们脱线后调整方向提供依据。必须得记录下来


curr_online
last_online


curr_online=is_online_1*100    +is_online_2*10    +is_online_3


if(curr_online!=111){


    last_online=curr_online;


}








脱线时的判断


if(curr_online==111){
    if(last_online==110){
        //左脱，右拐
    }
    if(last_online==11){
        //右脱，左拐
    }
    //...
}


点击查看视频效果