Issue with analogWrite within compare match interrupt

Created Mon, 17 Dec 2012 22:33:39 +0000 by RoboNegro

RoboNegro

Mon, 17 Dec 2012 22:33:39 +0000

Greetings fellow ChipKITTENS,

I am working on a project where I am using a ChipKIT Max32 and an ArduinoMEGA (connected via SPI, I configured the ChipKIT to be the master and the Arduino to be the slave) to control the movement of a motor. The Arduino generates a setpoint that I want the motor to go to. The Arduino generates a new setpoint at a rate of 50 times per second. The Arduino then sends the setpoint value to the ChipKIT, which reads the setpoint at 50 Hz.

I configured the ChipKIT, using a compare match interrupt, to execute its instructions at a rate of 2000 Hz. (This high frequency is needed for proper motor control, any slower and the force control law will not respond fast enough to respond to changes in the motor.) I made a loop, inside the compare match interrupt loop, that executes at a rate of 50 Hz. This inner loop reads the setpoint from the Arduino.

Everything works fine as long as I do NOT include the analogWrite(pwm_a,torque), once I include this the loop will not execute. I am confused because the analogWrite function only takes 2 microseconds to execute (wrote a simple code using micros() to calculate this). And the loop WITHOUT the analogWrite function takes 28 microseconds to execute. The instructions inside the ChipKIT compare match interrupt need to be executed in 500 microseconds (1/2000 = 0.0005 sec). So there should be more than enough time for the interrupt to perform its calculations.

Can anyone offer any insight into this issue? My code is listed below:

Code for master:

//The following code illustrates how to define initial OC1 pin state for the
//output compare toggle mode of operation in 32-bit mode

#include <plib.h>
#include <SPI.h>

int count = 0;
int curr_pos;
float CPG_setpoint;

int zero_pos = 551;
int max_pos, min_pos;
float CPG_ang, min_ang, max_ang, curr_ang;
float torque;

int pwm_a = 3;  // PWM control for motor outputs 1 and 2 is on digital pin 3
int dir_a = 12;  // dir control for motor outputs 1 and 2 is on digital pin 12

int pin = 49;
int error;

unsigned long starttime, endtime, time;

void setup()
{ 
  Serial.begin(9600);

  pinMode(dir_a, OUTPUT);
  
  // initialize SPI
  // takes care of pin assignment MOSI, MISO, SCK, and SS
  SPI.begin(); 
  // set SPI bit order to most significant bit first
  SPI.setBitOrder(MSBFIRST);
  
  // configure module for OC1 pin low, toggle high
  OC1CON = 0x0001;
  // enable OC1 module
  OC1CONSET = 0x8000;
  
  // configure timer2 for prescaler of 2
  T2CON = 0x0018;
  // turn off OC1 while doing setup
  OC1CON = 0x0000;
  // configure for compare toggle mode
  OC1CON = 0x0023;
  
  // initialize compare register 1
  // compare register and period must be equal
  // 1 Hz = 2625a00
  // 2000 Hz = 4e20
  OC1R = 0x4e20;
  // set period
  PR2 = 0x4e20;
  
  // configure interrupt
  
  // clear the OC1 interrupt flag
  IFS0CLR = 0x00000040;
  // enable OC1 interrupt
  IEC0SET =0x00000040;
  // set OC1 interrupt priority to 7, the highest level
  IPC1SET = 0x001C0000;
  
  // set subpriority to 3, maximum
  IPC1SET = 0x00030000;
  
  // enable timer2
  T2CONSET = 0x8000;
  // enable OC1
  OC1CONSET = 0x8000;
  
  // slight delay avoids false trigger at start.	 
  delay(1);  
}

void loop()
{
}

extern "C"
{
void __ISR(_OUTPUT_COMPARE_1_VECTOR,ipl7) OC1_IntHandler(void)	
  { 
    starttime = micros();	 
    // clear interrupt flag
    IFS0CLR = 0x0040;   
    
    // read current position
    curr_pos = analogRead(A0);
    // convert current position from encoder counts to radians
    curr_ang = (100.0)*(6.28318)*((curr_pos - zero_pos)/1024.0);
    
    // send current position to CPG at 50 Hz
    // receive CPG setpoint from ArduinoMEGA at 50 Hz
    if(count%40 == 0){
      // direct write slave select pin (pin 53) LOW to talk to slave
      LATGCLR = 1000000000;
      // send current position to slave and receive setpoint from slave
      // convert byte to char, without this conversion I am not able to receive 
      // negative values
      CPG_setpoint = (char)SPI.transfer(curr_pos);
      // direct write slave select pin (pin 53) HIGH 
      LATGSET = 1000000000;
      // convert char to float
      CPG_setpoint = (float)CPG_setpoint;     
    }   
    
    // calculate torque to be sent to motor
    torque = (CPG_setpoint - curr_ang);
    
    // set direction pin of h-bridge using direct writes of h-bridge direction pins
    // if torque > 0, motor spins CCW, if torque < 0 motor spins CW
    if (torque > 0)    
        {LATACLR = 100;}   
    if (torque < 0)
        {LATASET = 100;}
    
// round torque value because analogWrite only allows integers to be written to //it    
    torque = round(torque); 
    // take absolute value of torque because analogWrite does not allow negative //numbers
    torque = abs(torque);    
    
    // map torque value to PWM
    torque = map(torque,0,40,0,255);
    
    // apply PWM signal to h-bridge to drive motor
    //analogWrite(pwm_a,torque);
    
    ++count;
    endtime = micros();
    time = endtime - starttime;
    
    //Serial.println(torque);    
  }//ISR
}//extern "C"

Code for slave (ArduinoMEGA):

#include <stdio.h>
#include <avr/io.h>
#include <SPI.h>
// define the pin outs

int pwm_a = 3;  //PWM control for motor outputs 1 and 2 is on digital pin 3
int dir_a = 12;  //dir control for motor outputs 1 and 2 is on digital pin 12

float c = 1.0, tau1 = 0.10, tau2 = 2.0*tau1;
float CPG;
float CPG_pos;
float h;
float I1 = 1.0, I2 = 1.0, I3 = 1.0, I4 = 1.0;
float IC[] = {0.01,0.01,0.01,0}, K[] = {0,0,0,0};
float CPG_mapped;

byte CPG_byte;

int zero_pos = 551;
int max_pos, min_pos;
float CPG_ang, min_ang, max_ang;

int count = 0;

float B = 2.5, Y = 2.5, timestep  = 0.020, M = 0.5*timestep;

// SPI data recieved, current CPG position
float curr_pos; 

void setup()
{
  Serial.begin(9600);
  
  //send data on master in, slave out (MISO)
  pinMode(MISO,OUTPUT);
  pinMode(53,INPUT);
  
  Serial.println("Enabling SPI in slave mode");
  Serial.println("SPI Slave, at your service.");
  
  // enable SPI in slave mode
  SPCR = 0x40; 
  SPI.setBitOrder(MSBFIRST);
  
  // initialize Timer1
  cli();          // disable global interrupts
  TCCR5A = 0;     // set entire TCCR1A register to 0
  TCCR5B = 0;     // same for TCCR1B
 
  // set compare match register to desired timer count:
  //313 = 50 Hz
  //156 = 100 Hz
  //80 = 200 Hz
  OCR5A = 313;
  
  // turn on CTC mode:
  TCCR5B |= (1 << WGM52);
  // Set CS50, CS51, CS52 bits for prescaler value: 1024
  TCCR5B |= (1 << CS50);
  TCCR5B |= (0 << CS51);
  TCCR5B |= (1 << CS52);
  // enable timer compare interrupt:
  TIMSK5 |= (1 << OCIE5A);
  sei(); // enable global interrupts
  
  delay(500);
}

void loop()
{
}

ISR(TIMER5_COMPA_vect)
{
  CPG = CPG_function();  
  curr_pos = SPI.transfer(CPG);
}

float CPG_function()
{
  //solves Matsuoka equations for neuron1
  rungeKutta1(tau1,c,h,CPG_pos,I1,I2,I3,K);
  IC[0] = IC[0] + (1.0/6.0)*(K[0] + 2.0*(K[1] + K[2]) + K[3])*timestep;
  rungeKutta2(tau2,I1,I2,K);
  IC[1] = IC[1] + (1.0/6.0)*(K[0] + 2.0*(K[1] + K[2]) + K[3])*timestep;
        
  //solves Matsuoka equations for neuron2
  rungeKutta1(tau1,c,h,-(CPG_pos),I3,I4,I1,K);
  IC[2] = IC[2] + (1.0/6.0)*(K[0] + 2.0*(K[1] + K[2]) + K[3])*timestep;
  rungeKutta2(tau2,I3,I4,K);
  IC[3] = IC[3] + (1.0/6.0)*(K[0] + 2.0*(K[1] + K[2]) + K[3])*timestep;
        
  I1 = IC[0]; I2 = IC[1]; I3 = IC[2]; I4 = IC[3];
  CPG = IC[0] - IC[2];   
    
  CPG = CPG*100.0; 
   
}

//neuron1
float rungeKutta1(float tau1, float c, float h, float g, float IC0, float IC1, float IC2, float *K)
{
  K[0] = (1.0/tau1)*(c - IC0 - B*IC1 - Y*max(IC2,0) - h*max(g,0));
  K[1] = (1.0/tau1)*(c - (IC0 + M*K[0]) - B*(IC1 + M*K[0]) - Y*max(IC2 + M*K[0],0) - h*max((g),0));
  K[2] = (1.0/tau1)*(c - (IC0 + M*K[1]) - B*(IC1 + M*K[1]) - Y*max(IC2 + M*K[1],0) - h*max((g),0));
  K[3] = (1.0/tau1)*(c - (IC0 + timestep*K[2]) - B*(IC1 + timestep*K[2])  - Y*max(IC2 + timestep*K[2],0) - h*max((g),0));  
}//end of rungeKutta1 function

//neuron2
float rungeKutta2(float tau2, float IC0, float IC1, float *K)
{
  K[0] = (1.0/tau2)*(max(IC0,0) - IC1);
  K[1] = (1.0/tau2)*(max(IC0 + M*K[0],0) - IC1 + M*K[0]);
  K[2] = (1.0/tau2)*(max(IC0 + M*K[1],0) - IC1 + M*K[1]);
  K[3] = (1.0/tau2)*(max(IC0 + timestep*K[2],0) - IC1 + timestep*K[2]);
}//end of rungeKutta2 function

Sidenote: I am using an Ardumoto motor drive shield with L298 h-bridge driver on the ChipKIT to drive the motor and control its direction.

Thanks

Jacob Christ

Tue, 18 Dec 2012 01:49:27 +0000

analogWrite() may use timer registers and your use of them maybe confusing the issue. I'm not 100% sure of this, but in the back of my head I think this is the case. Can you use a different OC register?

Jacob

RoboNegro

Tue, 18 Dec 2012 16:57:21 +0000

Thanks for the response Jacob, and yes I think you are correct. I took a look at the analogWrite() function and it resets Timer2 everytime it is called. So I think I'm confusing the ISR because it is using Timer2 to generate its interrupt.

I will continue to look into this.

Sidenote: for those that might be interested, in order to look at the analogWrite() function the code is located in the Chipkit folder >hardware>pic32>cores>pic32>wiring_analog

GeneApperson

Thu, 20 Dec 2012 00:16:02 +0000

analogWrite uses output compare units to produce the PWM output. Timer 2 is used by analogWrite as the clock source for the output compares. When you call analogWrite, it is reprogramming the timer.

Gene Apperson Digilent