class PIDController { private: float kp, ki, kd; float setpoint = 0.0f; float integral = 0.0f; float previousError = 0.0f; uint32_t lastTime = 0;
float minOutput = 0.0f; float maxOutput = 255.0f;
public: PIDController(float p = 1.0f, float i = 0.0f, float d = 0.0f) : kp(p), ki(i), kd(d) {}
void setTunings(float p, float i, float d) { kp = p; ki = i; kd = d; }
void setSetpoint(float sp) { setpoint = sp; }
void setOutputLimits(float min, float max) { minOutput = min; maxOutput = max; }
float compute(float input) { uint32_t currentTime = millis(); float deltaTime = (currentTime - lastTime) / 1000.0f;
if (deltaTime <= 0) return 0.0f;
float error = setpoint - input;
float proportional = kp * error;
integral += error * deltaTime; if (integral > maxOutput / ki) integral = maxOutput / ki; if (integral < minOutput / ki) integral = minOutput / ki; float integralTerm = ki * integral;
float derivative = (error - previousError) / deltaTime; float derivativeTerm = kd * derivative;
previousError = error; lastTime = currentTime;
float output = proportional + integralTerm + derivativeTerm;
if (output > maxOutput) output = maxOutput; if (output < minOutput) output = minOutput;
return output; }
void reset() { integral = 0.0f; previousError = 0.0f; lastTime = millis(); } };
class MotorController { private: GPIOPin enablePin; GPIOPin directionPin1; GPIOPin directionPin2; GPIOPin encoderPinA; GPIOPin encoderPinB;
PIDController pidController;
volatile long encoderCount = 0; float currentSpeed = 0.0f; float targetSpeed = 0.0f;
uint32_t lastSpeedUpdate = 0; uint32_t SPEED_UPDATE_INTERVAL = 50;
public: MotorController(uint8_t en, uint8_t dir1, uint8_t dir2, uint8_t encA, uint8_t encB) : enablePin(en, true), directionPin1(dir1, true), directionPin2(dir2, true), encoderPinA(encA), encoderPinB(encB), pidController(0.5f, 0.1f, 0.05f) {}
void initialize() { attachInterrupt(digitalPinToInterrupt(encoderPinA.read()), [this]() { handleEncoderInterrupt(); }, RISING);
Serial.println("Motor controller initialized"); }
void setSpeed(float rpm) { targetSpeed = rpm; pidController.setSetpoint(rpm); }
void update() { uint32_t currentTime = millis();
if (currentTime - lastSpeedUpdate >= SPEED_UPDATE_INTERVAL) { lastSpeedUpdate = currentTime;
updateCurrentSpeed();
float controlOutput = pidController.compute(currentSpeed);
applyMotorControl(controlOutput); } }
float getCurrentSpeed() const { return currentSpeed; }
void stop() { targetSpeed = 0.0f; pidController.setSetpoint(0.0f); applyMotorControl(0); }
private: void handleEncoderInterrupt() { if (encoderPinB.read()) { encoderCount++; } else { encoderCount--; } }
void updateCurrentSpeed() { static long lastCount = 0; long countDiff = encoderCount - lastCount;
float revolutions = countDiff / 360.0f; float timeSeconds = SPEED_UPDATE_INTERVAL / 1000.0f; currentSpeed = revolutions / timeSeconds * 60.0f;
lastCount = encoderCount; }
void applyMotorControl(float output) { if (output > 0) { directionPin1.setHigh(); directionPin2.setLow(); } else if (output < 0) { directionPin1.setLow(); directionPin2.setHigh(); output = -output; } else { directionPin1.setLow(); directionPin2.setLow(); }
if (output > 128) { enablePin.setHigh(); } else { enablePin.setLow(); } } };
MotorController motorController(9, 8, 7, 2, 3);
void setup() { Serial.begin(9600); motorController.initialize();
Serial.println("Motor control system ready"); }
void loop() { motorController.update();
static uint32_t lastChange = 0; static float speeds[] = {0.0f, 500.0f, 1000.0f, 500.0f, 0.0f}; static int speedIndex = 0;
if (millis() - lastChange > 3000) { lastChange = millis(); motorController.setSpeed(speeds[speedIndex]);
Serial.printf("Setting target speed: %.1f RPM\n", speeds[speedIndex]);
speedIndex = (speedIndex + 1) % 5; }
static uint32_t lastPrint = 0; if (millis() - lastPrint > 1000) { lastPrint = millis(); Serial.printf("Current speed: %.1f RPM\n", motorController.getCurrentSpeed()); } }
|