MotorController works

src/MotorController.cpp

@@ -4,56 +4,119 @@
 const MotorConfig defaultConfig = {
     .minPWM = 0,
     .maxPWM = 255,
-    .inverted = false,
+    .inverted = true,
-    .responseSpeed = 10,
+    .responseSpeed = 50,
-    .lookupTable = {0, 28, 57, 85, 114, 142, 171, 199, 228, 255}
+    .lookupTable = {0, 10, 20, 30, 40, 50, 60, 70, 80, 100} // Percent values for 0% to 100%
 };
 
 MotorController::MotorController(int motorPin)
-    : motorPWMPin(motorPin), currentSpeed(0) {
+    : motorPWMPin(motorPin), currentSpeed(0), targetSpeed(0)
+{
     loadDefaults();
     pinMode(motorPWMPin, OUTPUT);
     stopMotor();
 }
 
-void MotorController::loadDefaults() {
+void MotorController::loadDefaults()
+{
     config = defaultConfig;
 }
 
-void MotorController::setTargetSpeed(int targetSpeedPercent) {
+void MotorController::setTargetSpeed(int targetSpeedPercent)
-    targetSpeedPercent = constrain(targetSpeedPercent, 0, 100);
+{
-    int tableIndex = map(targetSpeedPercent, 0, 100, 0, MotorConfig::lookupTableSize - 1);
+    if (targetSpeed != targetSpeedPercent)
-    uint8_t targetPWM = config.lookupTable[tableIndex];
+    {
-    uint8_t adjustedPWM = applyDynamicResponse(targetPWM);
+        Serial.print("MC: set targetSpeed to: ");
-    setPWM(adjustedPWM);
+        Serial.println(targetSpeed);
-    currentSpeed = targetSpeedPercent;
+    }
+    targetSpeed = constrain(targetSpeedPercent, 0, 100);
 }
 
-int MotorController::getSpeed() const {
+void MotorController::maintain()
+{
+    targetSpeed = constrain(targetSpeed, 0, 100);          // Ensure targetSpeed is valid
+    int adjustedSpeed = applyDynamicResponse(targetSpeed); // Dynamic adjustment in percentage
+
+    // Interpolate between lookup table values
+    int lowerIndex = map(adjustedSpeed, 0, 100, 0, MotorConfig::lookupTableSize - 1);
+    int upperIndex = min(lowerIndex + 1, MotorConfig::lookupTableSize - 1);
+    int lowerPercent = config.lookupTable[lowerIndex];
+    int upperPercent = config.lookupTable[upperIndex];
+
+    float interpolationFactor = (adjustedSpeed - lowerIndex * (100 / (MotorConfig::lookupTableSize - 1))) / (100.0 / (MotorConfig::lookupTableSize - 1));
+    int interpolatedPWM = map(lowerPercent + interpolationFactor * (upperPercent - lowerPercent), 0, 100, config.minPWM, config.maxPWM);
+
+    setPWM(interpolatedPWM);      // Apply final interpolated PWM value
+    currentSpeed = adjustedSpeed; // Update currentSpeed in percentage
+}
+
+int MotorController::getSpeed() const
+{
     return currentSpeed;
 }
 
-void MotorController::stopMotor() {
+void MotorController::stopMotor()
+{
     setTargetSpeed(0);
+    maintain();
 }
 
-MotorConfig MotorController::getConfig() const {
+MotorConfig MotorController::getConfig() const
+{
     return config;
 }
 
-void MotorController::setConfig(const MotorConfig &newConfig) {
+void MotorController::setConfig(const MotorConfig &newConfig)
+{
     config = newConfig;
 }
 
-void MotorController::setPWM(uint8_t pwmValue) {
+void MotorController::setPWM(uint8_t pwmValue)
-    uint8_t constrainedPWM = constrain(pwmValue, config.minPWM, config.maxPWM);
+{
+    int constrainedPWM = constrain(pwmValue, config.minPWM, config.maxPWM);
     analogWrite(motorPWMPin, config.inverted ? 255 - constrainedPWM : constrainedPWM);
 }
 
-int MotorController::applyDynamicResponse(int targetValue) {
+int MotorController::applyDynamicResponse(int targetPercent)
-    static int smoothedValue = 0;
+{
-    int delta = targetValue - smoothedValue;
+    // Intern in Promille arbeiten für höhere Genauigkeit
-    int step = delta / config.responseSpeed;
+    static int smoothedPromille = 0;
-    smoothedValue += step;
+    static unsigned long lastUpdateTime = millis();
-    return smoothedValue;
+
+    unsigned long currentTime = millis();
+    unsigned long elapsedTime = currentTime - lastUpdateTime;
+
+    if (elapsedTime == 0)
+        return smoothedPromille / 10; // Rückgabe in Prozent
+
+    // Zielwert in Promille berechnen
+    int targetPromille = targetPercent * 10;
+
+    // Dynamische Reaktionsgeschwindigkeit in Promille pro Sekunde
+    int maxChangePerMs = config.responseSpeed * 10; // Konfigurierbarer Faktor
+
+    // Berechne maximale Änderung basierend auf vergangener Zeit
+    int maxDelta = (maxChangePerMs * elapsedTime) / 1000;
+
+    // Tatsächliche Änderung begrenzen
+    int delta = targetPromille - smoothedPromille;
+    if (delta > maxDelta)
+        delta = maxDelta;
+    else if (delta < -maxDelta)
+        delta = -maxDelta;
+
+    // Smoothed Promille aktualisieren
+    smoothedPromille += delta;
+    lastUpdateTime = currentTime;
+
+    // Debug-Ausgabe
+    Serial.print("TargetPromille: ");
+    Serial.print(targetPromille);
+    Serial.print(", Delta: ");
+    Serial.print(delta);
+    Serial.print(", SmoothedPromille: ");
+    Serial.println(smoothedPromille);
+
+    // Rückgabe in Prozent (gerundet)
+    return (smoothedPromille + 5) / 10; // Rundung durch Addition von 5 vor Division
 }

src/MotorController.h

@@ -5,7 +5,8 @@
 #include <Arduino.h>
 
 // Motor configuration structure
-struct MotorConfig {
+struct MotorConfig
+{
     uint8_t minPWM;
     uint8_t maxPWM;
     bool inverted;
@@ -14,10 +15,12 @@ struct MotorConfig {
     uint8_t lookupTable[lookupTableSize];
 };
 
-class MotorController {
+class MotorController
+{
 private:
     int motorPWMPin;
     int currentSpeed;
+    int targetSpeed;
     MotorConfig config;
 
     int applyDynamicResponse(int targetValue);
@@ -26,6 +29,7 @@ private:
 public:
     MotorController(int motorPin);
 
+    void maintain();
     void stopMotor();
     void setTargetSpeed(int targetSpeedPercent);
     int getSpeed() const;
@@ -33,7 +37,6 @@ public:
     MotorConfig getConfig() const;
     void setConfig(const MotorConfig &newConfig);
     void loadDefaults();
-
 };
 
 #endif // MOTORCONTROLLER_H

src/main.cpp

@@ -20,14 +20,15 @@ const int motorPin = 9;      // Motor PWM pin
 const int pedalPin = A0;     // Pedal input pin
 
 // Global objects
-MotorController motor(motorPin);   // Motor control object
+MotorController motor(motorPin); // Motor control object
-PedalController pedal(pedalPin);   // Pedal input object
+PedalController pedal(pedalPin); // Pedal input object
 
 // Combined settings structure
-struct Settings {
+struct Settings
-    MotorConfig motorConfig;
+{
-    PedalConfig pedalConfig;
+  MotorConfig motorConfig;
-    uint8_t checksum;
+  PedalConfig pedalConfig;
+  uint8_t checksum;
 };
 
 Settings settings;
@@ -47,208 +48,258 @@ void configurePWMFrequency();
 void stopMotorAtNeedleUp();
 void printHelp();
 
-void setup() {
+void setup()
-    // Initialize pins
+{
-    pinMode(hallSensor, INPUT);
+  // Initialize pins
-    pinMode(encoderPinA, INPUT_PULLUP);
+  pinMode(hallSensor, INPUT);
-    pinMode(encoderPinB, INPUT_PULLUP);
+  pinMode(encoderPinA, INPUT_PULLUP);
-    pinMode(encoderButton, INPUT_PULLUP);
+  pinMode(encoderPinB, INPUT_PULLUP);
+  pinMode(encoderButton, INPUT_PULLUP);
 
-    // Configure PWM frequency
+  pinMode(LED_BUILTIN, OUTPUT);
-    configurePWMFrequency();
 
-    // Attach interrupts for encoder
+  // Configure PWM frequency
-    attachInterrupt(digitalPinToInterrupt(encoderPinA), handleEncoder, CHANGE);
+  configurePWMFrequency();
-    attachInterrupt(digitalPinToInterrupt(encoderPinB), handleEncoder, CHANGE);
-    attachInterrupt(digitalPinToInterrupt(encoderButton), handleEncoderButton, FALLING);
 
-    // Initialize serial and load settings
+  // Attach interrupts for encoder
-    Serial.begin(9600);
+  attachInterrupt(digitalPinToInterrupt(encoderPinA), handleEncoder, CHANGE);
-    loadSettings();
+  attachInterrupt(digitalPinToInterrupt(encoderPinB), handleEncoder, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoderButton), handleEncoderButton, FALLING);
 
-    // Initialize OLED display
+  // Initialize serial and load settings
+  Serial.begin(9600);
+  loadSettings();
+
+  // Initialize OLED display
   //  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
   //      Serial.println(F("SSD1306 allocation failed"));
   //  }
   //  display.clearDisplay();
- //   display.display();
+  //   display.display();
 
-    printHelp();
+  printHelp();
 }
 
-void loop() {
+void loop()
-    // Handle serial input for debugging and manual control
+{
-    handleSerialInput();
+  // Handle serial input for debugging and manual control
+  handleSerialInput();
+  motor.maintain();
 
-    if (autoCalibrating) {
+  if (autoCalibrating)
-        CalibrationState currentState = pedal.autoCalibrate(false);
+  {
-        if (currentState != lastCalibrationState) {
+    CalibrationState currentState = pedal.autoCalibrate(false);
-            lastCalibrationState = currentState;
+    if (currentState != lastCalibrationState)
-            switch (currentState) {
+    {
-                case RUNNING:
+      lastCalibrationState = currentState;
-                    Serial.println("Calibration running...");
+      switch (currentState)
-                    break;
+      {
-                case SUCCESS:
+      case RUNNING:
-                    Serial.println("Auto-calibration completed.");
+        Serial.println("Calibration running...");
-                    break;
+        break;
-                case ERROR:
+      case SUCCESS:
-                    Serial.println("Auto-calibration failed.");
+        Serial.println("Auto-calibration completed.");
-                    break;
+        break;
-                default:
+      case ERROR:
-                    break;
+        Serial.println("Auto-calibration failed.");
-            }
+        break;
-        }
+      default:
-        motor.setTargetSpeed(0); // Ensure motor stays off during calibration
+        break;
-    } else {
+      }
-        int pedalValue = serialControlEnabled ? 0 : pedal.getPedal();
-        motor.setTargetSpeed(pedalValue);
     }
+    motor.setTargetSpeed(0); // Ensure motor stays off during calibration
+  }
+  else if (serialControlEnabled)
+  {
+    // Serial control active, do nothing here; speed controlled via serial commands
+  }
+  else
+  {
+    int pedalValue = pedal.getPedal();
+    motor.setTargetSpeed(pedalValue);
+  }
 
-    // Update display
+  // Update display
- //   updateDisplay();
+  //   updateDisplay();
 
-    delay(10); // Short delay for stability
+  delay(10); // Short delay for stability
 }
 
-void stopMotorAtNeedleUp() {
+void stopMotorAtNeedleUp()
-    motor.stopMotor();
+{
-    delay(100); // Stabilize
+  motor.stopMotor();
+  delay(100); // Stabilize
 }
 
-void loadSettings() {
+void loadSettings()
-    EEPROM.get(0, settings);
+{
+  EEPROM.get(0, settings);
 
-    // Validate checksum
+  // Validate checksum
-    if (calculateChecksum(settings) != settings.checksum) {
+  if (calculateChecksum(settings) != settings.checksum)
-        Serial.println("Invalid settings checksum, loading defaults.");
+  {
-        motor.loadDefaults();
+    Serial.println("Invalid settings checksum, loading defaults.");
-        pedal.autoCalibrate(true);
+    motor.loadDefaults();
+    pedal.autoCalibrate(true);
 
-        settings.motorConfig = motor.getConfig();
-        settings.pedalConfig = pedal.getConfig();
-        settings.checksum = calculateChecksum(settings);
-
-        saveSettings();
-    } else {
-        motor.setConfig(settings.motorConfig);
-        pedal.setConfig(settings.pedalConfig);
-        Serial.println("Settings loaded from EEPROM.");
-    }
-}
-
-void saveSettings() {
     settings.motorConfig = motor.getConfig();
     settings.pedalConfig = pedal.getConfig();
     settings.checksum = calculateChecksum(settings);
 
-    EEPROM.put(0, settings);
+    saveSettings();
-    Serial.println("Settings saved to EEPROM.");
+  }
+  else
+  {
+    motor.setConfig(settings.motorConfig);
+    pedal.setConfig(settings.pedalConfig);
+    Serial.println("Settings loaded from EEPROM.");
+  }
 }
 
-uint8_t calculateChecksum(const Settings &s) {
+void saveSettings()
-    uint8_t sum = 0;
+{
-    const uint8_t *data = reinterpret_cast<const uint8_t *>(&s);
+  settings.motorConfig = motor.getConfig();
-    for (size_t i = 0; i < sizeof(Settings) - 1; i++) {
+  settings.pedalConfig = pedal.getConfig();
-        sum ^= data[i];
+  settings.checksum = calculateChecksum(settings);
+
+  EEPROM.put(0, settings);
+  Serial.println("Settings saved to EEPROM.");
+}
+
+uint8_t calculateChecksum(const Settings &s)
+{
+  uint8_t sum = 0;
+  const uint8_t *data = reinterpret_cast<const uint8_t *>(&s);
+  for (size_t i = 0; i < sizeof(Settings) - 1; i++)
+  {
+    sum ^= data[i];
+  }
+  return sum;
+}
+
+void handleEncoder()
+{
+  static uint8_t lastState = 0;
+  uint8_t currentState = (digitalRead(encoderPinA) << 1) | digitalRead(encoderPinB);
+  if ((lastState == 0b00 && currentState == 0b01) ||
+      (lastState == 0b01 && currentState == 0b11) ||
+      (lastState == 0b11 && currentState == 0b10) ||
+      (lastState == 0b10 && currentState == 0b00))
+  {
+    // Increment
+  }
+  else if ((lastState == 0b00 && currentState == 0b10) ||
+           (lastState == 0b10 && currentState == 0b11) ||
+           (lastState == 0b11 && currentState == 0b01) ||
+           (lastState == 0b01 && currentState == 0b00))
+  {
+    // Decrement
+  }
+  lastState = currentState;
+}
+
+void handleEncoderButton()
+{
+  // Handle encoder button press
+}
+
+void updateDisplay()
+{
+  display.clearDisplay();
+  display.setTextSize(1);
+  display.setTextColor(SSD1306_WHITE);
+
+  display.setCursor(0, 0);
+  display.print("Pedal: ");
+  display.println(pedal.getPedal());
+
+  display.display();
+}
+
+void handleSerialInput()
+{
+  if (Serial.available())
+  {
+    char input = Serial.read();
+    switch (input)
+    {
+    case 'c':
+      Serial.println("Starting auto-calibration...");
+      pedal.autoCalibrate(true);
+      autoCalibrating = true;
+      lastCalibrationState = IDLE; // Reset state tracking
+      break;
+    case 'x':
+      Serial.println("Stopping auto-calibration or serial control.");
+      autoCalibrating = false;
+      serialControlEnabled = false;
+      motor.stopMotor();
+      break;
+    case 'r':
+      Serial.println("Loading defaults.");
+      motor.loadDefaults();
+      pedal.loadDefaults();
+      break;
+    case 's':
+      Serial.println("Saving settings to EEPROM.");
+      saveSettings();
+      break;
+    case 'l':
+      Serial.println("Loading settings from EEPROM.");
+      loadSettings();
+      break;
+    case 'h':
+      printHelp();
+      break;
+    case '+':
+      serialControlEnabled = true;
+      motor.setTargetSpeed(motor.getSpeed() + 1);
+      Serial.print("Increased Speed to: ");
+      Serial.println(motor.getSpeed());
+      break;
+    case '-':
+      serialControlEnabled = true;
+      motor.setTargetSpeed(motor.getSpeed() - 1);
+      Serial.print("Decreased Speed to: ");
+      Serial.println(motor.getSpeed());
+      break;
+    case '0':
+    case '1':
+    case '2':
+    case '3':
+    case '4':
+    case '5':
+    case '6':
+    case '7':
+    case '8':
+    case '9':
+      if (serialControlEnabled)
+      {
+        motor.setTargetSpeed((input - 0x30) * 10);
+        Serial.print("Set Speed to: ");
+        Serial.println(motor.getSpeed());
+      }
+      break;
     }
-    return sum;
+    Serial.println(input);
+  }
 }
 
-void handleEncoder() {
+void printHelp()
-    static uint8_t lastState = 0;
+{
-    uint8_t currentState = (digitalRead(encoderPinA) << 1) | digitalRead(encoderPinB);
+  Serial.println("Available commands:");
-    if ((lastState == 0b00 && currentState == 0b01) ||
+  Serial.println("c - Start auto-calibration");
-        (lastState == 0b01 && currentState == 0b11) ||
+  Serial.println("x - Stop auto-calibration or serial control");
-        (lastState == 0b11 && currentState == 0b10) ||
+  Serial.println("r - Load defaults");
-        (lastState == 0b10 && currentState == 0b00)) {
+  Serial.println("s - Save settings to EEPROM");
-        // Increment
+  Serial.println("l - Load settings from EEPROM");
-    } else if ((lastState == 0b00 && currentState == 0b10) ||
+  Serial.println("h - Show this help");
-               (lastState == 0b10 && currentState == 0b11) ||
+  Serial.println("+ - Increment PWM");
-               (lastState == 0b11 && currentState == 0b01) ||
+  Serial.println("- - Decrement PWM");
-               (lastState == 0b01 && currentState == 0b00)) {
-        // Decrement
-    }
-    lastState = currentState;
 }
 
-void handleEncoderButton() {
+void configurePWMFrequency()
-    // Handle encoder button press
+{
-}
+  // Configure Timer1 for higher PWM frequency (e.g., ~8 kHz)
-
+  TCCR1B = (TCCR1B & 0b11111000) | 0x01; // Set prescaler to 1
-void updateDisplay() {
-    display.clearDisplay();
-    display.setTextSize(1);
-    display.setTextColor(SSD1306_WHITE);
-
-    display.setCursor(0, 0);
-    display.print("Pedal: ");
-    display.println(pedal.getPedal());
-
-    display.display();
-}
-
-void handleSerialInput() {
-    if (Serial.available()) {
-        char input = Serial.read();
-        switch (input) {
-        case 'c':
-            Serial.println("Starting auto-calibration...");
-            pedal.autoCalibrate(true);
-            autoCalibrating = true;
-            lastCalibrationState = IDLE; // Reset state tracking
-            break;
-        case 'x':
-            Serial.println("Stopping auto-calibration or serial control.");
-            autoCalibrating = false;
-            serialControlEnabled = false;
-            motor.stopMotor();
-            break;
-        case 'r':
-            Serial.println("Loading defaults.");
-            motor.loadDefaults();
-            pedal.loadDefaults();
-            break;
-        case 's':
-            Serial.println("Saving settings to EEPROM.");
-            saveSettings();
-            break;
-        case 'l':
-            Serial.println("Loading settings from EEPROM.");
-            loadSettings();
-            break;
-        case 'h':
-            printHelp();
-            break;
-        case '+':
-            serialControlEnabled = true;
-            motor.setTargetSpeed(motor.getSpeed() + 1);
-            Serial.print("Increased Speed to: ");
-            Serial.println(motor.getSpeed());
-            break;
-        case '-':
-            serialControlEnabled = true;
-            motor.setTargetSpeed(motor.getSpeed() - 1);
-            Serial.print("Decreased Speed to: ");
-            Serial.println(motor.getSpeed());
-            break;
-        }
-        Serial.println(input);
-    }
-}
-
-void printHelp() {
-    Serial.println("Available commands:");
-    Serial.println("c - Start auto-calibration");
-    Serial.println("x - Stop auto-calibration or serial control");
-    Serial.println("r - Load defaults");
-    Serial.println("s - Save settings to EEPROM");
-    Serial.println("l - Load settings from EEPROM");
-    Serial.println("h - Show this help");
-    Serial.println("+ - Increment PWM");
-    Serial.println("- - Decrement PWM");
-}
-
-void configurePWMFrequency() {
-    // Configure Timer1 for higher PWM frequency (e.g., ~8 kHz)
-    TCCR1B = (TCCR1B & 0b11111000) | 0x01; // Set prescaler to 1
 }