#include <PinChangeInterrupt.h>
// --- Hardware Pins ---
const int pinPA = 10;
const int pinPB = 11;
const int encoderA = A14;
const int encoderB = A15;
const int potPin = A0;
// --- PID Constants (Tuning for Position) ---
// Note: These usually differ significantly from Speed constants.
double Kp = 4.0; // Aggressiveness of movement
double Ki = 2.0; // Keep very low to prevent hunting/oscillation
double Kd = 1.5; // Dampens the approach to prevent overshoot
// --- Global Variables ---
volatile long current_position = 0;
long target_position = 0;
long command = 0;
// --- PID State ---
double integral = 0;
double last_error = 0;
void pin_change_isr() {
int pinA = digitalRead(encoderA);
int pinB = digitalRead(encoderB);
static int oldA = 0;
if (pinA != oldA) {
oldA = pinA;
current_position += (pinA ^ pinB) ? (-1) : 1;
}
}
void setup() {
Serial.begin(115200);
pinMode(pinPA, OUTPUT);
pinMode(pinPB, OUTPUT);
pinMode(encoderA, INPUT_PULLUP);
pinMode(encoderB, INPUT_PULLUP);
attachPCINT(digitalPinToPCINT(encoderA), pin_change_isr, CHANGE);
}
void setMotorPWM(int pwm) {
// deadband
if (abs(pwm) < 15) pwm = 0;
// Note: if using IBT-2 as motor driver, one should ensure that both PWM pins are never high simultaneously
pwm = constrain(pwm, -255, 255);
if (pwm > 0) {
digitalWrite(pinPB, LOW);
analogWrite(pinPA, pwm);
} else if (pwm < 0) {
digitalWrite(pinPA, LOW);
analogWrite(pinPB, -pwm);
} else {
digitalWrite(pinPA, LOW);
digitalWrite(pinPB, LOW);
}
}
void task_position_control() {
static unsigned long last_time = 0;
const int period_ms = 10; // Fast loop (100Hz) is better for position
unsigned long now = millis();
if (now - last_time >= period_ms) {
double dt = (now - last_time) / 1000.0;
last_time = now;
// 1. Get Target from Potentiometer (mapping to pulses)
// Example: Map pot to 0-10 full rotations (36 ppr * 5 turns)
target_position = map(analogRead(potPin), 0, 1023, 0, 36*2);
// 2. Calculate Error
long pos_snapshot = current_position; // Snapshot of volatile
double error = target_position - pos_snapshot;
// 3. PID Math
integral += error * dt;
integral = constrain(integral, -50, 50); // Anti-windup
double derivative = (error - last_error) / dt;
command = (int)(Kp * error + Ki * integral + Kd * derivative);
command = constrain(command, -255, 255);
setMotorPWM(command);
last_error = error;
}
}
void loop() {
task_position_control();
static unsigned long report_time = 0;
if (millis() - report_time > 100) {
Serial.print("Target:"); Serial.print(target_position);
Serial.print(" Actual:"); Serial.print(current_position);
Serial.print(" Command:"); Serial.print(command);
Serial.println();
report_time = millis();
}
}Fwd/rev
quadrature output
speed input
(pgUp / pgDown)