Pages: [1]   Go Down
Author Topic: Interrupt problem  (Read 488 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 24
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi everyone,

My senior project is a PID controller to a rotary inverted pendulum (Furuta pendulum). To carry out this, I'm using:
-arduino mega 2560.
-two optical incremental encoders.
-a DC motor.
-H-bridge (LMD18200).

-The PID controller is inside an ISR routine as shown in the code.
-Encoder data is got by using external interrupts (attachInterrupt() function) and data is sent through serial port.
-I read that interrupts can cause comms fail, and when I see the serial on the screen the data encoder get wrong when pendulum is on upright position.
I guess that I have to disabled interrupts at some time, and then enable them again, but I still got the same problem and therefore the pendulum losses the upright position.
when error_2 variable is above 20 degrees the motor stops, and when is below -20 degrees motor stops too (a safe mode).
Here is the code. I hope someone of you guys can help me to find what's wrong in the code

Code:
/*Furuta Pendulum PID controller */
/*there is an schmitt trigger to filter encoder channels*/
#include <avr/interrupt.h>

const int A_2=18; /*channel A from encoder 2 plugged in pin 18*/
const int B_2=19; /*channel B from encoder 2 plugged in pin 19*/
const int dir=10; /*H-bridge pin plugged in pin 8*/
const int pwm=8; /*H-bridge pin plugged in pin 7*/
const int brake=9;/*H-bridge pin plugged in pin 6*/

const int S=53;/*switch*/
const int led=52;
volatile float count_2 = 0.0;
float angle_2,error2,V,Int_E,Area,Der_E,lastE;
const float setpoint=180.0; /*reference*/
const float dt=0.0041; /*sample time*/
const float kp=15.0; /*proportional gain*/
const float ki=0.0; /*integral gain*/
const float kd=0.0; /*derivative gain*/

ISR(TIMER2_OVF_vect)
  {
   
    Area=(error2+lastE)*0.00205;/*el 0.00205=dt/2*/
    Int_E=(Area+Int_E);
    Der_E=(error2-lastE)/dt;
    lastE=error2;
    V=kp*error2+ki*Int_E+kd*Der_E;
   
  }
 
void setup()
{
  TCCR2B=0x06;
  TCCR2A=0x00;
  ASSR=0x00;
  TIMSK2=0x01;
  TCNT2=0X00;
  TIFR2=0x01;
  SREG=0x80;
 
  pinMode(A_2, INPUT);
  pinMode(B_2, INPUT);
  pinMode (dir, OUTPUT);
  pinMode (pwm, OUTPUT);
  pinMode (brake, OUTPUT);
  pinMode(S,INPUT);
  pinMode(led,OUTPUT);
  attachInterrupt(5,doA2,CHANGE);
  attachInterrupt(4,doB2,CHANGE);
  Serial.begin (19200); Serial.println("START READING");
}

void loop()
{
  if(digitalRead(S)==HIGH)
  {digitalWrite(led,HIGH);
  attachInterrupt(5,doA2,CHANGE);
  attachInterrupt(4,doB2,CHANGE);
  angle_2=count_2*(360.0/10000.0);
  error2 = setpoint - angle_2;
  if (error2>0.0)
  { 
     motor_CCW ();
     analogWrite(pwm,V);

     if(V>255.0)
     {
       V=255.0;
       analogWrite(pwm,V);
     }
  }
  if (error2>=20.0)
        {
          motorStop ();
        }
  if(error2<0.0)
     {
       if(V<0.0)
       {
       motor_CW ();   
       V=V*(-1.0);
       analogWrite(pwm,V);
       }
       motor_CW();
       analogWrite(pwm,V);
       if(V>255.0)
     {
       V=255.0;
       analogWrite(pwm,V);
       
     }
   if(error2<=-20.0)
         {
          motorStop ();
         }
     }
   if(error2==0.0)
      {
        motorStop ();

      }
 
  Serial.print(" P2 = ");/*position encoder 2*/
  Serial.print(angle_2);/*position value encoder 2*/
  Serial.print(", E2 = ");
  Serial.print(error2);
  Serial.print(", P=");
  Serial.print(kp*error2);
  Serial.print(", I=");
  Serial.print(Int_E);
  Serial.print(", D=");
  Serial.print(Der_E);
  Serial.print(", V=");/*output control*/
  Serial.print(V);
  Serial.println();
  detachInterrupt(5);
  detachInterrupt(4);
 
  }digitalWrite(led,LOW);
}

void doA2(){
  // look for a low-to-high on channel A
  if (digitalRead(A_2) == HIGH) {
    // check channel B to see which way encoder is turning
    if (digitalRead(B_2) == LOW) { 
      count_2 = count_2 + 1;         // CW
    }
    else {
      count_2 = count_2 - 1;         // CCW
    }
  }
  else   // must be a high-to-low edge on channel A                                       
  {
    // check channel B to see which way encoder is turning 
    if (digitalRead(B_2) == HIGH) {   
      count_2 = count_2 + 1;          // CW
    }
    else {
      count_2 = count_2 - 1;          // CCW
    }
  }

}
void doB2(){
  // look for a low-to-high on channel B
  if (digitalRead(B_2) == HIGH) {   
   // check channel A to see which way encoder is turning
    if (digitalRead(A_2) == HIGH) { 
      count_2 = count_2 + 1;         // CW
    }
    else {
      count_2 = count_2 - 1;         // CCW
    }
  }
  // Look for a high-to-low on channel B
  else {
    // check channel B to see which way encoder is turning 
    if (digitalRead(A_2) == LOW) {   
      count_2 = count_2 + 1;          // CW
    }
    else {
      count_2 = count_2 - 1;          // CCW
    }
  }
}

void motor_CW ()/* motor spins clockwise direction*/
{
  digitalWrite(dir,HIGH);
  delayMicroseconds(1);
  digitalWrite(pwm,HIGH);
  delayMicroseconds(1);
  digitalWrite(brake,LOW);
}
void motor_CCW ()/*motor spins counter clockwise direction*/
{
  digitalWrite(dir,LOW);
  delayMicroseconds(1);
  digitalWrite(pwm,HIGH);
  delayMicroseconds(1);
  digitalWrite(brake,LOW);
}
void motorStop ()/*motor brakes*/
{
  digitalWrite (dir, LOW);
  delayMicroseconds(1);
  digitalWrite (pwm, LOW);
  delayMicroseconds(1);
  digitalWrite (brake, HIGH);
}
Logged

Leeds, UK
Offline Offline
Edison Member
*
Karma: 80
Posts: 1729
Once the magic blue smoke is released, it won't go back in!
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Firstly you need to change one of the lines to this
Code:
volatile float angle_2,error2,V,Int_E,Area,Der_E,lastE;

secondly, remove this from loop()
Code:
  attachInterrupt(5,doA2,CHANGE);
  attachInterrupt(4,doB2,CHANGE);

It only needs to be in the setup(), which it is already.
Logged

~Tom~

Offline Offline
Newbie
*
Karma: 0
Posts: 24
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi TCWORLD,

well, I did just what you said
Quote
Firstly you need to change one of the lines to this
Code:
volatile float angle_2,error2,V,Int_E,Area,Der_E,lastE;

secondly, remove this from loop()
Code:
  attachInterrupt(5,doA2,CHANGE);
  attachInterrupt(4,doB2,CHANGE);

It only needs to be in the setup(), which it is already.

But if I remove this
Code:
attachInterrupt(5,doA2,CHANGE);
  attachInterrupt(4,doB2,CHANGE);
from the loop, I don't get nothing through serial about encoder position it only shows me '0' all the time

I tried using PID library but the problem remains

Logged

Leeds, UK
Offline Offline
Edison Member
*
Karma: 80
Posts: 1729
Once the magic blue smoke is released, it won't go back in!
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

You'll need to remove:
  detachInterrupt(5);
  detachInterrupt(4);

from the end of the loop aswell.
Logged

~Tom~

Offline Offline
Newbie
*
Karma: 0
Posts: 24
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi again,
Quote
You'll need to remove:
  detachInterrupt(5);
  detachInterrupt(4);

from the end of the loop aswell.

Done,  serial shows encoder position again, but the main problem persist, I mean when pendulum is on the upright position and PID is being executed, encoder loss the upright position, stops showing 180 degrees (completely upward), and when pendulum is at 0 degrees (completely downward) serial should show 0 degrees but it's not this way, serial shows an angle value different of 0 degrees.
I tested the code for reading encoder in a new sketch and it works great. I also did the same for a code to test H-bridge and it works pretty well, but when both codes are inside one sketch, the problem arise
Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Done,  serial shows encoder position again, but the main problem persist, I mean when pendulum is on the upright position and PID is being executed, encoder loss the upright position, stops showing 180 degrees (completely upward), and when pendulum is at 0 degrees (completely downward) serial should show 0 degrees but it's not this way, serial shows an angle value different of 0 degrees.
I tested the code for reading encoder in a new sketch and it works great. I also did the same for a code to test H-bridge and it works pretty well, but when both codes are inside one sketch, the problem arise

I'm sorry, but I can't understand what you're trying to describe there. I get that the serial output is interfering with handling the decoder interrupts - is that the issue? What is the serial I/O being used for?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

Offline Offline
Newbie
*
Karma: 0
Posts: 24
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

well, serial shows pendulum position (amount of degrees) 180 degrees (upward) and 0 degrees (downward).
 Through what serial shows I realize that encoder is not showing position it should be, but the problem isn't the serial, is something inside the whole code that for real I don't now what is, I said the problem was interrupt because I read here http://www.practicalarduino.com/projects/water-flow-gauge (I got the printed version, but I found this link) that interrupts could cause problems inside the loop if data was being sending to the host. The book says
Quote
Note that the example program disables interrupts while sending data to the host. This is important because otherwise it may end up in a situation where the next pulse arrives before the transmission has finished, and the interrupt could cause problems for the serial connection
That's why I thought interrupts was the problem, even in the code  https://github.com/practicalarduino/WaterFlowGauge/blob/master/WaterFlowGauge.pde you can check out that despite of the fact interrupt is set on set up, in the loop it is disabled and enabled again. But I haven`t be able to make my code works.
Logged

Pages: [1]   Go Up
Jump to: