我正在接近Lego NXT编程,我从一个非常好的网站开始http://nxtprograms.com。特别是,我遵循了使用光传感器作为平衡传感器的简单Segway的说明(http://nxtprograms.com/NXT2/segway/index.html)。我研究了他的(Dave Parker)NXT-G程序,并在他的机器人和一个更简单的自定义版本(没有"驱动程序")上成功复制。无论是他的程序还是我的程序,一切都正常。我很快就转向了NXC编程,因为我已经了解C语言,并且想挖掘更大的潜力。我用相同的PID常数和计算很好地复制了平衡算法。结果是,C程序根本不起作用:机器人自我平衡1到2秒,振荡比NXT-G的振荡更大,然后摔倒了。
我比较了NXT-G和C的基本逻辑,没有发现任何区别。所以我想知道在我的程序中,电机控制是否出了问题。我想控制电机的NXT-G图形框做的更多的事情只是调用OnFwdSync(…)/OnRevSync(……)。这将解释为什么我的程序会产生大的初始振荡。
有没有人有NXC程序驱动简单赛格威lego的例子,或者可以解释为什么我的程序中的电机控制失败?在底部,我附上了我的程序的源代码。
谢谢。
#define KP 25
#define KI 1
#define KD 10
#define KO 0.5
#define LSAMPLE 100
//#include "MotorActions.nbc"
int balance(const int RIF)
{
int output = 0;
int previous_error = 0;
int error = 0;
int I = 0;
int D = 0;
int counter = 0;
int actual;
while (true)
{
actual = SENSOR_3;
error = actual - RIF;
counter++;
I += error;
D = (error - previous_error);
previous_error = error;
/**
* Compute de PID compensation
*/
output = KO * (KP * error + KI * I + KD * D);
if ((output > 1000) || (output < -1000))
break;
if (output < 0)
OnFwdSync(OUT_BC, -output, 0);
else
OnRevSync(OUT_BC, output, 0);
}
return (counter);
}
int read_light()
{
int L = 0;
int ctr = 0;
while (ctr < LSAMPLE)
{
L += SENSOR_3;
ctr++;
Wait(10);
}
return (L / LSAMPLE);
}
task main()
{
int i = 0;
int RIF;
int count;
while (i++ < 3)
{
//PlaySound(SOUND_LOW_BEEP);
Wait(800);
}
Wait(400);
//PlaySound( SOUND_DOUBLE_BEEP );
SetSensorColorRed(IN_3);
ClearSensor(SENSOR_3);
RIF = SENSOR_3;
count = balance(RIF);
Off(OUT_BC);
}
尝试更改常量。试着把KP提高到一个更大的值,32,也许吧?