MotorController works

This commit is contained in:
Marcel Peterkau 2024-12-13 15:15:27 +01:00
parent 1ecf83fcc7
commit c62427a3a1
3 changed files with 322 additions and 205 deletions

View File

@ -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
}

View File

@ -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

View File

@ -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<const uint8_t *>(&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<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() {
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
}