Page 1 of 3

Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 16:41
by ricardocrl
Hello all,

I've been trying to get used to the new position regulation methods in NXC, but it's not that easy to understand how to get everything I want. For instance:

1 - How to wait for an angle to be reached? My approach was:

Code: Select all

  PosRegSetAngle(OUT_A, 400);
  until(MotorTachoCount(OUT_A) >= 400);
But I'm not sure if it can go wrong sometimes, because the PID controller can balance around 400 and the call to MotorTachoCount may happen on the exact moments of lower angles than 400. If get it correctly, the final angle may be a little different than the goal, right?

2 - How to run and stop a motor, without angle regulation, without disabling the position regulation mode? For instance, stop a motor dependent on a sensor value? Do we have to switch regulation modes all the time?

Thanks for the help.

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 17:59
by mattallen37
1: Here is what I do. I use a function that checks if the position is >= the wanted position - the tolerance, and also if it is <= the wanted position + the tolerance. This means that I pass three values into the function; the port, the tolerance, and the position.

Here is an example:

Code: Select all

bool PosRegComplete(byte port, int tolerance, long position){
  return (MotorTachoCount (port)>=(position-tolerance)&&MotorTachoCount (port)<=(position+tolerance));
}

long Position;

task main ()
{
  PosRegEnable (OUT_A);
  PosRegSetMax (OUT_A, 0, 0);
  while (true)
  {
    Position=Random(360)-180;
    PosRegSetAngle (OUT_A, Position);
    until(PosRegComplete(OUT_A, 5, Position));
    ClearScreen();
    NumOut(0, LCD_LINE1, Position);
    PlayTone(1000,1);
  }
}
I have the PlayTone in there so you know when it has completed, and goes on to the next loop. Comment out the until line, and it will zoom so fast, that it will always be making a buzz.

The neat thing about these functions, is that it is all based on absolute position. It is not based on dead reckoning from the last position, or anything like that. Indeed, no PID is 100% perfect, so no, it won't get to the exact position every time. However, because it's based on absolute position, that little error will not add up over time :D

As far as your question 2, I'm not really sure... I haven't tried to do what you want.

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 19:39
by ricardocrl
Cool solution, with a fancy call. :-)

Then, I add the suggestion of using "inline" to get faster response about completeness.

The question remains about the tolerance. But as a parameter, we are always in charge to make sure nothing goes wrong. But if the PID doesn't perform that well + slow CPU to check MotorTachoCount(), we can get problems (we should pay attention to other tasks taking the cpu time, if they exist in the program). Maybe all these is veeery unlikely to happen.

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 20:38
by mattallen37
The tolerance is to set the acceptable range. The MotorTachoCount() will most certainly not be exactly the same as the position you requested, that's why I have the tolerance parameter.

The APR PID is in the FW, not program code. I don't think that keeping the CPU busy with program code has a big effect on the APR. I think the same is true for the MotorTachoCount(). And remember, the function I shared is for the program to know when the motor is in the acceptable position range. It should be used in the same loop/task as the other APR commands, so slow CPU due to multiple tasks shouldn't be an issue.

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 20:59
by linusa
I'm not sure that "checking TachoLimit within a certain tolerance" is the best way to do it. If those new NXC commands behave a bit like the original ones (RotateMotor*), then you probably should just wait until the runstate (I believe command "MotorRunState") ist not running (I believe OUT_MODE_RUNNING or OUT_RUNSTATE_RUNNING or similar) anymore.

Another way might be to check the motor's speed. You do it like this, pseudocode:
    1. Record current tacholimit
    2. Wait x milliseconds
    3. Calc diff in tacholimit (current - recorded)
    4. If diff (aka current speed) == 0: Motor stopped, break from this loop
    5. Else, go to 1.
This (or similar) also has the advantage that it will detect stalls and exit. A disadvantage is however, that in rare cases, should the PID control cause ocillations, the condition "tacholimit now == tacholimit some time ago" is true, but "tacholimit now != target tacholimit". In that case, the wait loop exited too early. You can work around this by requiring several "iterations while stopped", or other things.

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 21:02
by linusa
To avoid "wasting CPU", you should at least put a "Wait(1)" into your loop. The firmware only updates the I/O registers, such as TachoCount, once ever millisecond, anyway. Checking more than once every 1ms is unnecessary. Wait() will also give CPU time to other tasks...

Since CPU time is equally distributed amongst NXC tasks, you CAN'T "starve one task to death". If you've got N tasks, each task will get at least 1/Nth of the total available CPU time -- more, if some tasks use Wait()...

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 21:15
by mattallen37
linusa wrote:To avoid "wasting CPU", you should at least put a "Wait(1)" into your loop. The firmware only updates the I/O registers, such as TachoCount, once ever millisecond, anyway. Checking more than once every 1ms is unnecessary. Wait() will also give CPU time to other tasks...

Since CPU time is equally distributed amongst NXC tasks, you CAN'T "starve one task to death". If you've got N tasks, each task will get at least 1/Nth of the total available CPU time -- more, if some tasks use Wait()...
Sure it would be better, but probably only if used in an until statement. I posted an example showing one use of the function. You could use it in an if, while...

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 21:15
by mattallen37
linusa wrote:I'm not sure that "checking TachoLimit within a certain tolerance" is the best way to do it. If those new NXC commands behave a bit like the original ones (RotateMotor*), then you probably should just wait until the runstate (I believe command "MotorRunState") ist not running (I believe OUT_MODE_RUNNING or OUT_RUNSTATE_RUNNING or similar) anymore.

Another way might be to check the motor's speed. You do it like this, pseudocode:
    1. Record current tacholimit
    2. Wait x milliseconds
    3. Calc diff in tacholimit (current - recorded)
    4. If diff (aka current speed) == 0: Motor stopped, break from this loop
    5. Else, go to 1.
This (or similar) also has the advantage that it will detect stalls and exit. A disadvantage is however, that in rare cases, should the PID control cause ocillations, the condition "tacholimit now == tacholimit some time ago" is true, but "tacholimit now != target tacholimit". In that case, the wait loop exited too early. You can work around this by requiring several "iterations while stopped", or other things.
I like my method better. It's a lot more simple, and should require much less CPU time.

Edit: That's basically what I do to detect if a motor has stalled (like to re-home it).

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 21:23
by linusa
mattallen37 wrote: I like my method better. It's a lot more simple,
You are free to like whatever you want, of course. Simplicty is a big advantage of your method, yes. If you capsulate the other way into a method, however, you can use it just as easily.
mattallen37 wrote: and should require much less CPU time.
You're free to test it, but as of now, if you just use this

Code: Select all

until(PosRegComplete(OUT_A, 5, Position));
the complete opposite is true: You have a busy loop, using up the maximum amount of CPU. If you don't include a Wait() statement, there is no way to use up more CPU than in this loop. Seriously.

This method could also fail. I don't know how much overshooting / oscillating the new APR does, but in certain cases, there could be a varying load on the motors. If during the final "braking approach", let's say just 10 degrees before the target (TachoLimit), there was a huge increase in motor load, the PID regulation might need some time to regulate this. And in that case, your PosRegComplete() call would return true, while in reality, the APR is still trying to move the motor to the correct postion. Have a look at this picture, blue line: http://www.mindstorms.rwth-aachen.de/tr ... tateEx.png

I know that APR doesn't produce such big oscillations anymore, but who says this couldn't occur? So if you exit your "wait until finished" loop the first time the TachoCount passes the target tolerance zone, you might exit too early.

If the RUNSTATE == IDLE (i.e. != RUNNING) thing works, this will be the best option, since in that case the APR clearly says: "Ok, I'm done regulating, motor is free again."

Re: Absolute Position Regulation Tips [NXC]

Posted: 22 May 2011, 21:39
by mattallen37
linusa wrote:
mattallen37 wrote: and should require much less CPU time.
You're free to test it, but as of now, if you just use this

Code: Select all

until(PosRegComplete(OUT_A, 5, Position));
the complete opposite is true: You have a busy loop, using up the maximum amount of CPU. If you don't include a Wait() statement, there is no way to use up more CPU than in this loop. Seriously.
Yes I know it should have a Wait(1); or Yield() in the loop. Like I said though, that was an example of how you could use the function.
linusa wrote:This method could also fail. I don't know how much overshooting / oscillating the new APR does, but in certain cases, there could be a varying load on the motors. If during the final "braking approach", let's say just 10 degrees before the target (TachoLimit), there was a huge increase in motor load, the PID regulation might need some time to regulate this. And in that case, your PosRegComplete() call would return true, while in reality, the APR is still trying to move the motor to the correct postion. Have a look at this picture, blue line: http://www.mindstorms.rwth-aachen.de/tr ... tateEx.png

I know that APR doesn't produce such big oscillations anymore, but who says this couldn't occur? So if you exit your "wait until finished" loop the first time the TachoCount passes the target tolerance zone, you might exit too early.
It seems to regulate really well. I understand what you mean, and if that was an issue, I would do something like checking several times to be sure it wasn't flying past the position.
linusa wrote:If the RUNSTATE == IDLE (i.e. != RUNNING) thing works, this will be the best option, since in that case the APR clearly says: "Ok, I'm done regulating, motor is free again."
I don't think you understand the APR. It never says "Ok, I'm done regulating, motor is free again."; never. It is always actively trying to go to the position (that's what's so incredibly awesome about it).