读取三个光感的黑白范围（前文有光感校对示例原理）
rd_max_1rd_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){
//右脱，左拐
}
//...
}
点击查看视频效果