Go Down

Topic: Oscilating Motor Instability (Read 397 times) previous topic - next topic

dgelman

Hello,

I am controlling a motor using a PID controller and a master and slave quadrature encoder on a DUE.  When the error between master and slave become greater than 255, my motor cannot read steady states and oscillates. One way I have figured out to solve this is essentially never letting the error go up 255 by incrementing encoder counts by decimals.  Doing this makes my control very slow - It will take many revolutions in my master side to complete my slave side.

I tried tuning and changing encoder counts.. I am wondering if people have clues and suggestions?

Code: [Select]
/*
Project:
Name: Daniel G
Date: 10/15/2012
Company: RRI
*/

/*
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 Master_CHA = 2;   // Interrupt INT0 on Pin 2    0
int Master_CHB = 4;   //
int Slave_CHA = 3;    // Interrupt INT1 on Pin 3   1
int Slave_CHB = 6;    //

int Master_CHB_val = 0;
int Slave_CHB_val = 0;
volatile float masterEncoder = 0;
volatile float slaveEncoder = 0;

// Control Signal and Error
float u = 0;
float err = 0;

// PID Control
int Kp = 10;
double Ki = .008;
double Kd = 0.2;

// Error Variables
float err_old = 0;
float err_intgrl = 0;
float err_dot = 0;
int max_int = 255;
int max_der = 255;

void setup()
{
  Serial.begin(115200);
  // slaveEncodertting Motor Pins
  pinMode(In1, OUTPUT); // Setting In1 (direction indicator)
  pinMode(In2, OUTPUT); // Setting In2 (direction indicator)
  pinMode(EnA, OUTPUT); // Setting PWM output
 
  // 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
//  PWMC_ConfigureClocks(4000 * PWM_MAX_DUTY_CYCLE , 0, VARIANT_MCK);
// 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 += 0.1;    // Increment Master count for a direction
  else masterEncoder -= 0.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.1;    // Increment Slave count for a direction
  else slaveEncoder -= 0.1;                        // 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;
 
  err_dot = err - err_old;
  err_dot = constrain(err_dot, -max_der, max_der);

  err_intgrl = err + err_intgrl;
  err_intgrl = constrain(err_intgrl, -max_int, max_int);
 
  u = float(Kp*err + Kd*err_dot + Ki*err_intgrl);
  u = constrain(u, -255, 255);
 
  if (u < 0)  {setDirection(LOW, HIGH);  u = -u;}
  else  setDirection(HIGH, LOW);
 
  analogWrite(EnA, u); // Write PWM value as the encoder count of the Master (controller)

  err = err_old;
  Serial.print(masterEncoder);
  Serial.print("|");
  Serial.print(slaveEncoder);
  Serial.print("|");
  Serial.println(u);
}
Dialup is the future.

Go Up
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy