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

Автор: Daniele Benedettelli

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

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

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

There are a number of additional motor commands that you can use to control the motors more precisely. In this chapter we discuss them: ResetTachoCount, Coast (Float), OnFwdReg, OnRevReg, OnFwdSync, OnRevSync, RotateMotor, RotateMotorEx, and basic PID concepts.

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

When you use the Off() command, the servo motor stops immediately, braking the shaft and holding position. It is also possible to stop the motors in a more gentle way, not using the brake. For this use Float() or Coast()command indifferently, that simple cut the power flowing to motor. Here is an example. First the robot stops using the brakes; next without using the brakes. Note the difference. Actually the difference is very small for this particular robot. But it makes a big difference for some other robots. task main() { OnFwd(OUT_AC, 75); Wait(500); Off(OUT_AC); Wait(1000); OnFwd(OUT_AC, 75); Wait(500); Float(OUT_AC); }

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

The commands OnFwd() and OnRev() are the simplest routines to move motors. The NXT servomotors have a built-in encoder that allows you to control precisely shaft position and speed; NXT firmware implements a PID (Proportional Integrative Derivative) closed-loop controller to control motors' position and speed using encoders as feedback. If you want your robot to go perfectly straight, you can use a synchronization feature that makes the selected couple of motors run together and wait for each other in case one of them is slowed down or even blocked; in a similar way, you can set a couple of motors to run together in sync, with a percentage of steering to turn left, right or spin in place, but always keeping sync. There are many commands to unleash servomotors' full power! 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!