My PID controller only controls speed in one direction, the other motor direction moves at top speed - also my code only runs for 100 seconds until it stops?

// ************ DEFINITIONS************
float kp = 0.02;
float ki = 0.00015 ;
float kd = 0;

unsigned long t;
unsigned long t_prev = 0;

const byte interruptPinA = 2;
const byte interruptPinB = 3;
volatile float EncoderCount = 0;
const float PWMPin = 6;
const byte DirPin1 = 7;
const byte DirPin2 = 8;
int Pin13 = 13;
int NorthSouth = 0;
float x = 0.5;

volatile unsigned long count = 0;
unsigned long count_prev = 0;

float Theta, RPM, RPM_d;
float Theta_prev = 0;
int dt;
float RPM_max = 52;

#define pi 3.1416
float Vmax = 5;
float Vmin = -5;
float V = 0.1;
float e, e_prev = 0, inte, inte_prev = 0;

//**********FUNCTIONS******************
//     Void ISR_EncoderA
//     Void ISR_EncoderB
//     Void Motor Driver Write
//     Timer Interrupt
//*************************************
void ISR_EncoderA() {
  bool PinB = digitalRead(interruptPinB);
  bool PinA = digitalRead(interruptPinA);

  if (PinB == LOW) {
    if (PinA == HIGH) {
      EncoderCount++;
    }
    else {
      EncoderCount--;
    }
  }

  else {
    if (PinA == HIGH) {
      EncoderCount--;
    }
    else {
      EncoderCount++;
    }
  }
}



void ISR_EncoderB() {
  bool PinB = digitalRead(interruptPinA);
  bool PinA = digitalRead(interruptPinB);

  if (PinA == LOW) {
    if (PinB == HIGH) {
      EncoderCount--;
    }
    else {
      EncoderCount++;
    }
  }

  else {
    if (PinB == HIGH) {
      EncoderCount++;
    }
    else {
      EncoderCount--;
    }
  }
}



//***Motor Driver Functions*****

void WriteDriverVoltage(float V, float Vmax) {

  NorthSouth = digitalRead(Pin13);
  
  int PWMval = int(255 * abs(V)/Vmax);
  if (PWMval > 255) {
    PWMval = 255;
  }
  if (NorthSouth == HIGH) {
    digitalWrite(DirPin1, HIGH);
    digitalWrite(DirPin2, LOW);
  }
  else if (NorthSouth == LOW) {
    digitalWrite(DirPin1, LOW);
    digitalWrite(DirPin2, HIGH);
  }
  else {
    digitalWrite(DirPin1, LOW);
    digitalWrite(DirPin2, LOW);
    
  }

  analogWrite(PWMPin, PWMval);
}

void setup() {
  Serial.begin(9600);
  pinMode(interruptPinA, INPUT_PULLUP);
  pinMode(interruptPinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPinA), ISR_EncoderA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(interruptPinB), ISR_EncoderB, CHANGE);
  pinMode(DirPin1, OUTPUT);
  pinMode(DirPin2, OUTPUT);
  pinMode(Pin13, INPUT);

  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = 12499; //Prescaler = 64
  TCCR1B |= (1 << WGM12);
  TCCR1B |= (1 << CS11 | 1 << CS10);
  TIMSK1 |= (1 << OCIE1A);
  sei();
}

void loop() {
  if (count > count_prev) {
    t = millis();
    Theta = EncoderCount / 4172.0;
    dt = (t - t_prev);
    RPM_d = RPM_max * (x);
    if (t / 1000.0 > 100) {
      RPM_d = 0;
    }
    RPM = (Theta - Theta_prev) / (dt / 1000.0) * 60;
    e = RPM_d - RPM;
    inte = inte_prev + (dt * (e + e_prev) / 2);
    V = kp * e + ki * inte + (kd * (e - e_prev) / dt) ;
    if (V > Vmax) {
      V = Vmax;
      inte = inte_prev;
    }
    if (V < Vmin) {
      V = Vmin;
      inte = inte_prev;
    }

   WriteDriverVoltage(V, Vmax);

    Serial.print(RPM_d); Serial.print(" \t");
    Serial.print(RPM); Serial.print(" \t ");
    Serial.print(V); Serial.print("\t  ");
    Serial.print(e); Serial.println("  ");

    Theta_prev = Theta;
    count_prev = count;
    t_prev = t;
    inte_prev = inte;
    e_prev = e;
  }

}


ISR(TIMER1_COMPA_vect) {
  count++;
  Serial.print(count * 0.05); Serial.print(" \t");
}

Did it just start doing this today? Did you back out the latest changes to the program?
Paul

1 Like

I've been working on this for a while and am just getting to the point where I'm happy with all my wiring. I want to control direction with a slide switch (Pin13)

Using print within an interrupt can cause issue(s) ... look here.

1 Like

I deleted the " Serial.print(count * 0.05); Serial.print(" \t");" but to no avail - the issue is still there

ISR(TIMER1_COMPA_vect) {
count++;
Serial.print(count * 0.05); Serial.print(" \t");
}

You're doing a lot of printing each time count > count_prev, also your print speed is slow. Probably the print buffer overflows. Could try printing every 1/2 second or so and increasing the print speed to something much higher than 9600, i.e. 115200.

I think you have a problem when EncoderCount is going down. The Theta - Theta_prev goes negative so the RPM is negative. The target RPM (RPM_max * 0.5) is a positive number so the error term goes up which increases the voltage to the motor which causes to encoder to count down faster which makes the error even more...

Aren't you trying to reach a certain position and not a certain rotational velocity? You should make the desired Theta the setpoint.