Tuning PID
#include <Arduino.h>
// Pin definitions
#define ENCA 4 // Encoder A pin
#define ENCB 5 // Encoder B pin
#define PH_A 6 // Motor A phase pin (controls direction)
#define EN_A 7 // Motor A enable pin (controls speed)
// Global variables to track motor position and PID control
volatile int position = 0; // Current encoder position
long previousTime = 0; // Previous time for PID calculations
float previousError = 0; // Error from last iteration for derivative calculation
float errorIntegral = 0; // Integral of error over time
long delayStart = 5000; // Time delay for switching target positions
long startTime = 0; // Time when the motor starts moving toward the target
bool targetReached = false; // Flag to check if the target is reached
const int positionThreshold = 3; // Threshold to consider the target reached
long timeToReachTarget = 0; // Time taken to reach the target
bool targetLock = false; // Locks the target to avoid resetting time on oscillations
int currentTarget = 120; // Current target position
// PID parameters (tunable via serial input)
float kp = 1.0; // Proportional gain
float ki = 0.0; // Integral gain
float kd = 0.5; // Derivative gain
// Function prototypes
void setupPins();
void setupEncoder();
void motorControl(int targetPosition);
float calculatePID(int targetPosition);
void handleTarget();
void updatePIDValues();
void updateEncoder();
void setup() {
Serial.begin(9600); // Initialize serial communication
setupPins(); // Setup the motor control and encoder pins
setupEncoder(); // Initialize the encoder interrupt
// Print instructions for entering PID values
Serial.println("Enter kp, ki, and kd values separated by spaces (e.g., 1.0 0.5 0.1):");
delayStart = millis(); // Initialize the delay timer
startTime = millis(); // Set initial start time
}
// Main loop
void loop() {
updatePIDValues(); // Check for new PID values via serial input
handleTarget(); // Switch between target positions every 5 seconds
}
// Setup motor control and encoder pins
void setupPins() {
pinMode(ENCA, INPUT);
pinMode(ENCB, INPUT);
pinMode(PH_A, OUTPUT);
pinMode(EN_A, OUTPUT);
}
// Setup encoder interrupt
void setupEncoder() {
attachInterrupt(digitalPinToInterrupt(ENCA), updateEncoder, RISING);
}
// Motor control function using PID output
void motorControl(int targetPosition) {
float controlSignal = calculatePID(targetPosition);
// Constrain control signal to 0-255 for motor power
float motorPower = constrain(fabs(controlSignal)+12 , 0, 255);
// Set motor direction
digitalWrite(PH_A, controlSignal > 0 ? LOW : HIGH); // LOW = reverse, HIGH = forward
// Set motor speed
analogWrite(EN_A, motorPower);
}
// Calculate PID control output
float calculatePID(int targetPosition) {
long currentTime = micros();
float deltaTime = (currentTime - previousTime) / 1.0e6; // Convert to seconds
previousTime = currentTime;
// Read the current motor position
noInterrupts();
int currentPosition = position;
interrupts();
// Calculate error and its derivative
int error = currentPosition - targetPosition;
float errorDerivative = (error - previousError) / deltaTime;
// Calculate integral of error
errorIntegral += error * deltaTime;
// Compute PID control signal
float controlSignal = kp * error + kd * errorDerivative + ki * errorIntegral;
// Update previous error
previousError = error;
// Track time to reach target
if (abs(error) <= positionThreshold ) {
//startTime = millis();
targetReached = false;
}
if (abs(error) > positionThreshold ) {
timeToReachTarget = millis() - startTime;
targetReached = true;
// Debug output
Serial.print("Target: ");
Serial.print(targetPosition);
Serial.print(" Position: ");
Serial.print(currentPosition);
Serial.print(" Time to reach: ");
Serial.println(timeToReachTarget);
}
return controlSignal;
}
// Handle target switching between positions every 5 seconds
void handleTarget() {
int newTarget;
// Toggle between target positions every 5 seconds
if (millis() < delayStart + 5000) {
newTarget = 120; // Move to position 120
} else {
newTarget = -120; // Move to position -120
}
// If the target position has changed, reset the startTime and lock flags
if (newTarget != currentTarget) {
currentTarget = newTarget; // Update current target
startTime = millis(); // Reset startTime when switching target
targetReached = false; // Reset target reached flag
targetLock = false; // Unlock the target to allow timing
timeToReachTarget = 0; // Reset time to reach target
//Serial.print("Switched to new target: ");
//Serial.println(currentTarget);
}
motorControl(currentTarget); // Control motor to reach the current target
// Reset delay timer every 10 seconds (to create a 5-second loop)
if (millis() > delayStart + 10000) {
delayStart = millis();
}
}
// Update PID values from serial input
void updatePIDValues() {
if (Serial.available()) {
float newKp = Serial.parseFloat();
float newKi = Serial.parseFloat();
float newKd = Serial.parseFloat();
if (newKp != 0 || newKi != 0 || newKd != 0) {
kp = newKp;
ki = newKi;
kd = newKd;
// Debug output for new PID values
Serial.print("Updated PID values: kp=");
Serial.print(kp);
Serial.print(", ki=");
Serial.print(ki);
Serial.print(", kd=");
Serial.println(kd);
}
}
}
// Interrupt function to update encoder position
void updateEncoder() {
// Update position based on encoder input
position += digitalRead(ENCB) ? -1 : 1;
}
1. What do Kp, Ki, and Kd do?
2. Steps to Tune PID
3. Example of Manual Tuning
4. Common Issues
5. Visualizing the Tuning Process
6. Fine-Tuning Tips
7. Tools
Last updated