diff --git a/src/MotorController.cpp b/src/MotorController.cpp index d92e67b..d03ae0a 100644 --- a/src/MotorController.cpp +++ b/src/MotorController.cpp @@ -4,56 +4,119 @@ const MotorConfig defaultConfig = { .minPWM = 0, .maxPWM = 255, - .inverted = false, - .responseSpeed = 10, - .lookupTable = {0, 28, 57, 85, 114, 142, 171, 199, 228, 255} + .inverted = true, + .responseSpeed = 50, + .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) { - targetSpeedPercent = constrain(targetSpeedPercent, 0, 100); - int tableIndex = map(targetSpeedPercent, 0, 100, 0, MotorConfig::lookupTableSize - 1); - uint8_t targetPWM = config.lookupTable[tableIndex]; - uint8_t adjustedPWM = applyDynamicResponse(targetPWM); - setPWM(adjustedPWM); - currentSpeed = targetSpeedPercent; +void MotorController::setTargetSpeed(int targetSpeedPercent) +{ + if (targetSpeed != targetSpeedPercent) + { + Serial.print("MC: set targetSpeed to: "); + Serial.println(targetSpeed); + } + 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) { - uint8_t constrainedPWM = constrain(pwmValue, config.minPWM, config.maxPWM); +void MotorController::setPWM(uint8_t pwmValue) +{ + int constrainedPWM = constrain(pwmValue, config.minPWM, config.maxPWM); analogWrite(motorPWMPin, config.inverted ? 255 - constrainedPWM : constrainedPWM); } -int MotorController::applyDynamicResponse(int targetValue) { - static int smoothedValue = 0; - int delta = targetValue - smoothedValue; - int step = delta / config.responseSpeed; - smoothedValue += step; - return smoothedValue; +int MotorController::applyDynamicResponse(int targetPercent) +{ + // Intern in Promille arbeiten für höhere Genauigkeit + static int smoothedPromille = 0; + static unsigned long lastUpdateTime = millis(); + + 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 } diff --git a/src/MotorController.h b/src/MotorController.h index 085bdc8..8f6f39a 100644 --- a/src/MotorController.h +++ b/src/MotorController.h @@ -5,7 +5,8 @@ #include // 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 diff --git a/src/main.cpp b/src/main.cpp index 577cec9..64b7a35 100644 --- a/src/main.cpp +++ b/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 -PedalController pedal(pedalPin); // Pedal input object +MotorController motor(motorPin); // Motor control object +PedalController pedal(pedalPin); // Pedal input object // Combined settings structure -struct Settings { - MotorConfig motorConfig; - PedalConfig pedalConfig; - uint8_t checksum; +struct Settings +{ + MotorConfig motorConfig; + PedalConfig pedalConfig; + uint8_t checksum; }; Settings settings; @@ -47,208 +48,258 @@ void configurePWMFrequency(); void stopMotorAtNeedleUp(); void printHelp(); -void setup() { - // Initialize pins - pinMode(hallSensor, INPUT); - pinMode(encoderPinA, INPUT_PULLUP); - pinMode(encoderPinB, INPUT_PULLUP); - pinMode(encoderButton, INPUT_PULLUP); +void setup() +{ + // Initialize pins + pinMode(hallSensor, INPUT); + pinMode(encoderPinA, INPUT_PULLUP); + pinMode(encoderPinB, INPUT_PULLUP); + pinMode(encoderButton, INPUT_PULLUP); - // Configure PWM frequency - configurePWMFrequency(); + pinMode(LED_BUILTIN, OUTPUT); - // Attach interrupts for encoder - attachInterrupt(digitalPinToInterrupt(encoderPinA), handleEncoder, CHANGE); - attachInterrupt(digitalPinToInterrupt(encoderPinB), handleEncoder, CHANGE); - attachInterrupt(digitalPinToInterrupt(encoderButton), handleEncoderButton, FALLING); + // Configure PWM frequency + configurePWMFrequency(); - // Initialize serial and load settings - Serial.begin(9600); - loadSettings(); + // Attach interrupts for encoder + attachInterrupt(digitalPinToInterrupt(encoderPinA), handleEncoder, CHANGE); + 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() { - // Handle serial input for debugging and manual control - handleSerialInput(); +void loop() +{ + // Handle serial input for debugging and manual control + handleSerialInput(); + motor.maintain(); - if (autoCalibrating) { - CalibrationState currentState = pedal.autoCalibrate(false); - if (currentState != lastCalibrationState) { - lastCalibrationState = currentState; - switch (currentState) { - case RUNNING: - Serial.println("Calibration running..."); - break; - case SUCCESS: - Serial.println("Auto-calibration completed."); - break; - case ERROR: - Serial.println("Auto-calibration failed."); - break; - default: - break; - } - } - motor.setTargetSpeed(0); // Ensure motor stays off during calibration - } else { - int pedalValue = serialControlEnabled ? 0 : pedal.getPedal(); - motor.setTargetSpeed(pedalValue); + if (autoCalibrating) + { + CalibrationState currentState = pedal.autoCalibrate(false); + if (currentState != lastCalibrationState) + { + lastCalibrationState = currentState; + switch (currentState) + { + case RUNNING: + Serial.println("Calibration running..."); + break; + case SUCCESS: + Serial.println("Auto-calibration completed."); + break; + case ERROR: + Serial.println("Auto-calibration failed."); + break; + default: + break; + } } + 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 - // updateDisplay(); + // Update display + // updateDisplay(); - delay(10); // Short delay for stability + delay(10); // Short delay for stability } -void stopMotorAtNeedleUp() { - motor.stopMotor(); - delay(100); // Stabilize +void stopMotorAtNeedleUp() +{ + motor.stopMotor(); + delay(100); // Stabilize } -void loadSettings() { - EEPROM.get(0, settings); +void loadSettings() +{ + EEPROM.get(0, settings); - // Validate checksum - if (calculateChecksum(settings) != settings.checksum) { - Serial.println("Invalid settings checksum, loading defaults."); - motor.loadDefaults(); - pedal.autoCalibrate(true); + // Validate checksum + if (calculateChecksum(settings) != settings.checksum) + { + Serial.println("Invalid settings checksum, loading defaults."); + 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); - Serial.println("Settings saved to EEPROM."); + saveSettings(); + } + else + { + motor.setConfig(settings.motorConfig); + pedal.setConfig(settings.pedalConfig); + Serial.println("Settings loaded from EEPROM."); + } } -uint8_t calculateChecksum(const Settings &s) { - uint8_t sum = 0; - const uint8_t *data = reinterpret_cast(&s); - for (size_t i = 0; i < sizeof(Settings) - 1; i++) { - sum ^= data[i]; +void saveSettings() +{ + settings.motorConfig = motor.getConfig(); + settings.pedalConfig = pedal.getConfig(); + 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(&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() { - 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 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 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; - } - 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 +void configurePWMFrequency() +{ + // Configure Timer1 for higher PWM frequency (e.g., ~8 kHz) + TCCR1B = (TCCR1B & 0b11111000) | 0x01; // Set prescaler to 1 }