Go Down

Topic: Qaudrature Encoders, DC Motors ..Issue (Read 2360 times) previous topic - next topic

dgelman

Hello,

For my current set up - I am qaudrature encoders for both master and slave (DC motor with a gear box).  I am developing a PID controller to move a DC motor.  Slow motions work perfectly and the controller works well (both using a P controller and PID controller). 

But I see two issues:
a) When I move the master quickly, the system becomes unstable and the error (between the 2 encoders, master and slave) permentantly increases, which in turns increases my signal indefintely.
which causes,
b) the main loop doesn't response accordingly, the Serial.print i get back every so often becomes lagged.

When the master side moves quickly the error increases quickly which in turns causes my unstability.  Perhaps the error value overflows and screws up my controller.  Perhaps this is being done via improper data types?

I am very stuck on this and would appreciate any advice. 

Thanks community,
Danny

/*
Project:
Name: Daniel G
Date: 10/15/2012
Company:
*/

/*
EnA  |  In1  |  In2  |  Description
-----------------------------------------------
0   |   X   |   X   |  Free Running Motor Stop
1   |   1   |   0   |  Forward
1   |   0   |   1   |  ReverslaveEncoder
1   |   1   |   1   |  Fast Motor Stop
-----------------------------------------------

CHA: ____EEEE____EEEE____EEEE____EEEE
CHB: EE____EEEE____EEEE____EEEE____EE
When CHA rislaveEncoders check the state of CHB.  From here you can determine the
direction of the encoder corresponding to the Master or Slave
*/

#include <stdio.h>
int In1 = 8;         
int In2 = 9;         
int EnA = 10;         
int encoderPower = 7;
int Master_CHA = 0;   
int Master_CHB = 4;   
int Slave_CHA = 1;   
int Slave_CHB = 6;   
int LOOPCOUNTER = 0;
volatile float masterEncoder = 0;
volatile float slaveEncoder = 0;

float u = 0; // Control Signal
float err = 0; // Error
float Kp = 8;
float Ki = 0.05;
float Kd = 0.8;

float err_old = 0;
float err_intgrl = 0;
float err_dot = 0;

int max_int = 200;
int max_der = 200;

int Master_CHB_val = 0;
int Slave_CHB_val = 0;

void setup()
{
  Serial.begin(9600); // Begin serial communication

  // slaveEncodertting Motor Pins
  pinMode(In1, OUTPUT); // Setting In1 (direction indicator)
  pinMode(In2, OUTPUT); // Setting In2 (direction indicator)
  pinMode(EnA, OUTPUT); // Setting PWM output
 
  // Temperary HIGH to encoderPower Pin to give Vcc
  pinMode(encoderPower, OUTPUT);
  digitalWrite(encoderPower, HIGH);
 
  // Set Encoder Pins & Interrupt
  pinMode(Master_CHB, INPUT);
  pinMode(Slave_CHB, INPUT);
  attachInterrupt(Master_CHA, Master_checkDirection, RISING); // CHA causes interrupt service routine
  attachInterrupt(Slave_CHA, Slave_checkDirection, RISING); // CHA causes interrupt service routine
 
  TCCR1B = TCCR1B & 0b11111000 | 0x02; // PWM frequency changed from 500Hz to 3.922KHz - L298 typically can support 25KHz.
}

// ISR for Master (Controller)
void Master_checkDirection()
{
  Master_CHB_val = digitalRead(Master_CHB);      // When CHA RAISES check CHB
  if (Master_CHB_val == 0)  masterEncoder += 1;    // Increment Master count for a direction
  else masterEncoder -= 1;                         // Decrement Master count for the other direction   
}

// ISR for Slave (Motor)
void Slave_checkDirection()
{
  Slave_CHB_val = digitalRead(Slave_CHB); // When CHA RAISES check CHB
  if (Slave_CHB_val == 0)  slaveEncoder += 0.02;    // Increment Slave count for a direction
  else slaveEncoder -= 0.02;                        // Decrement Slave count for the other direction
}

// Function to set direction of PWM
void setDirection(boolean In1_State, boolean In2_State)
{
  digitalWrite(In1, In1_State);
  digitalWrite(In2, In2_State);
}

void loop()
{
  err = slaveEncoder - masterEncoder;
  LOOPCOUNTER++;
 
  if (err < 0)  setDirection(LOW, HIGH);
  else if (err > 0)  setDirection(HIGH, LOW);

  err_dot = err - err_old;
  if (err_dot > max_der) err_dot = max_der;
  else if (err_dot < -max_der)  err_dot = -max_der;

  err_intgrl = err + err_intgrl;
  if (err_intgrl > max_int)  err_intgrl = max_int;
  else if (err_intgrl < -max_int)  err_intgrl = -max_int;

  u = (unsigned char)(abs(Kp*err));// + Kd*err_dot + Ki*err_intgrl);
  constrain(u, 0, 255);
 
  analogWrite(EnA, u); // Write PWM value as the encoder count of the Master (controller)
  if (LOOPCOUNTER == 800) 
  {
    Serial.println(u); 
    LOOPCOUNTER = 0;
  }
  err_old = err;
 
}
Dialup is the future.

PaulS

Code: [Select]
volatile float masterEncoder = 0;
volatile float slaveEncoder = 0;

Why are these floats?

dgelman

Mainly, in the ISR of the slave (motor), I increment by 0.02 because there is a large scaling issue that the gear box of the DC motor presents.  Just to keep it all transparent, I kept them at floats.  I am testing all sorts of variations because I feel that the wrong data types are causing my issue.

Dialup is the future.

MSP-Fan

If the encoder AB-signals are processed in software which is not running in real time, you might have to look for a hardware interface doing the counting. Another option could be jitter of of the encoder AB-signals, which the software can't catch. You can find hardware interfaces for incremental encoder here: http://www.ichaus.biz/MD. 

dgelman

#4
Nov 02, 2012, 08:16 pm Last Edit: Nov 02, 2012, 08:21 pm by dgelman Reason: 1

If the encoder AB-signals are processed in software which is not running in real time, you might have to look for a hardware interface doing the counting. Another option could be jitter of of the encoder AB-signals, which the software can't catch. You can find hardware interfaces for incremental encoder here: http://www.ichaus.biz/MD.  


I am not sure how that would be a problem.  The embedded system runs as real-time as possible - there isnt any outside processing taking up any clock cycles and the processor should be enough to do this.  The ATmega16l runs the same code under CodeVision and it works wonderfully.  I am pretty sure something triggers (data type) when a large error is present (from moving the master quickly).  

Also, changing the gains as well as changing the increment values (such as ++1 or ++0.2 in the ISR) change the performance of when the motor becomes unstable.  For example when I set the incremental count really low - making the err between encoder positions not exceed much, I can manipulate as fast as possible but the motor is slow.  This problem has something to do with scalling due to the gear box present on the slave end - which in turn effects the err data (and type)

Your thoughts?
Dialup is the future.

Go Up