Two-interval timer coding

S

smuscat

Member
Hello,

I am trying to setup two interval timers working at the same time. I read the article found in the link: https://www.pjrc.com/teensy/td_timing_IntervalTimer.html, I copied and pasted the code and it worked fine. Currently, I am trying to implement PID programming for robotic arm at an interval timer at 1ms loop time and the forward kinematics print coordinates at every 100ms. I set all variable volatile to read within the two timer functions.
Reference is made from my Teensy 4.0 code, where at the top I declared timer 1 "intervaltimer myTimer" and timer 2 "intervalTimer myTimer2". Then set timer 1 at 1ms and timer 2 at 100ms( myTimer.begin(PID, 1000);//timer one ,myTimer2.begin(PRINT,1000000);//timer two. And finally, set priority 0 for PID and priority 1 for forward kinematics print. Timer 1 is working but timer 2 isn't, also tried to shift timer 2(print forward kinematics) to main program with no avail.
There something I am missing, kindly can you iluminate me how can I sort the problem, please.?

Thank you

Saviour

Code: 
#include <Wire.h>
#include <Servo.h>//gripper servo
#include <Math.h>
#include <movingAvg.h>
Servo myservo;

IntervalTimer myTimer;//inteval timer 1
IntervalTimer myTimer2;//interval timer 2

  // the pin with a LED

volatile int Servo_ADC_1=0;//pots readings from modified servo
volatile int Servo_ADC_2=0;
volatile int Servo_ADC_3=0;
volatile int Servo_ADC_4=0;
volatile int Servo_ADC_5=0;

volatile int servopot_1=15;//pins declaration from Teensy board // analog pin used to connect the potentiometer
volatile int servopot_2=16;
volatile int servopot_3=17;
volatile int servopot_4=20;
volatile int servopot_5=21;
volatile int byte_dummy=0;
volatile int val=0;
volatile int analog_channel_1=0;// analog channel from I2C chip ADS7828
volatile int analog_channel_2=0;
volatile int analog_channel_3=0;
volatile int analog_channel_4=0;
volatile int analog_channel_5=0;
volatile int analog_channel_6=0;
volatile int cnt=0;

volatile int error1=0;//pid error
volatile int error2=0;
volatile int error3=0;
volatile int error4=0;
volatile int error5=0;
//int pot1=0;
int angle=550;
volatile int cnt1=0;
volatile float integral1=0.0;//pid variables
volatile float derivative1=0.0;
volatile float integral2=0.0;
volatile float derivative2=0.0;
volatile float integral3=0.0;
volatile float derivative3=0.0;
volatile float integral4=0.0;
volatile float derivative4=0.0;
volatile float integral5=0.0;
volatile float derivative5=0.0;
volatile float Kd1=0.512;//221.4//136//1.43//0//   pid parameters //joint one
volatile float Ki1=2.18519;//0.0045//0.00226//0.00265//0.3
volatile float Kp1=2.0615;//2.385//0.2486//2.913//2.00
volatile float Kd2=0.0;//221.4//136//1.43//0   pid parameters //joint two
volatile float Ki2=0.01;//0.0045//0.00226//0.00265//0.3
volatile float Kp2=18.0;//2.385//0.2486//2.913//2.00
volatile float Kd3=0.0;//221.4//136//1.43//0pid parameters //joint three
volatile float Ki3=0.005;//0.0045//0.00226//0.00265//0.3
volatile float Kp3=18.0;//2.385//0.2486//2.913//2.00
volatile float Kd4=0.0;//221.4//136//1.43//0 pid parameters //joint four
volatile float Ki4=0.05;//0.0045//0.00226//0.00265//0.3
volatile float Kp4=10.0;//2.385//0.2486//2.913//2.00 pid parameters //joint five
volatile float Kd5=0.512;//221.4//136//1.43//0
volatile float Ki5=2.18519;//0.0045//0.00226//0.00265//0.3
volatile float Kp5=2.0165;//2.385//0.2486//2.913//2.00
volatile float errorBefore1=0.0;//pid variables
volatile float errorBefore2=0.0;
volatile float errorBefore3=0.0;
volatile float errorBefore4=0.0;
volatile float errorBefore5=0.0;
volatile float GAIN1=0;// pid outputs
volatile float GAIN2=0;
volatile float GAIN3=0;
volatile float GAIN4=0;
volatile float GAIN5=0;
volatile float temp_GAIN=0;
volatile int analog_before_2=0;
volatile int difference_2=0;
volatile int analog_step_2=0;

volatile int cnt2=0;
volatile  int i;
volatile int val1=0;
volatile int minium = 55;
volatile int maxium = 58;
volatile int step = 1;
volatile int curr =0;

volatile int wave=0;
volatile int potprev_2=2000;
volatile int potprev_3=2200;
volatile int temp_differ_2=0;
volatile int temp_differ_3=0;
volatile int differ_2=0;
volatile int differ_3=0;
volatile int value_2=0;
volatile int value_3=0;
volatile int deadband_2=0;
volatile boolean flag=false, flag1=false, flag2=false, flag3=false;



volatile float x_axis=0;
volatile float y_axis=0;
volatile float z_axis=0;
volatile float a=0.0;
volatile float b=0.0;
volatile float c=0.0;
volatile float x=0.0;


movingAvg Joint_1(20);
volatile int Avg_1=0;
movingAvg Joint_2(20);
volatile int Avg_2=0;
movingAvg Joint_3(20);
volatile int Avg_3=0;
movingAvg Joint_4(20);
volatile int Avg_4=0;



float pi=3.141592654;






void setup() {


  myTimer.begin(PID, 1000);//timer one
  myTimer2.begin(PRINT,1000000);//timer two
 
  myTimer.priority(0);
  myTimer2.priority(1);
    
  Wire.begin();
  Joint_1.begin();
  Joint_2.begin();
  Joint_3.begin();
  Joint_4.begin();
  analogReadResolution(12);
  analogWriteResolution(12);
  analogWriteFrequency(2,1000);// PWM for joint four
  analogWriteFrequency(3,1000);//PWM for joint four
  analogWriteFrequency(4,1000);
  myservo.attach(5);// gripper servo
  analogWriteFrequency(7,1000);//PWM for joint three
  analogWriteFrequency(8,1000);//PWM for joint three
  analogWriteFrequency(9,1000);//PWM for joint one
  analogWriteFrequency(10,1000);//PWM for joint one
  analogWriteFrequency(11,1000);//PWM for joint two
  analogWriteFrequency(12,1000);//PWM for joint two
  analogWriteFrequency(13,1000);//PWM for joint Five
  analogWriteFrequency(14,1000);//PWM for joint Five
 
   Serial.begin(921600);
  
}



void PRINT()
{

 a=-1.80542 + 0.000908359*Servo_ADC_1 ;
b=-2.19294 + 0.000946087*Servo_ADC_2;
c=2.32766 - 0.00129517*Servo_ADC_3;
x=2.64058 - 0.0014049*Servo_ADC_4;
//forward kinematics
x_axis=12*cos(a) + 5*sin(a) + 150*cos(x)*(cos(a)*cos(b)*cos(c) - cos(a)*sin(b)*sin(c)) - 150*sin(x)*(cos(a)*cos(b)*sin(c) + cos(a)*cos(c)*sin(b)) - 105*cos(a)*sin(b) + 92*cos(a)*cos(b)*cos(c) - 92*cos(a)*sin(b)*sin(c);
y_axis= 12*sin(a) - 5*cos(a) - 105*sin(a)*sin(b) + 150*cos(x)*(cos(b)*cos(c)*sin(a) - sin(a)*sin(b)*sin(c)) - 150*sin(x)*(cos(b)*sin(a)*sin(c) + cos(c)*sin(a)*sin(b)) + 92*cos(b)*cos(c)*sin(a) - 92*sin(a)*sin(b)*sin(c);
z_axis= 150*sin(b + c + x) + 92*sin(b + c) + 105*cos(b) + 50;


//val= map(analog_channel_6, 0, 4095, 35, 180);
//myservo.write(val);
   Serial.print(x_axis);
Serial.print(" ");
Serial.print(y_axis);
Serial.print(" ");
Serial.print(z_axis);
Serial.println();


 



}
void PID() {




 
   Servo_ADC_1=analogRead(servopot_1);//servos shaft pot readings
 Servo_ADC_2=analogRead(servopot_2);
 Servo_ADC_3=analogRead(servopot_3);
 Servo_ADC_4=analogRead(servopot_4);
 Servo_ADC_5=analogRead(servopot_5);

analog_channel_1=DVR(0x84);//pot readings from ADS7828
analog_channel_2=DVR(0xC4);
analog_channel_3=DVR(0x94);
analog_channel_4=DVR(0xD4);
analog_channel_5=DVR(0xA4);
analog_channel_6=DVR(0xE4);

Avg_1=Joint_1.reading(Servo_ADC_1);
//PID for joint1

  error1=analog_channel_1-Avg_1;//joint one pid
  integral1=integral1+error1;
  derivative1=error1-errorBefore1;
  errorBefore1=error1;
  GAIN1= Kp1*error1+Kd1*derivative1+Ki1*integral1;
  GAIN1=abs(GAIN1);
  if (GAIN1>1400)GAIN1=1400;

 if((error1)>50)
 { 
  analogWrite(10,0);
   analogWrite(9,GAIN1);
  
  
  }
  else if((error1)<-50)
  {
    analogWrite(9,0);
   analogWrite(10,GAIN1);
    
    }
 else if((error1>-50)||(error1<50))
  {
    analogWrite(9,0);
    analogWrite(10,0);
    integral1=0;
    GAIN1=0;
    
    }

cnt1++;

 //PID for joint 2
Avg_2=Joint_2.reading(Servo_ADC_2);

if(temp_differ_2+value_2==analog_channel_2)

{
  flag1=false;
  flag=false;
 
}

differ_2= analog_channel_2-potprev_2;
if(((differ_2>30)&&(differ_2<-30)&&(cnt1>1))||((flag=true)&&(cnt1>1)))
{
    
  if(flag1==false)
  {

  value_2=potprev_2;
  temp_differ_2=differ_2;
  }
 
value_2= position(analog_channel_2,value_2);

flag=true;

flag1 = true;
}
if(cnt1>1)cnt1=0;

  error2=value_2-Avg_2;
    if((error2<-80)||(error2>80))
  {
  integral2=integral2+error2;//integral windup
  }
  derivative2=error2-errorBefore2;
  errorBefore2=error2;
  GAIN2=0+ Kp2*error2+Kd2*derivative2+Ki2*integral2;//added deadband
 
  GAIN2=abs(GAIN2);
  if((error2)>10)
 { 
   if (GAIN2>4000){GAIN2=4000;}
  analogWrite(11,0);
   analogWrite(12,GAIN2);
  
  
  }
  else if((error2)<-10)
  {
    if (GAIN2>1500||GAIN2<4000){GAIN2=1500;}
   //if (GAIN2>4000)GAIN2=4000;
    analogWrite(12,0);
   analogWrite(11,GAIN2);
    
    }
 else if((error2>-10)||(error2<10))
  {
    analogWrite(11,0);
    analogWrite(12,0);
    integral2=0;
    //GAIN2=0;
    
    }
 
potprev_2= analog_channel_2;


//PID for joint 3

Avg_3=Joint_3.reading(Servo_ADC_3);
cnt2++;
if(temp_differ_3+value_3==analog_channel_3)

{
  flag3=false;
  flag2=false;
 
}

differ_3= analog_channel_3-potprev_3;
if(((differ_3>30)&&(differ_3<-30)&&(cnt2>1))||((flag2=true)&&(cnt2>1)))
{
    
  if(flag3==false)
  {

  value_3=potprev_3;
  temp_differ_3=differ_3;
  }
 
  value_3= position(analog_channel_3,value_3);

  flag2=true;
  flag3 = true;
}
if(cnt2>1)cnt2=0;



//PID for joint 3
  error3=value_3-Avg_3;//joint three pid
   if((error3<-80)||(error3>80))
  {
  integral3=integral3+error3;//integral windup
  }
  //integral3=integral3+error3;
  derivative3=error3-errorBefore3;
  errorBefore3=error3;
  GAIN3= Kp3*error3+Kd3*derivative3+Ki3*integral3;
  GAIN3=abs(GAIN3);
  //if (GAIN3>2500){GAIN3=2500;}

if((error3)>10)
 { 
 
   if (GAIN3>1000||GAIN3<4000){GAIN3=1000;}
 
 
 
  analogWrite(7,0);
   analogWrite(8,GAIN3);
  
  
  }
  else if((error3)<-10)
  {
    
    if (GAIN3>4000){GAIN3=4000;}
  
    analogWrite(8,0);
   analogWrite(7,GAIN3);
    
    }
 else if((error3>-10)||(error3<10))
  {  temp_GAIN=GAIN3;
    analogWrite(7,0);
    analogWrite(8,0);
    integral3=0;
    //GAIN3=0;
    
    }
potprev_3= analog_channel_3;

//PID for joint 4
Avg_4=Joint_4.reading(Servo_ADC_4);
  error4=analog_channel_4-Avg_4;//joint four pid
  integral4=integral4+error4;
  derivative4=error4-errorBefore4;
  errorBefore4=error4;
  GAIN4= Kp4*error4+Kd4*derivative4+Ki4*integral4;
  GAIN4=abs(GAIN4);
  if (GAIN4>4000) GAIN4=4000;


  
 
 
 if((error4)>20)
 { 
  analogWrite(3,0);
   analogWrite(2,GAIN4);
  
  
  }
  else if((error4)<-20)
  {
    analogWrite(2,0);
   analogWrite(3,GAIN4);
    
    }
 else if((error4>-20)||(error4<20))
  {
    analogWrite(2,0);
    analogWrite(3,0);
    integral4=0;
    GAIN4=0;
    
    }

    
    
    
   //PID for joint 5 
     error5=analog_channel_5-Servo_ADC_5;//joint five pid
  integral5=integral5+error5;
  derivative5=error5-errorBefore5;
  errorBefore5=error5;
  GAIN5= Kp5*error5+Kd5*derivative5+Ki5*integral5;
  GAIN5=abs(GAIN5);
  if (GAIN5>2000) GAIN5=2000;
if((error5)>20)
 { 
  analogWrite(13,0);
   analogWrite(14,0);
  
  
  }
  else if((error5)<-20)
  {
    analogWrite(14,0);
   analogWrite(13,GAIN5);
    
    }
 else if((error5>-20)||(error5<20))
  {
    analogWrite(13,0);
    analogWrite(14,GAIN5);
    integral5=0;
    GAIN5=0;
    
    }


}


void loop() {
  noInterrupts();
  interrupts();
}

int DVR(int hex)

{int lowbyte1=0, highbyte1=0, byte_dummy, pot_1=0;// ADS7828 I2C functions to convert Pots analog value to digital value


 
  Wire.beginTransmission(72);
Wire.send(hex);//10//2

Wire.endTransmission();

Wire.requestFrom(72,2);
while(Wire.available()>=2){
 highbyte1=Wire.receive();

 lowbyte1=Wire.receive();}
 byte_dummy=highbyte1<<8;
 pot_1=byte_dummy|lowbyte1;
//Serial.println(a);
//Serial.println(pot_1);
Wire.end();
  return pot_1;
}


// slow down robotic arm movement
int position(int potnew, int potprev)
{ int d=0;

d=potnew-potprev;

if((potnew>potprev)&&((d<-2)||(d>2)))
{

    potprev = potprev + 1;

}


if((potnew<potprev)&&((d<-2)||(d>2)))
{

potprev=potprev-1;


}

return potprev;


}
 
Without doing any detailed review, it looks like your Timer1 handler PID() function will take much longer than 1 ms to execute, so if you try to call that every 1 ms, nothing else will ever get a chance to run. As a test, try slowing it down to 10 or 100 ms, and then you can reconsider your I/O rates, etc. Also, your Timer2 is set for 1000000, which is 1 s, not 100 ms.
 
smuscat said:
Dear joepasquariello,

Big thank you,

I amended as adviced and worked

Saviour
Click to expand...
You're welcome. As a general rule, it's best to minimize code in your ISRs. For example, you might simply read the ADC and another inputs and set a "data ready" flag, and then do all of the calculations in loop() when it finds the flag is set.
 
You must log in or register to reply here.
Back
Top