PID Control Error in DC Motor Operation

[jo1867]

Hello, I would like to control the speed of a DC motor using a current controller and voltage controller, as shown in the diagram below. However, the current and speed are not being controlled properly, and the system is returning strange values that I did not set. Since the current is being measured correctly, it seems to be a code issue. Could you please check if there are any problems with the code?

I used an L298N motor driver, an INA219 current sensor, and a 6V, 1:50 gear ratio D20 DC motor with an encoder. I will attach the circuit diagram below. If you have any suggestions or alternative solutions, I would greatly appreciate it.

I've been trying to resolve this issue for several days without success, so I'm in desperate need of help. I hope you understand that I used ChatGPT for translation as I do not live in an English-speaking country.

#include <PID_v1.h>
#include <Encoder.h>
#include <Adafruit_INA219.h>
#include <Wire.h>

Encoder wheelLeft(2, 4); // Hall sensor connected to encoder pins 2,4
long positionLeft  = 0; // Initial position = 0
Adafruit_INA219 ina219;

#define IN3 9 // Digital output, pin for speed control
#define IN4 10 // Digital output
#define ENB 11 // Analog output, pin for direction control

unsigned long t = 0; 
unsigned long currenttime = 0;
unsigned long speedcommandLastUpdated = 0; // Variable to store the last update time of speedcommand
float busvoltage = 0;
double current = 0; // Changed from float to double

double speedcommand = 0, speed = 0, currentcommand = 0;
float filltered_rpm = 0;

double Kp = 2.5, Ki = 0.01, Kd = 0.1; // Adjusting PID parameters
double KpA = 2, KiA = 0.005, KdA = 0.01; // PID parameters for current control

double vcommand = 0;
PID speedPID(&speed, &currentcommand, &speedcommand , Kp, Ki, Kd, DIRECT); // Declare speed PID
PID currentPID(&current, &vcommand, &currentcommand, KpA, KiA, KdA, DIRECT); // Declare current PID

void setup() {
  Serial.begin(9600);
  Wire.setClock(400000);

  Serial.println("INA219 Current Measurement");

  if (!ina219.begin()) {
    Serial.println("No INA219");
    while (1) { delay(10); }
  }

  pinMode(IN3, OUTPUT); 
  pinMode(IN4, OUTPUT); // Set pins for direction control

  speed = 0;
  speedcommand  = 0;
  currentcommand = 0;
  vcommand = 0;

  speedPID.SetMode(AUTOMATIC); // Automatically calculate error
  speedPID.SetOutputLimits(-255, 255); // Set PID output limits
  currentPID.SetOutputLimits(-255, 255); // Set current PID output limits
  currentPID.SetMode(AUTOMATIC); // Set current PID mode

  ina219.setCalibration_16V_400mA(); // Calibrate the sensor
}

void loop() {
  // Process speedcommand input by user
  if (Serial.available()) {
    String data = Serial.readStringUntil('\n');
    speedcommand = data.toInt(); // Set speedcommand value
    speedcommandLastUpdated = millis(); // Update time when speedcommand is set
  }

  if (millis() - t > 100) { // Execute every 0.1 seconds
    t = millis(); // Save current time to t
    long newLeft = wheelLeft.read(); // Read encoder pulse count
    float leftRPM = (newLeft * 600) / 1400.0; // Calculate RPM based on pulses (for 1 minute)

    wheelLeft.write(0); // Reset pulse count

    // Calculate speed from encoder values
    speed = leftRPM; // Save RPM value to speed

    // Measure voltage and current
    busvoltage = ina219.getBusVoltage_V(); 
    current = ina219.getCurrent_mA(); // Measure current

    // Compute PID
    speedPID.Compute();
    currentPID.Compute();

    // Control motor based on vcommand calculated by PID
    if (vcommand == 0) {
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, LOW);
      analogWrite(ENB, 0); // Stop motor if speed is 0
    } 
    else if (vcommand > 0) {
      digitalWrite(IN3, LOW);
      digitalWrite(IN4, HIGH);
      analogWrite(ENB, vcommand); // Move forward if speed is positive
    } 
    else if (vcommand < 0) {
      digitalWrite(IN3, HIGH);
      digitalWrite(IN4, LOW);
      analogWrite(ENB, abs(vcommand)); // Move backward if speed is negative
    }

    // Output for debugging
    Serial.print("Speed Command = ");
    Serial.println(speedcommand);
    Serial.print("Speed = ");
    Serial.println(speed);
    Serial.print("V Command = ");
    Serial.println(vcommand);
    Serial.print("Current = ");
    Serial.println(current);
    Serial.print("Bus Voltage = ");
    Serial.println(busvoltage);
    Serial.print("Current Command = ");
    Serial.println(currentcommand);
    Serial.print("Encoder RPM = ");
    Serial.println(leftRPM);
    Serial.println();
  }
}

[jremington]

I recommend to get each individual aspect of the project working alone, before adding other parts.

For example, start by getting the motor control working. The PID algorithm has to be "tuned", that is, proper values for Ki, Kd and Kp selected, before it will work. In English, the search phrase "PID tuning" will find tutorials on how to do that systematically.

[blh64]

The INA219 just measures current. It can not adjust it so why are you trying to use it for PID? You do not need 2 PID controllers. You output is RPM, your input is PWM that you are providing to the L298N. You should only be using one PID.