RotateMotorPID simultaneously: 3 motors, 3 different targets

[HaWe]

I want to start 3 tasks simultaneously for 3 motors to approach multithreaded 3 different encoder targets.
Using my "homebrewed tasks" it works, but if I try RotateMotorPID instead it fails.

Code: Select all

#define TT 0
#define A1 1
#define A2 2

long TTtarget;
char TTrdy;

task  RotateToTargetTT() {
  char port=TT, pwr=-80;

  TTrdy=0; Wait(1);

  RotateMotorPID(port, pwr,  MEnc(port)-TTtarget, 20, 40, 100);

  Off(port);
  Wait(50);
  TTrdy= true;
}


long A1target;
char A1rdy;

task  RotateToTargetA1() {

  char port=A1, pwr=-100;
  A1rdy=0; Wait(1);

  RotateMotorPID(port, pwr,  MEnc(port)-A1target, 20, 40, 100);

  Off(port);
  Wait(50);
  A1rdy= true;
}


long A2target;
char A2rdy;

task  RotateToTargetA2() {

  char port=A2, pwr=-100;
  A2rdy=0; Wait(1);

  RotateMotorPID(port, pwr,  MEnc(port)-A2target, 20, 40, 100);

  Off(port);
  Wait(50);
  A2rdy= true;
}

task main() {
    TTtarget= -500;    start RotateToTargetTT;
    A1target=-2500;    start RotateToTargetA1;
    A2target=+1500;    start RotateToTargetA2;

    while (!TTrdy);  while (!A1rdy);  while (!A2rdy); // wait until all motors are finished

    TTtarget=+1200;    start RotateToTargetTT;
    A1target=+1000;    start RotateToTargetA1;
    A2target=-3000;    start RotateToTargetA2;

    while (!TTrdy);  while (!A1rdy);  while (!A2rdy); // wait until all motors are finished

   //...

   while(1);
}

But if I do it this way, all motors run one after each other but not simultaneously as I intended.
Does RotateMotorPID aquire a mutex so that all other motors can't be started at the same moment?

How can I break this behaviour to make all motors run multithreaded simultaneously ?



edit:
e.g., a homebrewed function which is working also multithreaded (but not exactly enough) looks like this:

Code: Select all

long TTtarget;
char TTrdy;
task  RotateToTargetTT() {
  char port=TT, pwr=-100;
  long dabs, oldabs;
  TTrdy=0; Wait(1);

  while (!(inArea(MEnc(port),TTtarget,5))) {
    dabs=abs(MEnc(port)-TTtarget);
    if (dabs>80) pwr=-100;
    else if (dabs<80) pwr=-70;
    else if (dabs<=60) pwr=-60;
    if ((oldabs<=dabs)&&(dabs<50)) pwr=-100;
    if ((inArea(MEnc(port),TTtarget,5))) { Off(port); Wait(10);}
    else {
      if (MEnc(port)<TTtarget) { OnFwd(port,-pwr); Wait(10);}
      if (MEnc(port)>TTtarget) { OnFwd(port,pwr); Wait(10);}
    }
    oldabs=dabs;
    Wait(1);
  }
  Off(port);
  Wait(50);
  TTrdy= true;
}

(the other 2 analogously)

[afanofosc]

The RotateMotor* routines acquire all 3 motor ports IF you use a variable instead of a constant for the motor port. That is the only way to make the routine thread-safe (i.e., callable by multiple threads simultaneously). If you use a constant the compiler uses __RM0 as the subroutine for port if you use OUT_A, __RM1 if you use OUT_B, and __RM2 if you use OUT_C (and more routines for combined motor constants). It uses _RMVar if you use a variable - and no other thread will be able to use the motors while one thread has those resources acquired.

Why, on earth, do you put the port constant into a char variable when you can just pass the constant into the routine?

John Hansen

[HaWe]

as you might recall I actually don't live on Earth.

On Betelgeuze, we always use variables to keep variable and agile mentally and physically - both in real and imaginary and transcendental (3rd level Thetan).
The advantage is clearly to see for us: we only have to write a function once and may use it several times just exchanging the value of the variable.

4th level Thetan requires HFP (Human Firmware Programming), I haven't finished this subject yet (to be honest, only very few Betelgeuzians actually have).

So could you please describe a dumb 3rd level Betelgeuzian how in the Entire Universerse I will have to rewrite my 3 PID regulation tasks?

Edit:
My mentor, Slartibartfass, who once was more familiar with tricky earthly affairs, recommended - without assuming any liability - to try it just by OUT_A, OUT_B, OUT_C... ?

kindly,
Ford Doc.

[afanofosc]

From the NBC history.txt file:

- Overhauled the I2C routines and the Motor routines. The old OUT_AB, OUT_AC,
OUT_BC, and OUT_ABC array variables are now constants. The motor API functions
are now thread-safe and blocking on the motor resource(s) requested. If you call
RotateMotor(OUT_AB, 75, 3600) then no other thread will be able to control motors
A and B until the already executing RotateMotor function finishes. But now you
can safely call RotateMotor(OUT_A, 75, 3600) on one thread and RotateMotor(OUT_B,
75, 720) on another simultaneously executing thread without having program errors
or erratic motor behavior. The same sort of revisions occurred for the Lowspeed
(aka I2C) routines so that it is now safe to call I2C functions for a port on one
thread while calling I2C functions for a different port on another thread. Since
the functions do not know at compile time which port is being utilized when you
use a variable as the port parameter, the RotateMotor functions acquire all three
motor port resources for the duration of their operation in that case. So you can
safely call RotateMotor using a variable for the port parameter on two
simultaneously executing threads but one of the two calls will be blocked until
the first one has completed executing.

If I were from another planet and had a hard time understanding the ways of humans I would rewrite my code like this:

Code: Select all

#define TT 0
#define A1 1
#define A2 2

long TTtarget;
char TTrdy;

task  RotateToTargetTT() {
  // hmmm, now that I think about it, it would be silly to store these two values in a variable when I 
  // use them 2 lines below and I have already used #defines above for the port number
  // maybe I'll use a #define for the power level too, if I think I might change it
  // and it gets used multiple times. 
//  char port=TT, pwr=-80;
  #define TT_PORT_WHY_NOT_BE_VERBOSE TT
  #define TT_POWER_LEVEL_JUST_IN_CASE -80

  TTrdy=0; Wait(MS_1);

  // what in tarnation is MEnc()?
  RotateMotorPID(TT_PORT_WHY_NOT_BE_VERBOSE, TT_POWER_LEVEL_JUST_IN_CASE,  MEnc(TT_PORT_WHY_NOT_BE_VERBOSE)-TTtarget, 20, 40, 100);

  Off(TT_PORT_WHY_NOT_BE_VERBOSE);
  Wait(MS_50); // I should use those pre-defined constants from NBCCommon.h, shouldn't I
  TTrdy= true;
}


long A1target;
char A1rdy;

task  RotateToTargetA1() {

  #define A1_PORT_WHY_NOT_BE_VERBOSE A1
  #define A1_POWER_LEVEL_JUST_IN_CASE -100
//  char port=A1, pwr=-100;
  A1rdy=0; Wait(MS_1);

  RotateMotorPID(A1_PORT_WHY_NOT_BE_VERBOSE, A1_POWER_LEVEL_JUST_IN_CASE,  MEnc(A1_PORT_WHY_NOT_BE_VERBOSE)-A1target, 20, 40, 100);

  Off(A1_PORT_WHY_NOT_BE_VERBOSE);
  Wait(MS_50);
  A1rdy= true;
}


long A2target;
char A2rdy;

task  RotateToTargetA2() {

  #define A2_PORT_WHY_NOT_BE_VERBOSE A2
  #define A2_POWER_LEVEL_JUST_IN_CASE -100
//  char port=A2, pwr=-100;
  A2rdy=0; Wait(1);

  RotateMotorPID(A2_PORT_WHY_NOT_BE_VERBOSE, A2_POWER_LEVEL_JUST_IN_CASE,  MEnc(A2_PORT_WHY_NOT_BE_VERBOSE)-A2target, 20, 40, 100);

  Off(A2_PORT_WHY_NOT_BE_VERBOSE);
  Wait(MS_50);
  A2rdy= true;
}

// my code uses the "start" keyword so I will rewrite it so that is not necessary.

task phase1()
{
  Precedes(RotateToTargetTT, RotateToTargetA1, RotateToTargetA2);
  TTtarget = -500;
  A1target = -2500;
  A2target = 1500; 
  // when this task ends the three tasks it precedes will all start simultaneously 
}

task phase2()
{
  Precedes(RotateToTargetTT, RotateToTargetA1, RotateToTargetA2);
  TTtarget = 1200;
  A1target = 1000;
  A2target = -3000; 
  // when this task ends the three tasks it precedes will all start simultaneously 
}

task phase1completion()
{
  Follows(phase1, RotateToTargetTT, RotateToTargetA1, RotateToTargetA2); // all 4 must finish before this task starts
  Precedes(phase2);
}


task main() {
  Precedes(phase1);
/*
    TTtarget= -500;    start RotateToTargetTT;
    A1target=-2500;    start RotateToTargetA1;
    A2target=+1500;    start RotateToTargetA2;

    while (!TTrdy);  while (!A1rdy);  while (!A2rdy); // wait until all motors are finished

    TTtarget=+1200;    start RotateToTargetTT;
    A1target=+1000;    start RotateToTargetA1;
    A2target=-3000;    start RotateToTargetA2;

    while (!TTrdy);  while (!A1rdy);  while (!A2rdy); // wait until all motors are finished

   //...

   while(1);
*/
}

The above code was written directly in this editor so it may not compile and may not actually work correctly. You can use the dreaded start keyword if you must but I would try to make it work using Precedes and Follows.

John Hansen

[HaWe]

very funny indeed, John!

first to clear your state of disorder about MEnc: it was
#define MEnc(m) MotorRotationCount(m)
I simply had no desire to each time write a whole medium-sized novel in case of polling an encoder value.

The idea that a constant passed to a function should cause a different behaviour than if had been passed to it by a variable of the same amount is probably supposed to be one of the most admirable achievements of the human mind.

To conceal this acquisition of programming accomplishment in a small paragraph of an even smaller history file which obviously could not have been overlooked is a supplementary masterpiece (congratulation, it's almost like making the plans for exploding the earth - to make place for a Hyperspace bypass - publicly available in a basement room of an administration building on Alpha Centauri)...

Precedes and follows is another knot of the brain convolutions with which we as Betelgeuzians find really hard to deal with (just used by higher Thetan levels than maybe 11 or even not until 351). If we want to start or stop anything, we - primitive enough - (please do not laugh now!) - just use "start" and "stop".

So before trying your benevolent code proposal I will try Slartibartfass' hint and I will report more then.

Therefore, many thanks from the heart for your efforts,
kind regards
Ford Doc.

[HaWe]

now the report as promised:
heureka!
using OUT_A/B/C as constants it's working simultaneously :)

But -
unfortunately the positioning error is only a little better than formerly with my own simple functions:
Rotate*PID: +/- 14-18 degrees (mostly)
myFunction +/- 20-25 degrees (mostly)

is it possible to achieve an even more exact positioning, e.g. using other P,I,D constants (currently 20,40,100) ?
has anyone got empirical knowledge?

edit: 40,40,90 seem to work better...

[HaWe]

there seems to exist another malfunction in the way RotateMotorPID works (except the accuracy):

when the motor once was overshooting the target, and I have been correcting the targetting error manually by turning the motor backwards, then the following targets seem to be overshooten even much more - as if there was sort of "cumulating error memory"....

(the encoder values itself seem to be shown complete correctly though).

very strange, and very annoying!

[afanofosc]

One thing to keep in mind wrt PID constants is that you really should use the #define constants from NBCCommon.h.

http://bricxcc.sourceforge.net/nbc/nxcd ... tants.html

There are only 8 valid values for PID. Any value you pass into Output module via its P, I, and D fields will be divided by 32 and truncated. So use PID_0 through PID_7 and you'll be good to go.

I am not sure exactly what is happening in the firmware when you manually adjust your motor position. Which counters are you displaying on the LCD? Perhaps it's internal MotorTachoCount thought it was exactly where it should be and then when you manually adjusted it, it thought it needed to rotate more (or less) so the subsequent deviation was more than you anticipated. In any case, you should show the internal MotorTachoCount on the screen if you aren't already since that is what the firmware uses when it is regulating the motor.

John Hansen

[HaWe]

I am displaying MotorRotationCount,
I have been resetting MotorRotationCount to 0 before,
and the relative targets to go are
(MotorRotationCount(port)-NextTarget )
the shown values always are very exact, but e.g. it approximates
1) target=100 => overshooting to 140 => manually turned back to 100
2) target=200 => overshooting to 300 => manually turned back to 200
3) target=300=> overshooting to 700 => manually turned back to 300
a.s.o. (happening when running the calibration routine)

the positions which actually have been reached are exactly the values of RotationCount, only the approximation itself is sometimes poor (>25)
(...and poorer and poorer if intermediately corrected manually).
At other times, the approximation is quite good (error < 5).

pid=40,40,90 were values I have been reading somewhere in the www missing any hints about it anywhere else.
what constants do you recommend for p,i,d?
32,32,128 ? or
32,32,96 ? or
48,48,128 ?

(I don't like #defined values if I can't clearly see what's behind them)

[afanofosc]

As I mentioned, since the firmware is using a different counter than you are using/displaying it might be worth testing the behavior when you use/display MotorTachoCount instead of MotorRotationCount since you are asking the firmware to regulate the motor for you. I have no specific PID recommendations other than to use multiples of 32 rather than any old number that you choose. The PID_0..PID_7 constants give you all the available multiples of 32 between 0 and 255.

John Hansen