Initialisation depot

ESP32/DCC-Loco/src/MotorDriver.cpp

@@ -0,0 +1,213 @@
+/**
+ * @file MotorDriver.cpp
+ * @brief TB67H450FNG Motor Driver Implementation
+ */
+
+#include "MotorDriver.h"
+
+MotorDriver::MotorDriver() 
+    : pinIN1(0), pinIN2(0), pinPWM(0), pinCurrentSense(255),
+      targetSpeed(0), currentSpeed(0), targetDirection(true),
+      loadCompensationEnabled(false), accelRate(10), decelRate(10),
+      lastSpeedUpdate(0), Kp(1.0), Ki(0.1), Kd(0.5), 
+      integral(0), lastError(0), targetCurrent(0) {}
+
+bool MotorDriver::begin(uint8_t in1Pin, uint8_t in2Pin, uint8_t pwmPin, uint8_t currentSensePin) {
+    pinIN1 = in1Pin;
+    pinIN2 = in2Pin;
+    pinPWM = pwmPin;
+    pinCurrentSense = currentSensePin;
+    
+    // Configure pins
+    pinMode(pinIN1, OUTPUT);
+    pinMode(pinIN2, OUTPUT);
+    pinMode(pinPWM, OUTPUT);
+    
+    if (pinCurrentSense != 255) {
+        pinMode(pinCurrentSense, INPUT);
+    }
+    
+    // Setup PWM
+    ledcSetup(pwmChannel, pwmFrequency, pwmResolution);
+    ledcAttachPin(pinPWM, pwmChannel);
+    ledcWrite(pwmChannel, 0);
+    
+    // Set initial state (brake)
+    digitalWrite(pinIN1, LOW);
+    digitalWrite(pinIN2, LOW);
+    
+    return true;
+}
+
+void MotorDriver::setSpeed(uint8_t speed, bool forward) {
+    targetSpeed = speed;
+    targetDirection = forward;
+}
+
+void MotorDriver::emergencyStop() {
+    targetSpeed = 0;
+    currentSpeed = 0;
+    
+    // Apply brake
+    digitalWrite(pinIN1, HIGH);
+    digitalWrite(pinIN2, HIGH);
+    ledcWrite(pwmChannel, 0);
+}
+
+void MotorDriver::update() {
+    updateAcceleration();
+    
+    if (loadCompensationEnabled && pinCurrentSense != 255) {
+        updateLoadCompensation();
+    } else {
+        applyMotorControl();
+    }
+}
+
+void MotorDriver::updateAcceleration() {
+    unsigned long currentTime = millis();
+    
+    if (currentTime - lastSpeedUpdate < 50) {
+        return; // Update every 50ms
+    }
+    
+    lastSpeedUpdate = currentTime;
+    
+    if (currentSpeed < targetSpeed) {
+        // Accelerate
+        uint8_t delta = (accelRate > 0) ? (255 / accelRate) : 1;
+        if (currentSpeed + delta >= targetSpeed) {
+            currentSpeed = targetSpeed;
+        } else {
+            currentSpeed += delta;
+        }
+    } else if (currentSpeed > targetSpeed) {
+        // Decelerate
+        uint8_t delta = (decelRate > 0) ? (255 / decelRate) : 1;
+        if (currentSpeed <= delta || currentSpeed - delta <= targetSpeed) {
+            currentSpeed = targetSpeed;
+        } else {
+            currentSpeed -= delta;
+        }
+    }
+}
+
+void MotorDriver::applyMotorControl() {
+    if (currentSpeed == 0) {
+        // Stop/brake
+        digitalWrite(pinIN1, LOW);
+        digitalWrite(pinIN2, LOW);
+        ledcWrite(pwmChannel, 0);
+        return;
+    }
+    
+    if (currentSpeed == 1) {
+        // Emergency stop (brake)
+        digitalWrite(pinIN1, HIGH);
+        digitalWrite(pinIN2, HIGH);
+        ledcWrite(pwmChannel, 0);
+        return;
+    }
+    
+    // Map DCC speed (2-127) to PWM (0-255)
+    uint16_t pwmValue = map(currentSpeed, 2, 127, 0, 255);
+    
+    // Set direction
+    if (targetDirection) {
+        // Forward
+        digitalWrite(pinIN1, HIGH);
+        digitalWrite(pinIN2, LOW);
+    } else {
+        // Reverse
+        digitalWrite(pinIN1, LOW);
+        digitalWrite(pinIN2, HIGH);
+    }
+    
+    // Set PWM
+    ledcWrite(pwmChannel, pwmValue);
+}
+
+void MotorDriver::updateLoadCompensation() {
+    // Read current
+    uint16_t currentMa = readCurrent();
+    
+    // PID control
+    float error = targetCurrent - currentMa;
+    integral += error;
+    
+    // Anti-windup
+    if (integral > 1000) integral = 1000;
+    if (integral < -1000) integral = -1000;
+    
+    float derivative = error - lastError;
+    lastError = error;
+    
+    float correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
+    
+    // Apply correction to PWM
+    if (currentSpeed == 0 || currentSpeed == 1) {
+        applyMotorControl();
+        return;
+    }
+    
+    uint16_t basePwm = map(currentSpeed, 2, 127, 0, 255);
+    int16_t adjustedPwm = basePwm + (int16_t)correction;
+    
+    // Clamp PWM
+    if (adjustedPwm < 0) adjustedPwm = 0;
+    if (adjustedPwm > 255) adjustedPwm = 255;
+    
+    // Set direction
+    if (targetDirection) {
+        digitalWrite(pinIN1, HIGH);
+        digitalWrite(pinIN2, LOW);
+    } else {
+        digitalWrite(pinIN1, LOW);
+        digitalWrite(pinIN2, HIGH);
+    }
+    
+    ledcWrite(pwmChannel, adjustedPwm);
+}
+
+uint16_t MotorDriver::readCurrent() {
+    if (pinCurrentSense == 255) {
+        return 0;
+    }
+    
+    // Read ADC value
+    uint16_t adcValue = analogRead(pinCurrentSense);
+    
+    // Convert to milliamps (this is hardware dependent)
+    // Assuming 3.3V reference, 12-bit ADC, and current sense amplifier
+    // Adjust this based on your actual hardware
+    uint16_t currentMa = (adcValue * 3300) / 4096; // Simplified conversion
+    
+    return currentMa;
+}
+
+uint16_t MotorDriver::getMotorCurrent() {
+    return readCurrent();
+}
+
+void MotorDriver::setLoadCompensation(bool enable) {
+    loadCompensationEnabled = enable;
+    
+    if (enable) {
+        integral = 0;
+        lastError = 0;
+    }
+}
+
+void MotorDriver::setPIDParameters(float kp, float ki, float kd) {
+    Kp = kp;
+    Ki = ki;
+    Kd = kd;
+}
+
+void MotorDriver::setAccelRate(uint8_t rate) {
+    accelRate = rate;
+}
+
+void MotorDriver::setDecelRate(uint8_t rate) {
+    decelRate = rate;
+}