Calling Motor Shield V2 Library Function in Timer Interrupt?

Hi everyone,

I need to implement a close loop control (PID Lib) for DC Motor.

I am using the Adafruit Motorshield V2 and the corresponding Library on Arduino Mega.

My main loop has huge jitter because of communication and serial printouts. Therefore I want to run the close loop completely inside timer interrupt function.

My sensor input and PID control works well when calling from timer interrupt, but when I call Motor shield function ( e.g. setSpeed(...) ), my application runs into deadlock.

Any solution?

Many Thanks
Richard

Without seeing your program I can't visualise the problem.

My first thought is that it is unlikely to be wise to call any function from an ISR unless it is guaranteed to complete within a few microsecs. And, of course, the code in the function must work while interrupts are paused.

I suspect the real solution is to re-write (or remove) the code that is causing the jitter.

...R

I cannot remove all this code in the main loop().
At least I would need an serial UI for user input, outputs and some logging.
Serial communication is slow and is causing huge jitter.

a closed loop PID controller needs to be called equidistantly in order to have good results. so actually I think it is a good idea to call it from interrupt.
maybe there is a problem with the I2C-communication, when called from interrupt

NBGer:
I cannot remove all this code in the main loop().

There is nothing to be gained by by commenting on this until you post the complete program so we can see what your are referring to.

...R

the code till now:
the problem is caused in the last 3 lines of code

#include <PID_v1.h>
// Includes Adafruit Motor Shield V2 
#include <Wire.h>
#include <Adafruit_MotorShield.h>

//-----------------------------------------------------------------------
// Arduino Shield Specific

// Objects Motor Shield and Motors
// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS_1 = Adafruit_MotorShield(0x60); 

Adafruit_DCMotor *Motor1 = AFMS_1.getMotor(1);

//-----------------------------------------------------------------------


///Define Variables we'll be connecting to
double kp = 5 , ki = 1 , kd = 0.01;             // modify for optimal performance
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,kp,ki,kd, DIRECT);

// Set to true to get debug output.
boolean verbose = false;

int ActSensValue = 0;
int PrevSensValue = 0;
int DirRotation = 0;
int SensDiff = 0;
const int sensorPin = A0;

unsigned long ActTime;
unsigned long PrevTime;
unsigned long TimeDiff;

signed long UMinSet;
signed long UMinAct;
signed int motPWM = 0;

const int SampleTime = 20;


#define SZE_LOG 256
typedef struct 
{
  unsigned long TDiff;
  int           SensValue;
  int           SensDiff;
  signed long   UMin;
  
} log_t;
int iLog = SZE_LOG;
bool LogReady = false;

log_t aLog[SZE_LOG];

void setup()
{
  // serial
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("DC Motor Closed Loop Test");
  
  //initialize the variables we're linked to
  Input = analogRead(sensorPin);
  Setpoint = 0;

  // Init Motor Shield
  AFMS_1.begin();  // create with the default frequency 1.6KHz
  
  Motor1->setSpeed(0);
  Motor1->run(RELEASE);


  ActSensValue = PrevSensValue = analogRead(sensorPin);
  //turn the PID on
  myPID.SetOutputLimits(-255, 255);
  myPID.SetSampleTime(50);
  myPID.SetMode(AUTOMATIC);

  timer_setup(SampleTime);
  
}

int SensorIn()
{
  ActTime = millis();
  TimeDiff = ActTime - PrevTime;
  
  ActSensValue = analogRead(sensorPin);
  if ( ActSensValue > PrevSensValue )
  {
    DirRotation = 1;  // clockwise
    SensDiff = ActSensValue - PrevSensValue;

     if ( (ActSensValue > 750) && (PrevSensValue < 250) )
     {
        DirRotation = -1;  // clockwise
        SensDiff = -(PrevSensValue + (1024 - ActSensValue));
     }
  
  }
  else
  {
     if ( (ActSensValue < 250) && (PrevSensValue > 750) )
     {
        DirRotation = 1;  // clockwise
        SensDiff = ActSensValue + (1024 - PrevSensValue);
     }
     else
     {
        DirRotation = -1;  // clockwise
        SensDiff = -(PrevSensValue - ActSensValue);
     }
  }

  UMinAct = (signed long)SensDiff * 1000 * 60 / (signed long)TimeDiff / 1024;

  if ( iLog < SZE_LOG)
  {
    aLog[iLog].TDiff = TimeDiff;
    aLog[iLog].SensValue = ActSensValue;
    aLog[iLog].SensDiff = SensDiff;
    aLog[iLog].UMin = UMinAct;
    ++iLog;
    if ( iLog == SZE_LOG )
    {
      LogReady = true;
    }
  }

  PrevTime = ActTime;
  PrevSensValue = ActSensValue;

}
void loop()
{
  SerialIn();

  if ( verbose )
  {
      Serial.print( " SensPrev: " );
      Serial.print(PrevSensValue , DEC);
      Serial.print( " SensAct: " );
      Serial.print(ActSensValue , DEC);
      Serial.print( " dT: " );
      Serial.print(TimeDiff , DEC);
      Serial.print( " dSens: " );
      Serial.print(SensDiff , DEC);
      Serial.print( " UMin: " );
      Serial.println(UMinAct , DEC);
  
  }

  //Input = analogRead(0);
  //myPID.Compute();
  //analogWrite(3,Output);

  int dir = (motPWM >= 0)?1:-1;
  Motor1->run((dir == 1)?FORWARD:BACKWARD);
  Motor1->setSpeed(abs(motPWM));


  // print Log

  if ( LogReady )
  {
    for ( int i = 0 ; i < SZE_LOG ; ++i )
    {
        Serial.print( " TDiff: " );
        Serial.print(aLog[i].TDiff , DEC);
        Serial.print( " SensValue: " );
        Serial.print(aLog[i].SensValue , DEC);
        Serial.print( " dSens: " );
        Serial.print(aLog[i].SensDiff , DEC);
        Serial.print( " UMin: " );
        Serial.println(aLog[i].UMin , DEC);
    
      
    }
    LogReady = false;
  }  
}

//===============================================================================
#define LINE_BUFFER_LENGTH 100

void SerialIn(void)
{
  static char line[ LINE_BUFFER_LENGTH ];
  static int lineIndex = 0;
  static bool lineIsComment, lineSemiColon;
  char c;

  lineSemiColon = false;
  lineIsComment = false;
  
       // Serial reception - Mostly from Grbl, added semicolon support
    while ( Serial.available()>0 ) 
    {
      //Serial.println( "getc" ); 
      c = Serial.read();
      if (( c == '\n') || (c == '\r') ) {             // End of line reached
        if ( lineIndex > 0 ) {                        // Line is complete. Then execute!
          line[ lineIndex ] = '\0';                   // Terminate string
          if (verbose) { 
            Serial.print( "Received : "); 
            Serial.println( line ); 
          }
          processIncomingLine( line, lineIndex );
          lineIndex = 0;
        } 
        else { 
          // Empty or comment line. Skip block.
        }
        lineIsComment = false;
        lineSemiColon = false;
        //Serial.println("ok");    
      } 
      else {
        if ( (lineIsComment) || (lineSemiColon) ) {   // Throw away all comment characters
          if ( c == ')' )  lineIsComment = false;     // End of comment. Resume line.
        } 
        else {
          if ( c <= ' ' ) {                           // Throw away whitepace and control characters
          } 
          else if ( c == '/' ) {                    // Block delete not supported. Ignore character.
          } 
          else if ( c == '(' ) {                    // Enable comments flag and ignore all characters until ')' or EOL.
            lineIsComment = true;
          } 
          else if ( c == ';' ) {
            lineSemiColon = true;
          } 
          else if ( lineIndex >= LINE_BUFFER_LENGTH-1 ) 
          {
            //Serial.println( "ERROR - lineBuffer overflow" );
            lineIsComment = false;
            lineSemiColon = false;
          } 
          else if ( c >= 'a' && c <= 'z' ) {        // Upcase lowercase
            line[ lineIndex++ ] = c-'a'+'A';
          } 
          else {
            line[ lineIndex++ ] = c;
          }
        }
      }
    }
}

void processIncomingLine( char* line, int charNB ) 
{
  int currentIndex = 0;

  switch ( line[ currentIndex++ ] ) 
  {              // Select command, if any

    case'V':
      verbose = !verbose;
      break;
    case'+':
      UMinSet +=10;
      UMinSet = (UMinSet > 300 )?300:UMinSet;
      Serial.print( " Speed_Soll (U/Min): " );
      Serial.println(UMinSet , DEC);
      break;
    case'-':
      UMinSet -= 10;
      UMinSet = (UMinSet < -300 )?-300:UMinSet;
      Serial.print( " Speed_Soll (U/Min): " );
      Serial.println(UMinSet , DEC);
      break;
    case'0':
      UMinSet = 0;
      Serial.print( " Speed_Soll (U/Min): " );
      Serial.println(UMinSet , DEC);
      Motor1->setSpeed(0);
      Motor1->run(RELEASE);
      break;
    case'I':
      Serial.print( " Sensor Value: " );
      Serial.print(ActSensValue , DEC);
      Serial.print( " Diff: " );
      Serial.print(SensDiff , DEC);
      Serial.print( " UMin: " );
      Serial.println(UMinAct , DEC);
      break;
    case 'L':
      iLog = 0;
      break;
    default:
      Serial.println( "Eingabefehler" );
      break;
  }  

  return;
}

//====================================================================
// Timer for Motor control

#define PIN_LED_CYCLE 13

void timer_setup(int frequ)
{
// initialize timer1 in CTC mode

  pinMode (PIN_LED_CYCLE, OUTPUT);

  noInterrupts();           // disable all interrupts

  TCCR4A = 0;

  TCCR4B = 0;

  TCNT4  = 0;

  int compare_value = 62500 / frequ;
  OCR4A = compare_value;            // compare match register 16MHz/256/2Hz

  TCCR4B |= (1 << WGM12);   // CTC mode

  TCCR4B |= (1 << CS12);    // 256 prescaler 

  TIMSK4 |= (1 << OCIE1A);  // enable timer compare interrupt

  interrupts();             // enable all interrupts

}

ISR(TIMER4_COMPA_vect)          // timer compare interrupt service routine
{
  digitalWrite(PIN_LED_CYCLE, digitalRead(PIN_LED_CYCLE) ^ 1);   // toggle LED pin

  SensorIn();

  Setpoint = UMinSet;
  Input = UMinAct;
  myPID.Compute();
  motPWM = Output;

  // if this code is active, there is deadlock
  int dir = (motPWM >= 0)?1:-1;
  Motor1->run((dir == 1)?FORWARD:BACKWARD);
  Motor1->setSpeed(abs(motPWM));
  
}

well, then don't do it in the ISR unless you have read the sourcecode of setSpeed() and verfied it is safe to use in the ISR.

What is this line intended to do

Motor1->run((dir == 1)?FORWARD:BACKWARD);

You might be much better off using a specialised stepper motor driver and the AccelStepper library.

...R

Robin2:
What is this line intended to do

Motor1->run((dir == 1)?FORWARD:BACKWARD);

You might be much better off using a specialised stepper motor driver and the AccelStepper library.

...R

I am using DC Motor, not stepper motor. So the library function is expecting PWM (0..255) and direction (clockwise or counter clockwise)

in general, it seems not to be very productive to analyse source code of library functions to make them callable from Interrupt routine.

Therefore I have changed my architecture a bit. I am using now FreeRTOS library, created a high priority task for the motor control and wakeup the task cyclically and equidistant from timer interrupt.

... and it works quite good ! I can measure a jitter of task about +/-1ms with task cycle time 20ms
while I am doing serial communication in main loop function

for me on my desk the issue is solved!

NBGer:
I am using DC Motor, not stepper motor.

Apologies. I don't where I got that idea - senior moment :slight_smile:

It's interesting that the overhead of FreeRTOS can be accommodated - it seems to me you have solved a performance problem by adding a lot of extra code. IMHO it should be possible to achieve the same result without FreeRTOS and with a lot less code - but glad to hear you have a solution.

...R

Robin2:
It's interesting that the overhead of FreeRTOS can be accommodated - it seems to me you have solved a performance problem by adding a lot of extra code. IMHO it should be possible to achieve the same result without FreeRTOS and with a lot less code - but glad to hear you have a solution.

...R

the key point is "preemptive Multitasking", so I am able to interrupt and delay the low priority things at any time. the "lot of extra code" of FreeRTOS is mostly not used right now, just the context switch needs a bit time (saving and restoring registers)