NXC: custom PID control for NXT?

[pepijndevos]

Have you seen this? http://www.mindstorms.rwth-aachen.de/tr ... torControl

[HaWe]

yes: it's code to sink a ship.
Or actually 2 or more.

Can you extract the code which is just for PID control?

[pepijndevos]

Actually, yea, it's here: http://www.mindstorms.rwth-aachen.de/tr ... e.nxc#L136

Note that this code is included multiple times with different defines to get around the variable problem. Even more magic is included for syncing, see http://www.mindstorms.rwth-aachen.de/tr ... r.nxc#L185

[HaWe]

the paragraph you pointed to is about synch mode of two 2motors which I actually don't need.
I need single control for 3 motors, but simultaneously - what surely will be possible.
I know that the PID (PD) code must be there somewhere over the rainbow - but where exactly, and which modules and helper functions are needed additionally?
I won't probably need the whole chunk - but which parts of it?

[linusa]

the paragraph you pointed to is about synch mode of two 2motors which I actually don't need.

No. Wrong. It's also about one single motor. The function does the same for 1 motor or for 2 synced motors. In any case, you control ONE SPEED. That's the abstraction / simplification. The syncing is done by the firmware.

I need single control for 3 motors, but simultaneously - what surely will be possible.

Yes, it does that.

It's included 3 times, so you get 3 functions for 3 motors, one for each motor. Called from 3 tasks. The reason is thread safety. It was developed when there were still some multi-tasking (compiler) bugs in NXC, and this was the only way to get it absolutely stable running.

I know that the PID (PD) code must be there somewhere over the rainbow - but where exactly, and which modules and helper functions are needed additionally?
I won't probably need the whole chunk - but which parts of it?

I've linked to that bit before in your other RotateMotorPID thread, but it seems you didn't read it. So I'll post it again. A good start is in line 563: http://www.mindstorms.rwth-aachen.de/tr ... e.nxc#L563

The K_D and K_P constants are used here for single motors: http://www.mindstorms.rwth-aachen.de/tr ... e.nxc#L613

Everything is done in fixpoint arithmetics, hence the scaling factors.

The crucial point is that this controller does NOT control position, but SPEED. Based on the distance to your target, there is a certain speed setpoint, and the controller tries to reach it -- this leads to a smooth, constant deceleration.

If you plot position vs time during such a constant deceleration, you get this curve: http://www.mindstorms.rwth-aachen.de/tr ... ration.png

If you go ahead and plot desired speed vs position, you can look up how fast you should be going at which position. This looks like this: http://www.mindstorms.rwth-aachen.de/tr ... om_pos.png . And that is exactly the lookup table.

Now, the helper function to set this new setpoint for speed (as in PWM power) is UpdatePower() (see http://www.mindstorms.rwth-aachen.de/tr ... e.nxc#L460 ), which is defined in MotorFunctions.nxc (I've linked to that file before).

It's all there, documented! I can't imagine a piece of code which more comments in it, really!

[HaWe]

well, thanks for your hints, but all I see is a HUGE function which does nothing I'm able to understand and where to begin and where to end.

Haven't you got a PID control which works for, say OUT_A, of maybe up to 30 lines which does the job? My own chess program and BT control and robot motor control is already nearly 4000 lines long and the memory is nearly completely filled by code. It's only a single function that I need, not a task, because it will have to be included in my own arm control task which controls elevation limits of it's own and of supplementary secondary limitations by all adjacent joints.
I'll copy that function for OUT_B and OUT_C by my own.

So I need a compact and effectively working short function (inline).

[mattallen37]

So what's wrong with my custom APR code? You tell it where you want the motor, and it goes there. In addition, it doesn't set the FW into some type of magic motor control mode, so you can still use standard motor commands intermittently. What more do you need?

[HaWe]

yes matt, I tried it, but the accuracy currently is >10 (in some experimental p,i,d adjustments >30) degrees, I'll need <5 (0,1,2,3,4) degrees. Thanks for reminding me to give you feed-back.

I'll have to figure out how to better adjust your p,i,d constants, maybe I'll manage to find a set of correct values, but I didn't get it yet. But I'll keep on trying. :)

[linusa]

I remember there was a "20 line minimal segway code" around the nxtasy forums once, which had a complete PID controller inside.

The best thing for you then, Helmut, ist to start small with your very own PID controller. You can use UpdatePower() from MotorFunctions.nxc, which is inline and just 1 call to SetOutput.

You can then create your own PID (or maybe just start with P) controller, which creates an error term from target and current position, and sets the power accordingly.

The other way is to keep looking, or to strip down the inner parts of MotorControl to the bare minimum you need -- start at the lines we've linked to. I see however, that you might not have enough memory for that lookup table. You could generate this on the fly (i.e. NO lookup) or use a table with less resolution! (Compiled) code size was no design objective for MotorControl, so it's not optimized for it, but that can be done. The advantage of your very OWN PID controlle would be, that you can do real position control, and not my "exotic" way of speed control.