讀取三個光感的黑白範圍（前文有光感校對示例原理）
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
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;
111脫線
....
脫線前一次的狀態為我們脫線後調整方向提供依據。必須得記錄下來
curr_online
last_online
curr_online=is_online_1*100 +is_online_2*10 +is_online_3
if(curr_online!=111)
脫線時的判斷
if(curr_online==111)
if(last_online==11)
//...
}