PID Controll For Lego Mindstorms Robots

http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

比例式循跡:

光測值: Sm=0.5*(Sb+Sw)
直線時 Motor A 輸出值: Msa
直線時 Motor B 輸出值: Msb

Black
光測值: Sb
全黑時 Motor A 輸出值: Mba
全黑時 Motor B 輸出值: Mbb

White
光測值: Sw
全白時 Motor A 輸出值: Mwa
全白時 Motor B 輸出值: Mwb

if ( S >= Sm ) then
Ma = Msa + (S-Sm)/(Sw-Sm) * (Mwa-Msa);
Mb = Msb + (S-Sm)/(Sw-Sm) * (Mwb-Msb);
else // (S < Sm)
Ma = Msa + (Sm-S)/(Sm-Sb) * (Mba-Msa);
Mb = Msb + (Sm-S)/(Sm-Sb) * (Mbb-Msb);
end if