controlling 2 motors independently

[groskiff1994]

Hello, and sorry for the mistakes I will do (I'm french).

MY LANGUAGE IS NXC

I try to control two motors independently in my program, I explain : my robot is the PnP from http://www.youtube.com/watch?v=-ZolqjEnhB4, and I have 1 motor for rotation along the main axis (left-right : OUT_A) and the other one to make the arm go up or down (OUT_B). But I don't manage to control these motors in the same time : I'd like to say "goTo(valdegreeforA, valdegreeforB)" and the robot would move both motors in the same time. But all I managed to get is one motor after the other one, or, by using OnFwd(motor, val), a low precision.

code with task :

Code: Select all

#define AXIS OUT_A
#define ARM OUT_B
#define espace -45
#define gauche -90
#define droite 90
#define haut 130
#define bas 0
#define degree1turn 20171
#define heighTomber 20

int stockGauche;
int degreeVal;
int goal;
bool axisMvt;
bool armMvt;
bool Mvt;


task orientation()
{
     axisMvt = true;
     
     //compute degree to turn to get the orientation degreeVal
     int degreMoteur = degreeVal*degree1turn/360 - MotorRotationCount(AXIS);
     RotateMotor(AXIS, 100, degreMoteur);
     axisMvt = false;
}

//move arm till a certain value in degree


task heigh()
{
     armMvt = true;
     
     //compute degree to turn to get the orientation goal
     int degreMoteur = goal - MotorRotationCount(BRAS);
     int strenghMotorB = 20;
     RotateMotor(ARM, forceMoteurB, degreMoteur);
     armMvt = false;
}

task mvtLook()
{
     while(true)
     {
                Mvt = (axisMvt||armMvt);
                Wait(100);
     }
}

void goTo(int axisVal, int armVal)
{
     until(Mvt == false);
     degreeVal = axisVal;
     goal = armVal;
     StartTask(heigh);
     StartTask(orientation);
}

task main()
{
     //INITIALISATION
     ResetTachoCount(AXIS);
     ResetTachoCount(BRAS);
     
     axisMvt = false;
     armMvt = false;
     
     StartTask(mvtLook);
     
     goTo(0, haut);
     goTo(espace, bas);

}

code with OnFwd and motorRotationCount :

Code: Select all

#define degA 1000
#define degB 1000

task main()
{
     ResetTachoCount(OUT_A);
     ResetTachoCount(OUT_B);
     
     int depart_A = MotorRotationCount(OUT_A);
     int depart_B = MotorRotationCount(OUT_B);
     
     int distA = 0;
     int distB = 0;
     
     OnFwd(OUT_A, 50);
     OnFwd(OUT_B, 50);
     
     bool reachA = false;
     bool reachB = false;
     
     do
     {
                  distA = MotorRotationCount(OUT_A) - depart_A;
                  distB = MotorRotationCount(OUT_B) - depart_B;
                  
                  reachA = (distA >= degA);
                  reachB = (distB >= degB);
                  
                  if (reachA)
                  {
                     Off(OUT_A);
                  }
                  
                  if (reachB)
                  {
                     Off(OUT_B);
                  }
                  
     } while (((reachA && reachB) == false));
     
     NumOut(0, LCD_LINE3, MotorRotationCount(OUT_A));
     NumOut(50, LCD_LINE3, MotorRotationCount(OUT_B));
     Wait(3000);
     
}

So, basically, I'd like to know how to be able to control 2 motors in 1 command, simultaneously.

Thank's you very much if you can help me !

[groskiff1994]

No answers ? Some guys are really skilled here, but they wouldn't know how to make 2 movements without waiting for the stop of the first ?

Please help me, give me advice, send me to tuto or even give me a link to another forum (if another exists, more frequented)

thank's

[HaWe]

I don't ever control all motors by 1 command...
I personally am going completely the other way:
I always start each motor command for each motor in 1 dedicated task (which is self-terminating if the goal is reached).
Each task then regulates each related motor target (or speed or what else) independently from all other ones.
And additionally such a task can be stopped at any time by a stop task command in any other thread (e.g., by a emergency stop task).

If I want to synchronize the movements, I calculate the difference of both motor counters and slow down the motion of the faster one (e.g., by reducing the power or setting the motors to "coast").

BTW: your video has been completely blocked (at least in Germany) because of copyright infringements of your background music - maybe you wish to use some "free" sound tracks?

[groskiff1994]

Thanks for showing me the way to do it, now I have a start : 1 task for each motor
Now I have to find a way to schedule actions in order not to have 2 commands for the same motor in the same time !

PS : the video is not mine, but you can find pictures here : http://us.mindstorms.lego.com/en-us/Com ... c0ed36210b

[HaWe]

define global variables ("semaphores")
char MotAready, MotBready, MotCready;

if a tasks shall take control of MotA (MotAControlTask) then you assign in your superior thread
MotAready=false;
and if the MotAControlTask has internally reached his goal then his last command line is: MotAready=true;
so all in all:

Code: Select all

char MotAready, MotBready, MotCready;

MotAControlTask1() {
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}

MotAControlTask2() {
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}
  
task main() {
  MotAready=false;
  start MotAControlTask1; // start 1st task
  while (!MotAready); // wait until 1st task has finished

  Wait(1); // give the time slice scheduler a chance to update all the motor and task states

  MotAready=false;
  start MotAControlTask2; // start 2nd task
  while (!MotAready); // wait until 2nd task has finished
  //...
  //...
}

If you wish you also may insert the MotAready=false cmd on to the first place of the internal task code; for safety, I would add also an additional 1ms wait state:

Code: Select all

MotAControlTask2() {
  MotAready=false;
  Wait(1);
  // do sth.
  // do sth else
  // do sth until end-condition
  MotAready=true;
}

get what I mean?

[groskiff1994]

I just realised that I didn't reply
This is exactly what I wanted to know, I will try to make it works soon !

for your help.