Программирование LEGO NXT роботов на языке NXC - Еще раз о моторах

Автор: Daniele Benedettelli

Перевод: © Ботов Антон aka =DeaD=, 2009

Эксклюзивно для www.roboforum.ru
копирование на другие ресурсы и публикация перевода
без разрешения его автора запрещены

Еще раз о моторах

Существует несколько дополнительных команд для моторов, которые вы можете использовать для более точного управления моторами. В этой главе мы обсудим эти команды: ResetTachoCount, Coast (Float), OnFwdReg, OnRevReg, OnFwdSync, OnRevSync, RotateMotor, RotateMotorEx, а также основы ПИД-управления.

Плавная остановка

Когда вы используете команду Off(), сервомотор мгновенно останавливается, блокируя вал и удерживая позицию. Существует способ более мягкой остановки мотора не используя блокировку вала. Для этого используйте команды Float() или что то же самое - Coast(), они просто отключают питание моторов. Вот пример, сначала робот останавливается используя блокировку вала, а потом останавливается без использования блокировки. Обратите внимание на разницу. На самом деле для конкретно этого робота разница достаточно незаметна, но для других роботов эта разница будет гораздо больше.

task main()
{
  OnFwd(OUT_AC, 75);
  Wait(500);
  Off(OUT_AC);
  Wait(1000);
  OnFwd(OUT_AC, 75);
  Wait(500);
  Float(OUT_AC);
}

Дополнительные команды

Команды OnFwd() и OnRev() являются простейшими командами для управления моторами.

Сервомоторы из набора NXT имеют встроенные энкодеры, которые позволяют вам достаточно точно контролировать положение вала и скорость его вращения. Встроенная в NXT прошивка реализует алгоритм ПИД-управления (Пропорциональный Интегральный Дифференциальный - от названия основных трёх его компонент) положением вала и скоростью его вращения с обратной связью через энкодеры.

Если вы хотите, чтобы ваш робот ездил строго по прямой, вы можете использовать возможность синхронизации двигателей, которая объединяет их в пару, в которой каждый двигатель замедляется, если второй не успевает крутиться с нужной скоростью. Подобным образом вы можете настроить пару моторов на работу в синхронизированном режиме с процентом скорости левого двигателя по отношению к правом, для поворота направо, налево или вращения на месте, при этом траектория будет выполняться достаточно точно. Существует множество команд, которые позволяют полностью использовать возможности сервомоторов из набора NXT! OnFwdReg(‘ports',‘speed',‘regmode') drives the motors specified by ‘ports' at the ‘speed' power applying the regulation mode that can be either OUT_REGMODE_IDLE, OUT_REGMODE_SPEED or OUT_REGMODE_SYNC. If IDLE is selected, no PID regulation will be applied; if SPEED mode is selected, the NXT regulates single motor to get a constant speed, even if load on motor varies; finally, if SYNC is selected, the couple of motors specified by ‘ports' move in sync as explained before. OnRevReg() acts as the precedent command, reversing direction.

task main() { OnFwdReg(OUT_AC,50,OUT_REGMODE_IDLE); Wait(2000); Off(OUT_AC); PlayTone(4000,50); Wait(1000); ResetTachoCount(OUT_AC); OnFwdReg(OUT_AC,50,OUT_REGMODE_SPEED); Wait(2000); Off(OUT_AC); PlayTone(4000,50); Wait(1000); OnFwdReg(OUT_AC,50,OUT_REGMODE_SYNC); Wait(2000); Off(OUT_AC); } This program shows well different regulation if you try to stop wheels holding the robot in your hand: first (IDLE mode), stopping a wheel you will not notice anything; then (SPEED MODE), trying to slow down a wheel, you'll see that NXT increases motor's power to overcome your hold, trying to keep speed constant; finally (SYNC mode), stopping a wheel will cause the other one to stop, waiting for the blocked one. OnFwdSync(‘ports',‘speed',‘turnpct') is the same as OnFwdReg() command in SYNC mode, but now you can also specify the 'turnpct' steering percentual (from -100 to 100). OnRevSync() is the same as before, simply reversing the motor's direction. The following program shows these commands: try changing steering number to see how it behaves. task main() { PlayTone(5000,30); OnFwdSync(OUT_AC,50,0); Wait(1000); PlayTone(5000,30); OnFwdSync(OUT_AC,50,20); Wait(1000); PlayTone(5000,30); OnFwdSync(OUT_AC,50,-40); Wait(1000); PlayTone(5000,30); OnRevSync(OUT_AC,50,90); Wait(1000); Off(OUT_AC); } Finally, motors can be set to turn by a limited number of degrees (remember that a full turn is 360°). For both following commands, you can act on motor's direction changing either the sign of the speed or the sign of the angle: so, if speed and angle have the same sign, motor will run forwards, if their sign is opposite, the motor will run backwards. RotateMotor(‘ports',‘speed',‘degrees') rotates the motor shaft specified by ‘ports' by a ‘degrees' angle at ‘speed' power (in 0-100 range).

task main() { RotateMotor(OUT_AC, 50,360); RotateMotor(OUT_C, 50,-360); } RotateMotorEx(‘ports',‘speed',‘degrees',‘turnpct',‘sync', 'stop') is an extension of the precedent command, that lets you synchronize two motors (e.g. OUT_AC) specifying a ‘turnpct' steering percentage (from -100 to 100) and a boolean flag ‘sync' (that can be set to true or false). It also lets you specify whether the motors should brake after the angle of rotation has completed using the boolean flag 'stop'. task main() { RotateMotorEx(OUT_AC, 50, 360, 0, true, true); RotateMotorEx(OUT_AC, 50, 360, 40, true, true); RotateMotorEx(OUT_AC, 50, 360, -40, true, true); RotateMotorEx(OUT_AC, 50, 360, 100, true, true); }

ПИД-управление

NXT firmware implements a digital PID (proportional integrative derivative) controller to regulate servomotors' position and speed with precision. This controller type is one of the simplest yet most effective closed loop feedback controller known is automation, and is often used. In rough words, it works so (I'll talk about position regulation for a discrete time controller): Your program gives the controller a set point R(t) to reach; it actuates the motor with a command U(t) , measuring its position Y(t) with the built-in encoder and calculates an error E(t) = R(t) – Y(t): here's why it is called a "closed loop controller", because the output position Y(t) is brought back to the controller's input to calculate the error. The controller transforms the error E(t) into the command U(t) so: U(t) = P(t) + I(t) + D(t), where P(t) = KP·E(t), I(t) = KI·( I(t–1) + E(t) ) and D(t) = KD·(E(t) – E(t –1)). It could seem quite hard for a novice, but I'll try to explain this mechanism as best as I can. The command is the sum of three contributes, the proportional part P(t), the integrative part I(t) and the derivative part D(t) . P(t) makes the controller quick in time, but it does not assure a null error at equilibrium; I(t) gives “memory” to the controller, in the sense that it takes trace of accumulated errors and compensates them, with the guarantee of a zero error at equilibrium; D(t) gives “future prediction” to the controller (as derivation in math), speeding up response. I know this can still be confusing, consider that entire academic books have been written on this argument! But we can still try it online, with our NXT brick! The simple program to fix things into memory is the following.

1. define P 50
2. define I 50
3. define D 50

task main(){ RotateMotorPID(OUT_A, 100, 180, P, I, D); Wait(3000); } The RotateMotorPID(port,speed, angle, Pgain,Igain,Dgain) let you move a motor setting different PID gains from the default ones. Try setting the following values (50,0,0): the motor does not rotate 180° exactly, since an uncompensated error remains (0,x,x): without proportional part, the error is very big (40,40,0): there's an overshoot, that means the motor shaft moves beyond the set point and then turns back (40,40,90): good precision and raising time (time to reach the set point) (40,40,200): the shaft oscillate, since derivative gain is too high Try other values to discover how these gains influence a motor's performance.

Подводим итоги

In this chapter you learned about the advanced motor commands available: Float(),Coast() that stop the motor gently; OnXxxReg(), and OnXxxSync() that allow feedback control on motors' speed and sync; RotateMotor() and RotateMotorEx() are used to turn motor's shaft by a precise number of degrees. You learned something about PID control too; it has not been an exhaustive explanation, but maybe I have caused a bit of curiosity in you: search the web about it!