Pages: [1]   Go Down
Author Topic: Oscilating Motor Instability  (Read 349 times)
0 Members and 1 Guest are viewing this topic.
London, ON
Offline Offline
Jr. Member
**
Karma: 0
Posts: 94
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
/*
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);
}
Logged

Dialup is the future.

Pages: [1]   Go Up
Jump to: