Saturday, 23 November 2013

Driving Multiple Motors

I've been working on a slightly more advanced test program capable of driving multiple stepper motors simultaneously...

Of course, they don't really run at the same time - that's not possible with this interface, but with some clever programming we can make them step, so that they appear to be moving at the same time.

This is desirable for speeding up operations by running in parallel, instead of sequentially driving motors.

Another important reason is controlling the Gripper action - in order to Roll (rotation) or Pitch (up/down) the wrist mechanism - we need to carefully apply counter-rotations to the left/right hand side motors to drive the differential gearing.

In my previous test program we stepped a motor by calling the drive_motor() function.  This was a blocking function, it would not return until all the steps have completed.   Instead of this, I've written a new routine called drive_all_motors() which takes an array of motor commands.  You can still pulse just one motor with this arrangement, or pulse multiple motors as required.

The motor control array is a relatively simple structure consisting of the desired number of steps and direction  for each of the six motors:

 // define motor control structure  
 typedef struct MTR_CTRL_TAG  
 {  
   int  steps;  
   int  dir;  
 } MTR_CTRL;  
   
 // declare our motor control array  
 MTR_CTRL motor_control[6];  

A motor which is not to be driven should have steps = 0
Assigning a positive number will result in that motor being pulsed to the specified number of steps

The implementation of drive_all_motors() takes this array, and calculates the total number of iterations needed to drive the motors.  After this, we form a tight loop and pulse each of the running motors, decrementing the number of remaining steps as we proceed:

 void drive_all_motors()  
 {  
   // calculate maximum number of steps (total iterations regardless of direction)  
   // for all motors  
   int max_steps = 0;  
   int motor;  
   for (motor = 0; motor < 6; motor++)  
   {  
     if ( motor_control[ motor ].steps > max_steps )  
     {  
       max_steps = motor_control[ motor ].steps;  
     }  
   }  
   
   // repeat until all possible steps have completed  
   int step, output = 0, control = CCLK + SYNC;  
   for (step = 0; step <= max_steps * 2; step++)  
   {  
     // for each active motor, drive it!  
     for (motor = 0; motor < 6; motor++)  
     {  
       // motor is active if we have any steps remaining  
       if ( motor_control[ motor ].steps > 0 )  
       {  
         // add motor address to control pattern  
         output = control + mtr_addr[ motor ];  
   
         // now, add direction bit if necessary  
         if ( motor_control[ motor ].dir == 1 )  
         {  
           output += CDIR;  
         }  
   
         // output control byte, and delay  
         digitalWriteByte( output );  
         digitalWriteByte( output - SYNC );  
         delay( PULSE_DELAY );  
   
         // output again with sync bit restored - returning to input mode  
         digitalWriteByte( output );  
         delay( PULSE_DELAY );  
   
         // decrement motor step counter (when clock pulse is low)  
         if ( (control & CCLK) == 0 )  
         {  
           motor_control[ motor ].steps--;  
         }  
        }  
      }  
   
     // toggle clock-bit for next clock pulse  
     control = control ^ CCLK;  
   }  
 }  

The delay statements results in very slow running speeds when driven from a Raspberry Pi.  This is intentional as I'm still debugging my circuits, but we'll speed things up later.  We'll also consider speed along with acceleration/deceleration control in later versions of our software.

I'm going to start work writing a variation of the LEARN program documented in the manual allowing users to program sequences of movements & play them back.  This program will use the above technique as the bases for this work.  I also want to start abstracting the implementation details to make things easier to modify for direct-drive variants, and porting to other platforms.


The full source code for ArmTest_v2.c has been added to the software page.