souko/BLDC_SewingMachineController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Calibration Works

Browse Source
This commit is contained in:
Marcel Peterkau
2025-12-04 01:18:10 +01:00
parent a8a3ff2366
commit 23c7acd386
5 changed files with 385 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/MotorController.cpp
View File

@@ -9,8 +9,8 @@ const MotorConfig defaultConfig = {
.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), targetSpeed(0)
MotorController::MotorController(int motorPin, bool *debugFlag)
: motorPWMPin(motorPin), currentSpeed(0), targetSpeed(0), debugFlag(debugFlag)
{
loadDefaults();
pinMode(motorPWMPin, OUTPUT);
@@ -44,6 +44,14 @@ void MotorController::maintain()
setPWM(interpolatedPWM); // Apply final interpolated PWM value
currentSpeed = adjustedSpeed; // Update currentSpeed in percentage
if (debugFlag && *debugFlag)
{
Serial.print("[MOTOR] current=");
Serial.print(currentSpeed);
Serial.print(" target=");
Serial.println(targetSpeed);
}
}
int MotorController::getSpeed() const

3
src/MotorController.h
View File

@@ -21,13 +21,14 @@ private:
int motorPWMPin;
int currentSpeed;
int targetSpeed;
bool *debugFlag;
MotorConfig config;
int applyDynamicResponse(int targetValue);
void setPWM(uint8_t pwmValue);
public:
MotorController(int motorPin);
MotorController(int motorPin, bool *debugFlag = nullptr);
void maintain();
void stopMotor();

454
src/PedalController.cpp
View File

@@ -1,32 +1,37 @@
// PedalController.cpp
#include "PedalController.h"
// Basiskonfiguration
// sinnvolle Defaults grob um deinen Bereich
// nicht gedrückt ~425, gedrückt ~227
static const PedalConfig defaultConfig = {
.minRaw = 0,
.maxRaw = 1023,
.minRaw = 230, // grob gedrückt
.maxRaw = 420, // grob nicht gedrückt
.calibrated = false,
.filterStrength = 8 // 8er Moving Average als Standard
.filterStrength = 3 // schnellerer Filter
};
// Tuning-Konstanten für die Auto-Cal
static const int STABLE_THRESHOLD = 3; // zulässige ADC-Differenz für "stabil"
static const int STABLE_THRESHOLD = 3; // ADC-Delta, ab dem Stabilität neu bewertet wird
static const unsigned long STABLE_TIME = 1000; // ms, die der Wert stabil sein muss
static const unsigned long SWEEP_TIME = 3000; // ms für die Sweep-Phase
static const int MIN_RANGE = 50; // minimale Spannweite min/max
static const int SAFETY_MARGIN_LOW = 5; // Puffer unten
static const int SAFETY_MARGIN_HIGH = 5; // Puffer oben
static const uint8_t MIN_FS = 2;
static const uint8_t MAX_FS = 32;
static const uint8_t DEFAULT_FS = 8;
static const int NOISE_TOLERANCE = 5; // max. erlaubtes Rauschen im Stabilitätsfenster
static const int MIN_RANGE = 50; // minimale ADC-Spannweite für gültigen Pedalweg
static const int SAFETY_MARGIN_LOW = 3; // Untere Grenze etwas anheben
static const int SAFETY_MARGIN_HIGH = 3; // Obere Grenze etwas absenken
static const int MOVE_DELTA = 15; // min. Abstand zur Referenz für "echte Bewegung"
static const int NEAR_TOLERANCE = 10; // max. Differenz zw. Wiederholmessungen
static const uint8_t MIN_FS = 1; // Filterstärke min.
static const uint8_t MAX_FS = 32; // Filterstärke max.
static const uint8_t DEFAULT_FS = 3; // Standard-Filterstärke
PedalController::PedalController(int analogPin,
int digitalPin,
uint8_t digitalNotPressedLevel)
uint8_t digitalNotPressedLevel,
bool *debugFlag)
: analogPin(analogPin),
digitalPin(digitalPin),
useDigital(digitalPin >= 0),
digitalNotPressedLevel(digitalNotPressedLevel ? 1 : 0),
debugFlag(debugFlag),
filteredValue(-1),
calState(IDLE),
calStep(0),
@@ -34,34 +39,34 @@ PedalController::PedalController(int analogPin,
lastChangeTime(0),
lastSample(0),
minSeen(1023),
maxSeen(0)
maxSeen(0),
lastStatusPrint(0),
firstReleased(0),
firstPressed(0),
secondReleased(0),
secondPressed(0),
step2MovedEnough(false),
step3NearReleased(false),
step4NearPressed(false)
{
if (useDigital)
{
pinMode(digitalPin, INPUT); // Hall-Module haben normalerweise eigenen Pullup
pinMode(digitalPin, INPUT); // nur noch Debug
}
loadDefaults();
}
// Rohwert lesen und glätten
// Rohwert lesen (unglättet)
int PedalController::readRaw()
{
int raw = analogRead(analogPin);
return applySmoothing(raw);
return analogRead(analogPin);
}
int PedalController::applySmoothing(int raw)
{
uint8_t fs = config.filterStrength;
// Filter deaktiviert? Direkt durchreichen.
if (fs <= 1)
{
filteredValue = raw;
return raw;
}
// Erster Wert: direkt übernehmen
if (filteredValue < 0)
{
@@ -69,9 +74,21 @@ int PedalController::applySmoothing(int raw)
return raw;
}
// 32-Bit Akkumulator, um Überlauf zu vermeiden
if (raw >= filteredValue)
{
filteredValue = raw;
return raw;
}
if (fs <= 1)
{
filteredValue = raw;
return raw;
}
// 32-Bit Akkumulator mit Rundung
int32_t acc = filteredValue * (int32_t)(fs - 1) + raw;
filteredValue = acc / fs;
filteredValue = (acc + (fs / 2)) / fs;
return (int)filteredValue;
}
@@ -86,86 +103,257 @@ CalibrationState PedalController::autoCalibrate(bool reset)
config = defaultConfig;
config.calibrated = false;
// interne Zustände für die Kalibrierung
calState = RUNNING;
calStep = 1; // Schritt 1: Pedal losgelassen
calStep = 1;
stepStartTime = now;
lastChangeTime = now;
filteredValue = -1;
int raw = readRaw();
lastSample = raw;
lastSample = applySmoothing(readRaw());
minSeen = 1023;
maxSeen = 0;
lastStatusPrint = 0;
firstReleased = 0;
firstPressed = 0;
secondReleased = 0;
secondPressed = 0;
step2MovedEnough = false;
step3NearReleased = false;
step4NearPressed = false;
Serial.println("CAL: Step 1 - Pedal NICHT drücken (voll loslassen und still halten).");
return calState;
}
// Kein Reset angefordert, aber auch keine aktive Kalibrierung
if (calStep == 0)
{
calState = IDLE;
return calState;
}
int raw = readRaw();
int raw = applySmoothing(readRaw());
int diff = abs(raw - lastSample);
printStatus(raw);
switch (calStep)
{
case 1: // Schritt 1: Pedal losgelassen halten
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
// Wert war lange genug stabil -> als unteres Ende merken
if (now - lastChangeTime > STABLE_TIME)
{
minSeen = raw;
config.minRaw = raw;
calStep = 2;
lastSample = raw;
lastChangeTime = now;
}
calState = RUNNING;
break;
case 2: // Schritt 2: Pedal voll durchtreten
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
// Wert war lange genug stabil -> als erstes oberes Ende merken
if (now - lastChangeTime > STABLE_TIME)
{
maxSeen = raw;
config.maxRaw = raw;
calStep = 3;
stepStartTime = now;
}
calState = RUNNING;
break;
case 3: // Schritt 3: Sweep, um echten min/max Bereich zu erfassen
case 1:
{ // 1. Losgelassen messen
// Rauschen tracken
if (raw < minSeen)
minSeen = raw;
if (raw > maxSeen)
maxSeen = raw;
if (now - stepStartTime > SWEEP_TIME)
if (diff > STABLE_THRESHOLD)
{
int low = minSeen + SAFETY_MARGIN_LOW;
int high = maxSeen - SAFETY_MARGIN_HIGH;
lastSample = raw;
lastChangeTime = now;
}
if (now - lastChangeTime > STABLE_TIME)
{
int noise = maxSeen - minSeen;
if (noise > NOISE_TOLERANCE)
{
Serial.print("CAL: FEHLER - zu viel Rauschen im losgelassenen Zustand (noise=");
Serial.print(noise);
Serial.println(").");
calState = ERROR;
calStep = 0;
break;
}
firstReleased = raw;
config.maxRaw = raw; // hoher Wert = losgelassen
minSeen = 1023;
maxSeen = 0;
lastSample = raw;
lastChangeTime = now;
step2MovedEnough = false;
calStep = 2;
Serial.print("CAL: Step 1 OK. firstReleased=");
Serial.println(firstReleased);
Serial.println("CAL: Step 2 - Pedal JETZT GANZ DURCHDRÜCKEN und halten.");
}
calState = RUNNING;
break;
}
case 2:
{ // 2. Voll drücken & halten (erste Press-Messung)
// Phase 1: Warten, bis wirklich weit genug vom losgelassenen Punkt weg
if (!step2MovedEnough)
{
if (abs(raw - firstReleased) > MOVE_DELTA)
{
step2MovedEnough = true;
lastSample = raw;
lastChangeTime = now;
Serial.println("CAL: Step 2 - Bewegung erkannt, warte auf stabil gedrückt.");
}
}
else
{
// Phase 2: gedrückte Position muss stabil werden
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
if (now - lastChangeTime > STABLE_TIME)
{
// Wert lange stabil -> als erste gedrückte Referenz übernehmen
firstPressed = lastSample;
config.minRaw = firstPressed; // gedrückt = niedriger
lastSample = raw;
lastChangeTime = now;
calStep = 3;
step3NearReleased = false;
Serial.print("CAL: Step 2 OK. firstPressed=");
Serial.println(firstPressed);
Serial.println("CAL: Step 3 - Pedal wieder LOSLASSEN und still halten.");
}
}
calState = RUNNING;
break;
}
case 3:
{ // 3. Wieder loslassen & halten (zweite Released-Messung)
// Phase 1: warten, bis wir wieder in der Nähe der losgelassenen Position sind
if (!step3NearReleased)
{
if (abs(raw - firstReleased) < MOVE_DELTA)
{
step3NearReleased = true;
lastSample = raw;
lastChangeTime = now;
Serial.println("CAL: Step 3 - in Nähe von losgelassen, warte auf stabile Position.");
}
}
else
{
// Phase 2: losgelassen-Position muss stabil werden
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
if (now - lastChangeTime > STABLE_TIME)
{
secondReleased = lastSample;
if (abs(secondReleased - firstReleased) > NEAR_TOLERANCE)
{
Serial.println("CAL: FEHLER - losgelassene Position inkonsistent zwischen erstem und zweitem Mal.");
calState = ERROR;
calStep = 0;
break;
}
lastSample = raw;
lastChangeTime = now;
step4NearPressed = false;
calStep = 4;
Serial.print("CAL: Step 3 OK. secondReleased=");
Serial.println(secondReleased);
Serial.println("CAL: Step 4 - Pedal erneut GANZ DURCHDRÜCKEN und halten.");
}
}
calState = RUNNING;
break;
}
case 4:
{ // 4. Wieder voll drücken & halten (zweite Press-Messung)
// Phase 1: warten, bis wir wieder in der Nähe der ersten Press-Position sind
if (!step4NearPressed)
{
if (abs(raw - firstPressed) < MOVE_DELTA)
{
step4NearPressed = true;
lastSample = raw;
lastChangeTime = now;
Serial.println("CAL: Step 4 - in Nähe von voll gedrückt, warte auf stabile Position.");
}
}
else
{
// Phase 2: gedrückte Position muss stabil werden
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
if (now - lastChangeTime > STABLE_TIME)
{
secondPressed = lastSample;
if (abs(secondPressed - firstPressed) > NEAR_TOLERANCE)
{
Serial.println("CAL: FEHLER - gedrückte Position inkonsistent zwischen erstem und zweitem Mal.");
calState = ERROR;
calStep = 0;
break;
}
lastSample = raw;
lastChangeTime = now;
calStep = 5;
Serial.print("CAL: Step 4 OK. secondPressed=");
Serial.println(secondPressed);
Serial.println("CAL: Step 5 - Pedal LOSLASSEN und still halten (Finalisierung).");
}
}
calState = RUNNING;
break;
}
case 5:
{ // 5. Final loslassen & stabil -> finalisieren
// Ziel-Release-Punkt aus den beiden Messungen mitteln
int relTarget = (firstReleased + secondReleased) / 2;
// Sind wir schon nah genug an der Release-Position?
if (abs(raw - relTarget) > MOVE_DELTA)
{
// Noch in Bewegung oder noch gedrückt -> Timer immer wieder zurücksetzen
lastSample = raw;
lastChangeTime = now;
calState = RUNNING;
break;
}
// In der Nähe -> jetzt Stabilität prüfen
if (diff > STABLE_THRESHOLD)
{
lastSample = raw;
lastChangeTime = now;
}
if (now - lastChangeTime > STABLE_TIME)
{
int relAvg = relTarget;
int pressAvg = (firstPressed + secondPressed) / 2;
int low = pressAvg;
int high = relAvg;
// Falls Richtung "invertiert" ist, drehen
if (high < low)
{
int tmp = low;
@@ -173,30 +361,46 @@ CalibrationState PedalController::autoCalibrate(bool reset)
high = tmp;
}
// Bereich zu klein, Kalibrierung fehlgeschlagen
if (high - low < MIN_RANGE)
{
Serial.println("CAL: FEHLER - Gesamtspanne zu klein. Pedalweg reicht nicht aus.");
config.calibrated = false;
calState = ERROR;
calStep = 0;
break;
}
config.minRaw = low;
config.maxRaw = high;
// Sicherheitsmargen
low += SAFETY_MARGIN_LOW;
high -= SAFETY_MARGIN_HIGH;
if (high <= low)
{
Serial.println("CAL: FEHLER - Sicherheitsmargen zerstören die Spannweite.");
config.calibrated = false;
calState = ERROR;
calStep = 0;
break;
}
config.minRaw = low; // gedrückt (niedriger)
config.maxRaw = high; // losgelassen (höher)
config.calibrated = true;
Serial.print("CAL: SUCCESS - minRaw=");
Serial.print(config.minRaw);
Serial.print(" maxRaw=");
Serial.println(config.maxRaw);
calState = SUCCESS;
calStep = 0;
}
else
{
calState = RUNNING;
}
break;
}
default:
// Fallback, falls irgendetwas entgleist
Serial.println("CAL: INTERNAL ERROR - unerwarteter Step.");
calState = ERROR;
calStep = 0;
break;
@@ -212,47 +416,33 @@ CalibrationState PedalController::getStatus() const
int PedalController::getPedal()
{
// 1) Digital-Sanity: Wenn DO "nicht gedrückt" meldet → 0
if (useDigital)
{
int d = digitalRead(digitalPin);
if ((uint8_t)d == digitalNotPressedLevel)
{
// Optional: Filter sanft Richtung "unten" ziehen
// aber sicherheitshalber nichts fahren
return 0;
}
}
// 2) Ohne gültige Kalibrierung: kein Pedal
// Ohne gültige Kalibrierung: kein Pedal
if (!config.calibrated)
{
return 0;
}
int raw = readRaw();
int raw = applySmoothing(readRaw());
int minR = config.minRaw;
int maxR = config.maxRaw;
int minR = config.minRaw; // gedrückt → niedriger
int maxR = config.maxRaw; // losgelassen → höher
// Defekter Config-Bereich, zur Sicherheit 0
if (maxR <= minR + 1)
{
return 0;
}
// Clamping
if (raw < minR)
raw = minR;
if (raw > maxR)
raw = maxR;
long span = (long)maxR - (long)minR;
long scaled = (long)(raw - minR) * 100L / span;
// WICHTIG: invertiertes Mapping, weil "hoch = losgelassen"
long scaled = (long)(maxR - raw) * 100L / span;
// kleine Deadzone unten
if (scaled < 5)
// kleine Deadzone unten (Pedal "nicht gedrückt")
if (scaled < 3)
scaled = 0;
if (scaled > 100)
scaled = 100;
@@ -269,7 +459,6 @@ void PedalController::setConfig(const PedalConfig &newConfig)
{
config = newConfig;
// FilterStrength sanitizen
if (config.filterStrength < MIN_FS || config.filterStrength > MAX_FS)
{
config.filterStrength = DEFAULT_FS;
@@ -302,47 +491,70 @@ void PedalController::loadDefaults()
lastSample = 0;
minSeen = 1023;
maxSeen = 0;
lastStatusPrint = 0;
firstReleased = 0;
firstPressed = 0;
secondReleased = 0;
secondPressed = 0;
step2MovedEnough = false;
step3NearReleased = false;
step4NearPressed = false;
}
void PedalController::printStatus(int raw)
{
if (!(debugFlag && *debugFlag))
{
return;
}
unsigned long now = millis();
if (now - lastStatusPrint < 1000)
return; // nur jede Sekunde
lastStatusPrint = now;
int digitalState = -1;
if (useDigital)
{
digitalState = digitalRead(digitalPin);
}
Serial.print("[CAL] Step=");
Serial.print(calStep);
Serial.print(" RAW=");
Serial.print(raw);
Serial.print(" Filtered=");
Serial.print(" Smooth=");
Serial.print(filteredValue);
Serial.print(" DO-State=");
if (useDigital)
Serial.print(digitalState);
{
Serial.print(digitalRead(digitalPin));
}
else
{
Serial.print("N/A");
}
Serial.print(" minSeen=");
Serial.print(minSeen);
Serial.print(" maxSeen=");
Serial.print(maxSeen);
Serial.print(" firstRel=");
Serial.print(firstReleased);
Serial.print(" firstPress=");
Serial.print(firstPressed);
Serial.print(" secondRel=");
Serial.print(secondReleased);
Serial.print(" secondPress=");
Serial.print(secondPressed);
Serial.println();
}
// Debug-Methoden
int PedalController::debugGetRaw()
{
return readRaw();
}
int PedalController::debugGetSmooth()
{
return applySmoothing(readRaw());
}
int PedalController::debugGetDO()
{
if (!useDigital)

35
src/PedalController.h
View File

@@ -5,7 +5,8 @@
#include <Arduino.h>
// Calibration state enumeration
enum CalibrationState {
enum CalibrationState
{
IDLE,
RUNNING,
SUCCESS,
@@ -13,21 +14,21 @@ enum CalibrationState {
};
// Pedal configuration structure
struct PedalConfig {
struct PedalConfig
{
int16_t minRaw;
int16_t maxRaw;
bool calibrated;
uint8_t filterStrength; // 1 = kein Filter, >1 = gleitender Mittelwert
};
class PedalController {
class PedalController
{
public:
// analogPin = ADC-Eingang
// digitalPin = optionaler Digital-Eingang des Hall-Boards (DO), -1 wenn keiner
// digitalNotPressedLevel = 0 (LOW) oder 1 (HIGH), welches Level "nicht gedrückt" bedeutet
explicit PedalController(int analogPin,
int digitalPin = -1,
uint8_t digitalNotPressedLevel = LOW);
uint8_t digitalNotPressedLevel = LOW,
bool *debugFlag = nullptr);
// Startet / läuft Auto-Kalibrierung
CalibrationState autoCalibrate(bool reset);
@@ -39,15 +40,19 @@ public:
PedalConfig getConfig() const;
void setConfig(const PedalConfig &newConfig);
void loadDefaults();
// Debug-Helfer
int debugGetRaw();
int debugGetSmooth();
int debugGetDO();
private:
int analogPin;
int digitalPin;
bool useDigital;
uint8_t digitalNotPressedLevel; // 0 oder 1
uint8_t digitalNotPressedLevel; // nur Debug
bool *debugFlag;
PedalConfig config;
int32_t filteredValue;
@@ -61,7 +66,17 @@ private:
int maxSeen;
unsigned long lastStatusPrint;
int readRaw(); // roher ADC-Wert (mit Glättung)
// Vier Referenzpunkte aus dem Ablauf
int firstReleased;
int firstPressed;
int secondReleased;
int secondPressed;
bool step2MovedEnough;
bool step3NearReleased;
bool step4NearPressed;
int readRaw();
int applySmoothing(int raw);
void printStatus(int raw);
};

19
src/main.cpp
View File

@@ -17,12 +17,14 @@ const int encoderPinA = 3; // Encoder pin A
const int encoderPinB = 4; // Encoder pin B
const int encoderButton = 5; // Encoder button
const int motorPin = 9; // Motor PWM pin
const int pedalPinAnalog = A1; // Pedal analog Input pin
const int pedalPinAnalog = A0; // Pedal analog Input pin
const int pedalPinDigital = 8; // Pedal digital Input pin
bool debug = false;
// Global objects
MotorController motor(motorPin); // Motor control object
PedalController pedal(pedalPinAnalog, pedalPinDigital, HIGH); // Pedal input object
MotorController motor(motorPin, &debug);
PedalController pedal(pedalPinAnalog, pedalPinDigital, LOW, &debug);
// Combined settings structure
struct Settings
@@ -145,11 +147,14 @@ void loop()
lastSensorPrint = now;
int raw = pedal.debugGetRaw();
int smooth = pedal.debugGetSmooth();
int pedalValue = pedal.getPedal();
int doState = pedal.debugGetDO();
Serial.print("[SENSOR] RAW=");
Serial.print(raw);
Serial.print(" Smooth=");
Serial.print(smooth);
Serial.print(" Pedal%=");
Serial.print(pedalValue);
Serial.print(" DO=");
@@ -261,6 +266,11 @@ void handleSerialInput()
char input = Serial.read();
switch (input)
{
case 'd':
debug = !debug;
Serial.print("Debug: ");
Serial.println(debug ? "ON" : "OFF");
break;
case 'c':
Serial.println("Starting auto-calibration...");
pedal.autoCalibrate(true);
@@ -334,6 +344,7 @@ void handleSerialInput()
void printHelp()
{
Serial.println("Available commands:");
Serial.println("d - Toggle Calibration debug output");
Serial.println("c - Start auto-calibration");
Serial.println("x - Stop auto-calibration or serial control");
Serial.println("r - Load defaults");