Compliant Motion Control on a DC Motor



If you just want the code you can find it on github.


Motivation

Creating a compliant DC motor or compliant motion controller could mean many things depending on what the application scenario is.  In this case I would like an electric motor driven mechanism to resist having it's position disturbed but also to accept a new position if it is pushed and held in place (i.e. over-riding the holding force of the motor).  I'd also like to be able to set the resistance digitally (from code) so that at different times the motor has different strengths of resistance.  Finally I'd also like to be able to digitally command (from code) a new position, despite what is happening through physical interaction, and whether or not the motor moves depends on the strength setting of the motor.  

These things together might seem like a bit of a contradiction.  However imagine a mechanism that has to move through many set positions as part of an animation sequence.  This mechanism will also be exposed to the public, so you might want to have the mechanism soft enough to prevent a finger being trapped, or you might want the mechanism to be pushed and pulled around interactively without causing any damage to the equipment.  This kind of physical interaction could also be useful for a more complex robot experience with people.  In fact, this kind of compliant motion control is the future for safe human-robot interaction.  

For instance, Rusty Squid produced a swarm of books which responded to the visual detection of movement in an audience.  These books were autonomous and exposed to a public, but in reality they could not be physically interacted with.  The motors used in the installation were high grade hobby RC servo motors, which include reduction gear-sets that do not like to be back-driven (it can entirely break the motor unit).  Check out this video of the Book Hive:



In this post I document my first attempt at creating a low-cost compliant motion controller.  I've made it as simple as possible and as well commented as possible.


Ingredients

To create such a compliant motor and motion control we need the following ingredients:

  • An Arduino or similar to read sensors and send movement commands.
  • A strong DC motor with no gearing, I'm using a 12V motor with 4.7 oz-in torque
  • A position encoder, the motor I am using has an integrated optical encoder with 1000 clicks per revolution.  The code is written for a quadrature encoder.  This one is good.
  • A DC motor controller, I'm using a DRV8801 from Polulu.
  • A potentiometer to provide some external input to the system.
  • A 12V power supply for your motor, I'm using a lab power pack with 2 amps of current available.
  • A computer to do some coding.
When you add it all together it looks a bit like this:



The Circuit 

The circuit is very simple.  You'll want to connect the DRV8801 as it is specified on this webpage:



In my setup I've connected DIR to pin 7 and PWM to pin 6.  I've also connected a potentiometer across Analog 14, Ground and Pin 53 (set high).  If you want to use the example code without modification you'll have to do the same.  I've also wired the quadrature encoder data into pins 2 and 3 - you may need to reverse these or your motor polarity.


Overview of the Controller Operation


In the above diagram we have the following elements that work together in a constant loop of operation:
  • Motor position: This is read from the rotary encoder and should be quite a reliable indication of where the motor actually is in reality.
  • Desired position: This is a position set in software, which is relative to the units read from the encoder.  
  • PID Algorithm: This is algorithm takes both the Motor Position and Desired Position and creates an error signal, which describes whether the motor should rotate clockwise or anti-clockwise and with how much magnitude in order to gain the desired position.
  • Error Signal: As above,  but note that the same value is fed into the Position Controller and the Backdrive Monitor.
  • Position Controller: This element takes the Error Signal and decides how to operate the motor.  There are two important characteristics.  First the direction, set through Pin 7, and this corresponds to whether the Error Signal is positive or negative.  The DRV8801 needs a high or low signal to determine direction,  .  Second, the power of the response, which is taken as the magnitude of the error and sent out of Pin 6 as a PWM signal. Using these two things combined, the motor should move toward the Desired Position if it is away from it.  When the Error Signal is zero, the PWM signal will also be zero, and so there should be no movement from the motor.
  • Backdrive Monitor: This is the crucial element for providing the motion compliance.  Without it we have just a standard motor controller that will fight to keep the desired position.  The Backdrive Monitor therefore watches the Error Signal for the sign that the motor is being held against moving toward the desired position.  If the motor is being held away from the desired position, then the Backdrive Monitor adjusts the Desired Position to reflect this.  This has the knock-on effect of changing the output of the PID Algorithm - the Error Signal - which changes the Position Controller and also itself, the Backdrive Monitor.  The result is that if the motor is held away from the Desired Position and is held steady, the resistance will be slowly reduced and the motor will 'relax' into it's new physically set position.
  • Position Sequence Controller: This last component simply sets a new desired position, and in this example we will use a pentiometer to set this externally and test the performance.  In application, this might be a series of positions in an animation or a signal from a more complex behavioural controller.  In this example, the Position Sequence Controller is very simple, and could do with more advanced features (discussed later).

More on the Backdrive Monitor

The Backdrive monitor needs more explanation.  The Backdrive Monitor watches the Error Signal over a period of time for the sign that the motor is being held against moving toward the Desired Position.  

This is achieved by storing the Error Signal values across a period of time, and then looking at how much variation there is across the whole set.  If the Error Signal values are all different, then the motor is either moving by itself to a new position, or it is being moved (moving) externally.  If the error values are all the same, then we can assume that the motor has been moved to a new position and is being held there, or it has met an obstruction and can't move.  The desired result is that if the motor is held away from the Desired Position and is held steady, the resistance of the motor will be gradually reduced and the motor will 'relax' into it's new physically set position.  If the microcontroller is fast enough, this realisation of movement can happen very fast, and so the compliance of the motion can feel very natural (not stuttered) whilst simultaneously offering some resistance.  

This observation of variance in the Error Signal is nicely achieved with Standard Deviation.  Standard Deviation gives a value from 0 upward to quantify the variance in a set of numbers.  Therefore, Even if the Error Signal values from the PID algorithm are arbitrary non-zero values and signed  (e.g. +22, or -31), if the values are consistent the standard deviation will be closer to 0.  This consistent measure of variance from 0 upward is useful.  It will allow us to observe the characteristic of  change over time.

Using Standard Deviation we can decide the likelihood of the motor being held, and also use it to ignore any jitter in the motor movement which will also cause the encoder value to jump around.  Jitter would occur if the motor is trying to move but not succeeding, and it makes it hard to tell if the motor is being held still.  Jitter can be removed by thresholding the returned Standard Deviation (SD) value.  That means we set a value to check the SD against, and use this to decide the correct course of action.  

For instance, if we passed our compliance check only on a SD value of 0, the motor would need to be held exactly in the same position for a period of time (i.e., stored Error Signal values all identical).  If we passed on an SD value of 1 or less, we could ignore a small amount of movement (less than 1).  If we passed on an SD value of 10 or less, the motor can be held quite roughly in the same position, and the compliance will kick in.  Of course, these values are dependent on what types of values your encoder returns and any jitter from your motor fighting against the held position.  This motor jitter is partly defined by the PID algorithm and the position controller.

You can probably see that through all of this there are quite a few variables that will need to be tuned to your specific system and applications.  You might be asking, what units are these values all in?  Quite bluntly, most of it is arbitrary.   The encoder count can be converted into an angle, but it is not necessarily useful to do so, and it will consume computing time.  Any definition of angle will be chewed up by the PID algorithm and converted into an Error Signal that cannot be directly sent to he motor controller.  So in reality, a PID system will always need to be tuned, and this is best done with a physical system and through iterative testing. Units of measurement are not especially useful in a system like this.  Hopefully you will discover that tuning the system is not too tricky, and it is actually quite fun to see how it responds and what the limits are.

Code



I've put loads of comments in code to try to make it as understandable as possible.  You can also find this on github.


// Arduino Pin definitions
// Check your wiring.
#define DRV_DIR_PIN 7
#define DRV_PWM_PIN 6
#define POT_IN_PIN     A14
#define POT_5V_PIN     53

// Encoder Variables.  Note volatile, an interupt changes them.
static int pinA = 2; // Our first hardware interrupt pin is digital pin 2
static int pinB = 3; // Our second hardware interrupt pin is digital pin 3
volatile byte aFlag = 0; // let's us know when we're expecting a rising edge on pinA to signal that the encoder has arrived at a detent
volatile byte bFlag = 0; // let's us know when we're expecting a rising edge on pinB to signal that the encoder has arrived at a detent (opposite direction to when aFlag is set)
volatile int encoderPos = 0; //this variable stores our current value of encoder position. Change to int or uin16_t instead of byte if you want to record a larger range than 0-255
volatile int oldEncPos = 0; //stores the last encoder position value so we can compare to the current reading and see if it has changed (so we know when to print to the serial monitor)
volatile byte reading = 0; //somewhere to store the direct values we read from our interrupt pins before checking to see if we have moved a whole detent
volatile int count;

// PID variables.
// Lots of resource for PID algorithms online
// http://www.csimn.com/CSI_pages/PIDforDummies.html
// http://innovativecontrols.com/blog/basics-tuning-pid-loops
long lastPIDTime;
float pid_setpoint = 0;
float errSum = 0;
float lastErr = 0;
float kp = 0.1;
float ki = 00.00;
float kd = 10;
float error = 0;


// The setpoint from the sequence monitor
// in this example set from a potentiometer.
float sequence_setpoint = 0;

// Window Average variables
#define SAMPLE_SIZE 20
int sampleIndex;
float samples[SAMPLE_SIZE];

// Used to intermittently calculate
// standard deviation and check if
// the motor is being held in a new
// position.
long heldUpdateTime;

// Being held time threshold value (ms)
#define HELD_TIME_THRESHOLD 25

// Standard Deviation threshold 
// value
#define SD_THRESHOLD 0.2

// Easing for Backdrive position adjustment
// Larger values, bigger jump / faster response
// when the motor is held in a position.
#define BACKDRIVE_EASING 0.3

void setup() {
    // Rotary Encoder Setup
  pinMode(pinA, INPUT); // set pinA as an input, pulled HIGH to the logic voltage (5V or 3.3V for most cases)
  pinMode(pinB, INPUT); // set pinB as an input, pulled HIGH to the logic voltage (5V or 3.3V for most cases)
  attachInterrupt(0,PinA,RISING); // set an interrupt on PinA, looking for a rising edge signal and executing the "PinA" Interrupt Service Routine (below)
  attachInterrupt(1,PinB,RISING); // set an interrupt on PinB, looking for a rising edge signal and executing the "PinB" Interrupt Service Routine (below)

  // These are the pins for the DC motor controller
  // At this point DIR is arbitary but we will set it
  // to a known value anyway.
  pinMode( DRV_PWM_PIN, OUTPUT );
  pinMode( DRV_DIR_PIN, OUTPUT );
  digitalWrite(DRV_DIR_PIN,HIGH);

  // Enable pins to attach a potentiometer,
  // we'll use this to set a target position
  // as if it were part of a movement instruction.
  pinMode( POT_5V_PIN, OUTPUT );
  digitalWrite( POT_5V_PIN, HIGH );
  pinMode( POT_IN_PIN, INPUT );

  // Initialise Windowed Average
  initSamples();
  
  // Time flow variables.
  // Take an initial time stamp.
  heldUpdateTime = millis();
  lastPIDTime = micros();

  // Show a reset so we know when something has gone wrong.
  Serial.begin(115200);
  Serial.println("*** R E S E T ***");
  delay(500);
}



void loop() {

   // Functions as per elements explained in
   // blog post.  See functions for comments.
   pidAlgorithm();
   sequenceController();
   backdriveMonitor();
   positionController();
}



void pidAlgorithm() {
  // Update our PID aglorithm
  // The algorithm takes care of time interval
  error = getPID( encoderPos );
}

void positionController() {
  // We have to tell the motor which way to rotate
  // based on the error value.  
  // You might need to switch this depending on how
  // you wire your DC motor.
  if( error < 0 ) {
    digitalWrite( DRV_DIR_PIN, HIGH );
  } else {
    digitalWrite( DRV_DIR_PIN, LOW );
  }

  // Set power based on error.
  // We use abs() because we only want the magnitude
  // of the error not the sign (+/-).
  // Because the error is set by the PID algorithm,
  // power does not map linearly to 0:255 range.
  // We cap it at 255 as that is the maximum value for
  // analogWrite.
  // Therefore, to get a different resistance from the 
  // motor you should to tweak the kp,ki and kd variables.
  // To compensate for the weight of an object, or gravity,
  // you'd probably need to bias power depending on direction
  // etc.
  float power = abs(error);
  power = constrain( power, 0, 255);
  analogWrite(DRV_PWM_PIN, power );
}

void backdriveMonitor() {
  // Every loop we update our windowed average
  // encoderPos is global and altered via interupt.
  addSample(encoderPos);

  // We periodically check to see if the error position
  // is actually being held in a stable position.  This
  // is periodic because calculating a standard deviation
  // is expensive.  Doing this routine more often will
  // make the compliance more responsive.
  // Scenario, someone holding the mechanism in a fixed
  // new position.  In which case, the standard deviation of
  // the error over a period time will be close to 0, even
  // though the error value itself may be non-zero.
  // When this occurs, we slowly alter the pid_setpoint to
  // match the new position. 
  // We do this slowly because otherwise the setpoint jumps
  // and this can be felt in the mechanism, it feels a bit
  // like a stepper motor.  Slowly adjusting the position
  // makes a smooth transition.
  // There are a couple of variables here worth playing with:
  //   updateTime > 25(?)
  //   sd < 2(?)
  //   error * -0.3(?)
  if( millis() - heldUpdateTime > HELD_TIME_THRESHOLD ) {
    heldUpdateTime = millis();

    // Grab SD value, decide if the motor is
    // being held by comparing against a 
    // fixed value.
    float sd = getStdDev();
    if( sd < SD_THRESHOLD ) {

      // Note, -= because we need to invert the error
      pid_setpoint -= (error * (BACKDRIVE_EASING) );
    }
  }
  
}


// Because we don't actually have an animation sequence
// we instead use an external POT to simulate a moving
// target.  This code therefore watches for a change of
// value on the POT to set a new sequence position.
void sequenceController() {
  
  // Read in a target position set by the potentiometer.
  // We are going to use this to simulate a movement 
  // routine.
  // The pot only updates the pid setpoint if it is
  // moved to a new position.
  // Otherwise the setpoint is managed by backdriving
  // the motor.
  // We threshold the target value to ignore jitter
  // from the pot reading.
  float target = analogRead( POT_IN_PIN );

  if( abs(target - sequence_setpoint) > 5 ) {
    pid_setpoint = target;
    sequence_setpoint = target;
  }
}


// Very simple PID routine.
// Registers the time it was called.
// Would be better to run it from an
// interupt timer.
// Set global kp,ki and kd variables.
// Tune these based on observed performance.
float getPID( float input ) {
  long now = micros();
  long timeChange = now - lastPIDTime;

  float error = pid_setpoint - input;
  
  errSum += error * timeChange;

  float dErr = 0;
  if( timeChange != 0 ){
   dErr = ( error - lastErr ) / timeChange;
  }

  float output = (kp * error);// + (ki * errSum ) + (kd * dErr );

  lastErr = error;
  lastPIDTime = now;

  return output;
}


// Ensure the buffer (storage) of values is
// initially set to 0
void initSamples() {
  for( int i = 0; i < SAMPLE_SIZE; i++ ) {
    samples[i] = 0;
  }
  sampleIndex = 0;
}

// Maintains a buffer (storage) of values from
// which to draw an average and standard deviation
// This function handles the wrap around on the 
// index value.
void addSample( float s ) {
  samples[sampleIndex] = s;
  if( sampleIndex < SAMPLE_SIZE ) {
    sampleIndex++;
  } else { sampleIndex = 0;}
}

// Calculates the mean (average) value
// from the current values in the buffer
float getMean(){
  int i;
  float ave = 0;
  for( i = 0; i < SAMPLE_SIZE;i++ ) {
    ave += samples[i];
  }
  return (ave/SAMPLE_SIZE);
}


// Calculates the Standard Deviation
// from the current values in the buffer
float getStdDev() {
  float mean = getMean();
  float sqDevSum = 0.0;
  for( int i = 0; i < SAMPLE_SIZE; i++ ) sqDevSum += ( pow( mean - samples[i], 2 ));
  if( sqDevSum == 0 ) return 0;
  return sqrt( sqDevSum / SAMPLE_SIZE );
}



// Interupt based encoder update.
// http://www.instructables.com/id/Improved-Arduino-Rotary-Encoder-Reading/?ALLSTEPS
// https://www.arduino.cc/en/uploads/Main/arduino-mega-schematic.pdf
// https://public.dm2301.livefilestore.com/y3pNzgDyTlsb1vvDEd6k9t_EF8JmGTnyBVKkhPDu8dlN07Lv27EpwtC4ps231SDiQ35ldjw-espWhy7earTCTnIXnTtJH30zLUrNkx4B_ArCpu7YjZrIwk0TSCbwD_g5y5L/PIN%20MAPPING%20ARDUINO%20MEGA.jpg?rdrts=138105891
// 
// Note(!): 
// Because I am using an arduino mega I have altered the example
// to use the PORT definition and bit operation to PINE & B00110000
// You'll need to adjust this depending on your arduino device.
void PinA(){
 
  cli(); //stop interrupts happening before we read pin values
  reading = PINE & B00110000; // read all eight pin values then strip away all but pinA and pinB's values
  
  if(reading == B00110000 && aFlag) {
    encoderPos--; //decrement the encoder's position count
    bFlag = 0; //reset flags for the next turn
    aFlag = 0; //reset flags for the next turn
    
  } else if ( reading == B00010000 ) bFlag = 1; //signal that we're expecting pinB to signal the transition to detent from free rotation
  sei(); //restart interrupts
}

void PinB(){
  
  cli(); //stop interrupts happening before we read pin values
  reading = PINE & B00110000;
  //readingA = digitalRead(pinA); // read all eight pin values then strip away all but pinA and pinB's values
  //readingB = digitalRead(pinB); //read all eight pin values then strip away all but pinA and pinB's values
  if(reading == B00110000 && bFlag) {
    encoderPos++; //increment the encoder's position count
    bFlag = 0; //reset flags for the next turn
    aFlag = 0; //reset flags for the next turn

  } else if ( reading == B00100000) aFlag = 1; //signal that we're expecting pinA to signal the transition to detent from free rotation
  sei(); //restart interrupts

}

Improvements 

 There is plenty of room for improvement in this code, such as:

  •  The PID routine would be better if it ran reliably on a timer interrupt routine. 
  • The backdrive monitor currently adjusts the Desired Position (pid_setpoint) using the easing technique. This could itself be replaced with a PID algorithm to provide a more resilient or characterful response to the motor being backdriven. 
  • The Position Controller simply maps the PID error signal to the range 0:255. In order to compensate for gravity or an unbalanced load, this mapping would need to be biased depending on the direction (sign) of the error. 
  •  In this example, the motor power is mapped from the error signal, which means that the resistance the motor offers increases as the motor is moved away from the setpoint (desired position). To improve this, the DRV8801 provides an analogue output signal that indicates the current draw of the motor. To provide a constant resistance, the output power could be limited by a reading the current consumption of the motor.

Popular Posts