[軌跡車範例] 比例控制循跡影片

本影片是100-101年度北區技專校院教學資源中心舉辦的「LEGO機器人研習營」的競賽題目。使用NXC程式語言控制樂高機器人完成循跡控制。

是不是走得很順暢呢?

[youtube=https://www.youtube.com/watch?feature=player_embedded&v=0ZMz-CS0bPY]

附上NXC程式碼:

//我是p控制task main()
{
SetSensorLight(S3);
int kp=2; //比例參數
int light_initial = Sensor(S3); //程式啟動時先抓一次光值,這時請將光感測器所投射出的光圈擺在黑線左測一半處
int light_realtime;                      //即時光值
int vl,vr;                                         //左右輪速

   while(true)
{
light_realtime = Sensor(S3);    //更新即時光值
vl = 40 + kp*(light_realtime-light_initial);    

      //計算左輪速,40 是基礎速度,代表當誤差為0時,機器人將以此速度直走
vr = 40 + kp*(light_realtime+light_initial);

      //計算右輪速, +/- 是代表進彎時的修正方向,可根據實際狀況修改

      OnFwd(OUT_B,vl);                      //出發!
OnFwd(OUT_C,vr);
}

}

 

 

發佈留言

發佈留言必須填寫的電子郵件地址不會公開。 必填欄位標示為 *