Motor con encoder: lazo PI a 100 y 200 rpm — Arduino Uno simulation

Gear motor JGA25 con encoder Hall detras de un L293D en un Uno. Las interrupciones cuentan los flancos A/B y un lazo PI ajusta el duty hasta la consigna.

An interactive Arduino Uno circuit simulation you can run free in your browser on Velxio, by dave.

Components

Sketch code

// Closed-loop speed control of a gear motor, read back through its encoder.
//
// The L293D drives the motor, the Hall encoder reports how fast the output
// shaft really turns, and a PI loop adjusts the duty until the two agree.
// Change TARGET_RPM, or the Gear ratio / Encoder PPR on the part, and watch
// the loop settle again.
#define EN   9   // L293D EN1: PWM sets the speed
#define IN1  7   // direction
#define IN2  8
#define ENC_A 2  // encoder outputs on the two interrupt pins
#define ENC_B 3

const float COUNTS_PER_REV = 11 * 4 * 30;  // PPR x4 decoding x gear ratio

volatile long counts = 0;

// x4 decoding: every edge on either channel is one count, and the other
// channel's level says which way the shaft went.
void onA() { counts += (digitalRead(ENC_A) == digitalRead(ENC_B)) ? -1 : 1; }
void onB() { counts += (digitalRead(ENC_A) == digitalRead(ENC_B)) ? 1 : -1; }

float integral = 0;

void setup() {
  Serial.begin(115200);
  pinMode(EN, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  pinMode(ENC_A, INPUT_PULLUP);
  pinMode(ENC_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_A), onA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_B), onB, CHANGE);
}

void loop() {
  // Alternate between two set points so the loop has something to do.
  float target = (millis() / 4000) % 2 ? 200 : 100;

  static long lastCounts = 0;
  static unsigned long lastMs = 0;
  delay(50);
  unsigned long now = millis();
  noInterrupts();
  long c = counts;
  interrupts();
  float rpm = (c - lastCounts) / COUNTS_PER_REV * 60000.0 / (now - lastMs);
  lastCounts = c;
  lastMs = now;

  float error = target - rpm;
  integral = constrain(integral + error * 0.05, -400, 400);
  int duty = constrain(0.4 * error + 1.2 * integral, 0, 255);
  analogWrite(EN, duty);

  Serial.print("target ");
  Serial.print(target, 0);
  Serial.print("  rpm ");
  Serial.print(rpm, 1);
  Serial.print("  duty ");
  Serial.println(duty);
}

More projects by dave