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.

Saturday 2 November 2013

Motor Addressing Solved

Finally, I have solved the motor selection problems.   This blog update will try and cover both problems in detail....


1. PCB interconnections (only applies to prototype models)

As suspected the PCB interconnects was crossed on my Armdroid causing motor selects to drive the wrong motor.   The cables feed the the bottom two data bits (CDIR & CCLK) from the interface to driver circuitry, and look like this:


They combine two channels into a single connector at the other end.


They need to be wired such that the outputs on the interface board below feed directly into the corresponding inputs on the driver circuit board above in the same sequence.   As pictured above, the ordering is - starting from the left-hand side, motor #1 - motor #6


The colour coding of my cables are Purple, Blue, White, and Grey

The labels on all my connectors will be replaced in time to correctly reflect the motor channels they represent.


2. Motor Addressing Logic

The ordering of the Motor Address Bits was not as documented

The give away is the 74LS138 IC6
If you look closely at the pin-out for the 74LS138, you'll see that pins 1 - 3 are the input select bits, illustrated as follows:
The Truth Table says, to get output Y1 selected (ignoring the reverse logic), where Y1 happens to be our motor select for channel #1, you need to set the input pins as follows:

C B A
L L H

L = LOGIC LOW,  H = LOGIC HIGH


Looking at the interface schematics :



You can see address bits D2 - D4 are wired to the 74LS138 as follows:

D4   -->   A (pin 1)
D3   -->   B (pin 2)
D2   -->   C (pin 3)

Which means to select motor #1 the address bit pattern would have to be:

D4   HIGH
D3   LOW 
D2   LOW 

The most significant address bit is actually D2, and least significant D4.

This is not quite what the article in the ETI magazine documented for the interface specification.   Previously, I was setting addresses with the assumption D4 was the most significant bit - for example 001 in binary, instead of 100 binary.

I revisited the construction manual looking for further clarification, unfortunately the documentation is somewhat ambiguous here.

Of course, none of this is actually a problem... we're dealing with an address pattern that is essentially reversible, so if your happy to re-wire your motor assignments, everything will then fall out in the wash.

I decided not to do this, and instead will compensate in software...
By introducing an array of motor addresses, we simply index into this array to fetch the desired motor address bit pattern:

 // motor address bits  
 // 1 0x08 = 00001000  
 // 2 0x04 = 00000100  
 // 3 0x0C = 00001100  
 // 4 0x02 = 00000010  
 // 5 0x0A = 00001010  
 // 6 0x06 = 00000110  
 //              |||  
 //              ||+-- A3  
 //              |+--- A2  
 //              +---- A1  
 const int mtr_addr[] = { 0x08, 0x04, 0x0C, 0x02, 0x0A, 0x06 };


When constructing our output control byte, the new code looks like :

   int output = CCLK + mtr_addr[ mtr-1 ] + SYNC;  

I no longer need to shift the address bits into position because they are now already defined in the correct position.   The mtr-1 of course is needed because all arrays in "C" have indexes starting from zero.

Actually, I prefer this approach because it allows other readers to easily adapt to their Armdroid's configuration.   This is essentially mapping logical motor numbers to physical motor addresses.  If a motor has been incorrectly wired to the wrong channel, you only need to modify this address array to compensate.   Good eh?


The updated source code for the test program will be added later this weekend to the software section.


I still have plenty of other problems to investigate.  Some of the motors, not all, are not reversing properly, yet everything works when operated by the manual controller.   I also rigged up a test circuit to check the signals coming from the feedback switches - and not convinced this is working properly either.    But, resolving the motor addressing is certainly one step in the right direction.....