[溫故知新] Java 版的P 控制循跡機器人

Java (leJOS) 的 P控制循跡機器人, 可以得到相當順暢的循跡效果。

參考資料: http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html

關鍵就在這兩個式子:

        vl = 200 + kp*(light.readValue() – light_initial);

vr = 200 – kp*(light.readValue() – light_initial);

1. light_initial是目標值, 也就是大約光感測器打出小紅圓圈一半位於白色場地, 一半位於黑色膠帶之上的光感測器數值。

2. light.readValue()是當下的光感測器數值, 兩者差值>0 會讓vl變小但vr變大, 機器人此時左轉, 差值愈大則機器人轉彎幅度愈大(愈接近原地旋轉)。

3. kp: 比例控制常數, 用來調整修正幅度, kp愈大機器人愈敏感。

4. 200 是基礎轉速, 就是當 light.readValue() 等於 light_initial 時, 機器人會以 200 前進。

如此機器人就能在直線是大幅減少偵測修正的次數, 並有效得知彎道的曲率。 是不是很棒呢?

來看看影片吧( NXC版本)。

============================================

import lejos.nxt.*;

public class Hy {

public static void main(String args[])
{
Button.ESCAPE.addButtonListener(new ButtonListener()
{
public void buttonPressed(Button b){System.exit(1);}
public void buttonReleased(Button b){}
});
int kp=20;
int light_initial = (30+60)/2; //half
LightSensor light = new LightSensor(SensorPort.S1);
int vl,vr;
while(true)
{

vl = 200 + kp*(light.readValue() – light_initial);
vr = 200 – kp*(light.readValue() – light_initial);
Motor.B.setSpeed(vl);
Motor.C.setSpeed(vr);
Motor.B.forward();
Motor.C.forward();
}
}
}

發佈留言

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