From 22f6e9bbd2ede988631bbef9f68cab9679943044 Mon Sep 17 00:00:00 2001 From: Musab Erdem Date: Thu, 18 Jul 2024 12:00:22 +0200 Subject: [PATCH] Komplette Umstellung auf v2 clean --- main/main.ino | 692 ++++++++++++++++++++------------ main_v2/main_v2.ino | 587 --------------------------- main_v2_clean/main_v2_clean.ino | 469 ---------------------- main_v3/main_v3.ino | 623 ---------------------------- main_v4/main_v4.ino | 57 --- 5 files changed, 434 insertions(+), 1994 deletions(-) delete mode 100644 main_v2/main_v2.ino delete mode 100644 main_v2_clean/main_v2_clean.ino delete mode 100644 main_v3/main_v3.ino delete mode 100644 main_v4/main_v4.ino diff --git a/main/main.ino b/main/main.ino index 59f1ecf..c1f6b76 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1,293 +1,469 @@ #include +#include +#include +#include +#include +#include -PRG_342 prg = PRG_342(); +#define DEBUG_PRINT_TIME 500 +#define SPEEDCHECK_TIME 200 +#define PID_DELAY 0 +#define LOG_TIMER_DELAY 100 +#define MEAS_DELAY 0 +#define K_FAKTOR 1.0995380532183 +#define OFFSET 0 +#define ALPHA 0.0008 +#define N_SAMPLES 100 +#define BLOCK_DETECTION_VALUE setpoint +#define ROTATION_DETECTION_VALUE 100 -int angle = 0; -int lastAngle = 0; -int actTorque = 0; -int setpoint = 0; -double lastOutput; +#define _getTorque abs(prg.getTorque(false, ALPHA)) +#define _getAngle prg.getAngle() + +struct Timer { + unsigned long startMillis; + unsigned long interval; + bool active; +}; + +void waitStart(Timer &timer, unsigned long interval) { + timer.startMillis = millis(); + timer.interval = interval; + timer.active = true; +} + +bool waitIsOver(Timer &timer) { + if (timer.active && (millis() - timer.startMillis >= timer.interval)) { + timer.active = false; + return true; + } + return false; +} + +Timer debug_timer; +Timer pid_timer; +Timer speedcheck_timer; +Timer log_timer; +Timer meas_timer; + +PRG_342 prg = PRG_342(K_FAKTOR); +Datalogger datalogger = Datalogger(); +PID myPID(0.0, 0.0, 0.0); + +//Messung / Logging +bool LOGGING = false; + +// Deklaration der Variablen für die Steuerung und Messung +int angle; +int lastAnglePID; +int lastAngle; +int actTorque; +//int actTorqueArray[100]; +float filteredTorque = 0; // gefilterter Wert +float filteredSetpoint = 0; +float setpoint; +int oldSetpoint; +static int lastOutput; +float manVolt; +unsigned char CW; +unsigned char Richtungswechsel; +float RotSpeed; +unsigned char BLOCK; +unsigned char BLOCK_SOLVED; + +bool In_Src_Sw = false; float Kp = 0.0; -float Ki = 0.0; +float Ki = 0.1; float Kd = 0.0; -int Pout; -int Iout; -int Dout; -int error; +/*float Kp = 1.0; +float Ki = 0.333; +float Kd = 1.0;*/ -bool OFFSET_ON = true; -bool manOut = false; +float WinkelArray[181]; -bool PID = false; -unsigned char PID_DELAY = 1; //in ms -unsigned int Imax = 217; //in mA -bool PRINT_DEBUG = false; -bool DEBUG_MODE = false; +// Steuervariablen +bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren +bool Pon = true; // PID-Regelung ein-/ausschalten +bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten -// Define variables for PID calculations -float integral = 0; -int derivative; -int previousError = 0; -unsigned long lastTime = 0; -unsigned long currentTime; -unsigned long timeChange; - -unsigned long timer1; -unsigned long timer2; -unsigned long PIDtimer; - -float RotSpeed; -unsigned int MilliAmpPerSecond = 160; -unsigned int maxChange; - -char serBuffer[4]; +// Initialisierung der verschiedenen Timer +unsigned long logtime; void setup() { - analogWriteResolution(12); // Set the resolution to 12 bits (0-4095) - analogReadResolution(12); + setAnalogResolution(12); - Serial.begin(115200); - prg.safeShutdown(5); + waitStart(debug_timer, DEBUG_PRINT_TIME); + waitStart(pid_timer, PID_DELAY); + waitStart(speedcheck_timer, SPEEDCHECK_TIME); + waitStart(log_timer, LOG_TIMER_DELAY); + waitStart(meas_timer, MEAS_DELAY); - timer1 = timer2 = PIDtimer = millis(); - angle = prg.getAngle(); - lastAngle = angle; + myPID.setOutputLimits(0, 4095); + myPID.setSampleTime(PID_DELAY); + myPID.setTunings(Kp, Ki, Kd); + + //prg.safeShutdown(3); + + UPDATE_VALUES(); + /*for (int i = 0; i < N_SAMPLES; i++) { + actTorqueArray[i] = actTorque; + }*/ + filteredTorque = actTorque; + + lastAngle = lastAnglePID = angle; + + for (int i = 0; i < 181; i++) { + WinkelArray[i] = -1; + } + + Serial.begin(19200); // Beginne serielle Kommunikation +} + +void loop() { + /*if (waitIsOver(meas_timer)) { + UPDATE_VALUES(); + for (int i = 0; i < N_SAMPLES - 1; i++) { + actTorqueArray[i] = actTorqueArray[i + 1]; + } + actTorqueArray[N_SAMPLES - 1] = actTorque; + actTorque = 0; + for (int j = 0; j < N_SAMPLES; j++) { + actTorque += actTorqueArray[j]; + } + actTorque = (float)actTorque / N_SAMPLES; + waitStart(meas_timer, MEAS_DELAY); + }*/ + UPDATE_VALUES(); + filteredTorque = exponentialFilter(actTorque, filteredTorque, ALPHA); // Verwenden Sie einen geeigneten Alpha-Wert + + COMMUNICATION_HANDLER(); + + if (DEBUG_MODE) { + if (waitIsOver(debug_timer)) { + DEBUG_PRINTER(); + waitStart(debug_timer, DEBUG_PRINT_TIME); + } + } + + if (waitIsOver(speedcheck_timer)) { + SPEEDCHECK(); + + BLOCK = BLOCK_DETECTION(abs(actTorque), abs(RotSpeed)); + + /*if (BLOCK == 0) { + if (BLOCK_SOLVED == 1) { + setpoint = oldSetpoint; + BLOCK_SOLVED = 0; + } else { + oldSetpoint = setpoint; + } + } else if (BLOCK == 1) { + if (BLOCK_SOLVED == 0) { + setpoint = 0; + BLOCK_SOLVED = 1; + } + }*/ + + waitStart(speedcheck_timer, SPEEDCHECK_TIME); + } + + + if (Richtungswechsel) { + myPID.reset(); + Richtungswechsel = 0; + } + + if (!BLOCK) { + // Aktualisieren des Setpoints basierend auf dem Winkel + int index = (float)angle * 0.002; // 0.5 Grad pro Index + if (index >= 0 && index < 181) { + setpoint = WinkelArray[index]; + } + if (In_Src_Sw) { + setpoint = float(prg.getSetPoint(1)) * 5.0; + } + } + + /*if (abs(setpoint) - abs(actTorque) <= 1000) { + myPID.setTunings(Kp, Ki, Kd); + } else { + myPID.setTunings(Kp, Ki, Kd); + }*/ + myPID.setTunings(Kp, Ki, Kd); + + if (lastAnglePID != angle && Pon && !manOut && ((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) { + unsigned int output = myPID.compute(setpoint, actTorque); + prg.setOutput(output); + prg.ActOut = output; + } else if (abs(actTorque) < 1000) { + myPID.reset(); + } + lastAnglePID = angle; + + if (LOGGING) { + if (waitIsOver(log_timer)) { + sendDataToDatalogger(); + waitStart(log_timer, LOG_TIMER_DELAY); + } + } + prg.isRunning == false; +} + +void interpolateWinkelArray() { + int lastSetIndex = -1; + + for (int i = 0; i < 180; i++) { + int start = i; + int k = i + 1; + while (WinkelArray[k] == -1) { + k++; + } + int end = k; + float startTorque = WinkelArray[start]; + float endTorque = WinkelArray[end]; + + for (int j = start + 1; j < end; j++) { + float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start); + WinkelArray[j] = interpolatedTorque; + } + } + + /*for (int i = 0; i < 181; i++) { + if (WinkelArray[i] != 0 || i == 0) { // Sicherstellen, dass der Startwert berücksichtigt wird + if (lastSetIndex != -1) { + int start = lastSetIndex; + int end = i; + float startTorque = WinkelArray[start]; + float endTorque = WinkelArray[end]; + + for (int j = start + 1; j < end; j++) { + float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start); + WinkelArray[j] = interpolatedTorque; + } + } + lastSetIndex = i; + } + }*/ +} + + + +float exponentialFilter(float currentValue, float previousFilteredValue, float alpha) { + return alpha * currentValue + (1.0 - alpha) * previousFilteredValue; +} + +void sendDataToDatalogger() { + datalogger.addData((float)((float)millis() - (float)logtime) / 1000.0, (float)angle / 1000.0, setpoint / 1000.0, actTorque / 1000.0, (float)prg.getOutVolt() / 1000.0, RotSpeed); +} + +unsigned char BLOCK_DETECTION(unsigned int torque, float speed) { + if (torque >= BLOCK_DETECTION_VALUE && speed < ROTATION_DETECTION_VALUE) { + return 1; + } else { + return 0; + } +} + +void setAnalogResolution(unsigned char res) { + analogWriteResolution(res); + analogReadResolution(res); +} + +void UPDATE_VALUES() { + angle = _getAngle; + actTorque = _getTorque; + + prg.updateActOut(actTorque); } void COMMUNICATION_HANDLER() { if (Serial.available() > 0) { - delay(10); - unsigned int SerIn = Serial.read(); - switch (SerIn) { - case 'a': - Serial.print((float)prg.getAngle() / 1000, 1); - Serial.print(";"); - Serial.println((float)prg.getTorque(false, 10) / 1000, 1); - break; - - case 'e': - prg.safeShutdown(10); - break; - - case 't': - prg.tareTorque(); - prg.tareAngle(); - break; - - case 'o': - OFFSET_ON = !OFFSET_ON; - break; - - case 'p': - delay(10); - Serial.readBytes(serBuffer, 4); - Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - break; - - case 'i': - delay(10); - Serial.readBytes(serBuffer, 4); - Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - break; - - case 'd': - delay(10); - Serial.readBytes(serBuffer, 4); - Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - break; - - case 's': - delay(10); - Serial.readBytes(serBuffer, 4); - setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("setpoint: "); - Serial.println(setpoint); - break; - - case 'm': - manOut = !manOut; - break; - - case 'v': - if (manOut) { - Serial.readBytes(serBuffer, 4); - unsigned int volt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - prg.setOutput(40.95 * volt, false); - } - break; - - case 'x': - PID = !PID; - error = 0; - previousError = 0; - integral = 0; - break; - - case 'r': - error = 0; - previousError = 0; - integral = 0; - derivative = 0; - Pout = 0; - Iout = 0; - Dout = 0; - break; - - case 'f': - prg.DynDecog(prg.ActOut); - break; - - case 'k': - delay(10); - Serial.readBytes(serBuffer, 4); - MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); - Serial.print("Max Milliampere per second: "); - Serial.println(MilliAmpPerSecond); - break; - - case 'b': - DEBUG_MODE = !DEBUG_MODE; - break; - - default: - Serial.println(SerIn); - break; - } + String command = receiveString(); + parseCommand(command); + Serial.read(); } } -void PID_CONTROL() { - // Get current time - currentTime = millis(); - timeChange = currentTime - lastTime; - lastTime = currentTime; - - // Read the current value from the sensor - actTorque = abs(prg.getTorque(true, 10)); - angle = abs(prg.getAngle()); - - // Calculate error - error = setpoint - actTorque; - - // Proportional term - Pout = Kp * error; - - // Integral term - integral += error * (float)timeChange / 1000.0; // Convert timeChange to seconds - // Limit the integral term to prevent windup - integral = constrain(integral, -100000, 100000); - Iout = Ki * integral; - - // Derivative term - derivative = (error - previousError) / ((float)timeChange / 1000.0); // Convert timeChange to seconds - Dout = Kd * derivative; - - // Calculate total output - int output = Pout + Iout + Dout; - - // Constrain output - output = constrain(output, 10, 4095); - - // Implement a smooth change to avoid sudden jumps - static int lastOutput = 0; - //int maxChange = 10; // Max change per loop iteration (adjust as needed) - maxChange = (MilliAmpPerSecond * 4095 * PID_DELAY) / (Imax * 1000); - if (abs(output - lastOutput) > maxChange) { - if (output > lastOutput) { - output = lastOutput + maxChange; - } else { - output = lastOutput - maxChange; - } - } - - // Write the PID output to the actuator - prg.setOutput(output, false); - lastOutput = output; - - // Update previous error - previousError = error; -} - void SPEEDCHECK() { - if (millis() - timer2 >= 200) { - timer2 = millis(); - int deltaAngle = prg.getAngle() - lastAngle; - RotSpeed = deltaAngle * 5.0; // Convert to speed per second - lastAngle = prg.getAngle(); - } -} + int deltaAngle = angle - lastAngle; + RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde + lastAngle = angle; -void DEBUG_TIMER() { - if (millis() - timer1 >= 500) { - timer1 = millis(); - PRINT_DEBUG = true; - } else { - PRINT_DEBUG = false; + if (RotSpeed > 0) { + if (CW == 0) { + Richtungswechsel = 1; + } + CW = 1; + } else if (RotSpeed < 0) { + if (CW == 1) { + Richtungswechsel = 1; + } + CW = 0; } } void DEBUG_PRINTER() { - if (PRINT_DEBUG) { - Serial.println("####################"); - Serial.print("Winkel: "); - Serial.println((float)prg.getAngle() / 1000, 3); - Serial.print("Drehmoment: "); - Serial.println((float)prg.getTorque() / 1000, 3); - Serial.print("Geschwindigkeit: "); - Serial.println(RotSpeed); - Serial.print("Ausgangsspannung: "); - Serial.println((float)prg.getOutVolt() / 1000, 3); - Serial.print("Setpoint: "); - Serial.println((float)setpoint / 1000, 3); - Serial.print("Error: "); - Serial.println((float)error / 1000, 3); - Serial.print("Kp: "); - Serial.println(Kp, 3); - Serial.print("Ki: "); - Serial.println(Ki, 3); - Serial.print("Kd: "); - Serial.println(Kd, 3); - Serial.print("Pout: "); - Serial.println(Pout); - Serial.print("Iout: "); - Serial.println(Iout); - Serial.print("Dout: "); - Serial.println(Dout); - Serial.print("Integral: "); - Serial.println(integral); - Serial.print("PID: "); - Serial.println(PID); - Serial.print("Max mA / s (SET): "); - Serial.println(MilliAmpPerSecond); - Serial.print("Max mA / s (ACT): "); - Serial.println((Imax * maxChange * 1000) / (4095 * PID_DELAY)); - Serial.print("maxChange: "); - Serial.println(maxChange); - Serial.print("isRunning: "); - Serial.println(prg.isRunning); - Serial.println("####################"); - } + // Debug-Informationen ausgeben + Serial.println("####################"); + Serial.print("Winkel: "); + Serial.println((float)angle / 1000, 3); + Serial.print("Drehmoment: "); + Serial.println((float)actTorque / 1000, 3); + Serial.print("Geschwindigkeit: "); + Serial.println(RotSpeed); + Serial.print("Drehrichtung: "); + Serial.println(CW); + Serial.print("Ausgangsspannung: "); + Serial.println((float)prg.getOutVolt() / 1000, 3); + Serial.print("Setpoint: "); + Serial.println((float)setpoint / 1000, 3); + Serial.print("Kp: "); + Serial.println(Kp, 3); + Serial.print("Ki: "); + Serial.println(Ki, 3); + Serial.print("Kd: "); + Serial.println(Kd, 3); + Serial.print("output: "); + Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); + Serial.print("Pon: "); + Serial.println(Pon); + Serial.print("manOut: "); + Serial.println(manOut); + Serial.print("manVolt: "); + Serial.println(manVolt); + Serial.print("LOGGING: "); + Serial.println(LOGGING); + Serial.println("####################"); } -void loop() { - if (DEBUG_MODE) { - DEBUG_TIMER(); - DEBUG_PRINTER(); + +String receiveString() { + String received = ""; + unsigned long startTime = millis(); + byte inByte[1]; + while ((char)inByte[0] != '\n' || Serial.available() > 0) { + //c = Serial.read(); + Serial.readBytes(inByte, 1); + received += (char)inByte[0]; } - COMMUNICATION_HANDLER(); - SPEEDCHECK(); + //Serial.println(received); + return received; +} - if (millis() - PIDtimer >= PID_DELAY) { // Small delay for stability - PIDtimer = millis(); - - if ((PID && !manOut && abs(prg.getTorque()) >= 2000 && abs(RotSpeed) >= 400) || (abs(prg.getTorque()) > setpoint)) { - PID_CONTROL(); +void parseCommand(const String &command) { + if (command[0] == 'p') { + float tempKp; + setParameter(command, 'Kp', tempKp); + Kp = tempKp; + myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings + datalogger.addConfig("Kp", String(Kp)); // Konvertierung zu String + } else if (command[0] == 'i') { + float tempKi; + setParameter(command, 'Ki', tempKi); + Ki = tempKi; + myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings + datalogger.addConfig("Ki", String(Ki)); // Konvertierung zu String + } else if (command[0] == 'd') { + float tempKd; + setParameter(command, 'Kd', tempKd); + Kd = tempKd; + myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings + datalogger.addConfig("Kd", String(Kd)); // Konvertierung zu String + } else if (command[0] == 'a') { + Serial.print(angle); + Serial.print(";"); + Serial.print(actTorque); + Serial.print(";"); + Serial.print(prg.getSetPoint(1)); + Serial.print(";"); + Serial.print(setpoint); + Serial.print(";"); + Serial.println(prg.ActOut); + } else if (command[0] == 'b') { + DEBUG_MODE = !DEBUG_MODE; + } else if (command[0] == 'e') { + prg.safeShutdown(5); + } + if (command[0] == 'f') { + prg.DynDecog(prg.ActOut); + } else if (command[0] == 'l') { + LOGGING = !LOGGING; + if (LOGGING) { + if (CW) { + datalogger.addConfig("Drehrichtung", "CW"); + } else { + datalogger.addConfig("Drehrichtung", "CCW"); + } + logtime = millis(); + waitStart(log_timer, LOG_TIMER_DELAY); + } else { + datalogger.logToSerial(); + datalogger.clear(); } + } else if (command[0] == 'm') { + manOut = !manOut; + } else if (command[0] == 'r') { + myPID.reset(); + } else if (command[0] == 's') { + setParameter(command, 's', setpoint); + for (int i = 0; i < 180; i++) { + WinkelArray[i] = setpoint; + } + } else if (command[0] == 'S') { + In_Src_Sw = !In_Src_Sw; + } else if (command[0] == 't') { + prg.tareTorque(); + } else if (command.startsWith("u") && command.endsWith("u\n")) { + Serial.println(command); // Debug-Ausgabe + for (int i = 0; i < 181; i++) { + WinkelArray[i] = -1; + } + String data = command.substring(1, command.length() - 1); + int lastIndex = 0; + while (lastIndex != -1) { + int index = data.indexOf(';', lastIndex); + String pair = (index == -1) ? data.substring(lastIndex) : data.substring(lastIndex, index); + int commaIndex = pair.indexOf(','); + if (commaIndex != -1) { + int angleIndex = pair.substring(0, commaIndex).toInt(); + float torqueValue = pair.substring(commaIndex + 1).toFloat(); + if (angleIndex >= 0 && angleIndex < 181) { + WinkelArray[angleIndex] = torqueValue; + Serial.print("Set WinkelArray["); + Serial.print(angleIndex); + Serial.print("] to "); + Serial.println(torqueValue); + } + } + lastIndex = (index == -1) ? -1 : index + 1; + } + interpolateWinkelArray(); + } else if (command[0] == 'v') { + if (manOut) { + setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert + manVolt = manVolt * 0.1575; + prg.ActOut = manVolt; + analogWrite(prg.TORQUE_OUT_PIN, manVolt); + } + } else if (command[0] == 'w') { + prg.tareAngle(); + } else if (command[0] == 'x') { + Pon = !Pon; + myPID.reset(); } - - prg.updateActOut(); - prg.isRunning = false; } + + +void setParameter(const String &command, char prefix, float ¶meter) { + int commaIndex = command.indexOf('.'); + if (commaIndex > 1) { + String paramString = command.substring(1, commaIndex) + "." + command.substring(commaIndex + 1); + parameter = paramString.toFloat(); // Konvertierung innerhalb der Funktion + /*Serial.print(prefix); + Serial.print(" gesetzt auf: "); + Serial.println(parameter, 6); // Sechs Dezimalstellen anzeigen*/ + } +} \ No newline at end of file diff --git a/main_v2/main_v2.ino b/main_v2/main_v2.ino deleted file mode 100644 index 7403613..0000000 --- a/main_v2/main_v2.ino +++ /dev/null @@ -1,587 +0,0 @@ -#include -#define MEAS_SAMPLES 4000 -#define MEAS_TIME 100 -#define K_FAKTOR 1.0995380532183 - -// Initialisiere die PRG_342-Klasse -PRG_342 prg = PRG_342(K_FAKTOR); - -//Messung / Logging -bool LOGGING = false; -int measTorque[MEAS_SAMPLES]; -int measAngle[MEAS_SAMPLES]; -unsigned int measSetpoint[MEAS_SAMPLES]; -unsigned int measMillis[MEAS_SAMPLES]; -unsigned int measVolt[MEAS_SAMPLES]; -unsigned int logCounter; -unsigned long timer; -unsigned long diff; -float ALPHA = 0.0008; - -// Deklaration der Variablen für die Steuerung und Messung -int angle = 0; // Aktueller Winkel -int lastAngle = 0; // Letzter gemessener Winkel -int actTorque = 0; // Aktuelles Drehmoment -int setpoint = 0; // Sollwert für Drehmoment -float tolerance = 0.0; -//int tolerance = 500; -float lowerBound; -float upperBound; -static int lastOutput = 0; -unsigned int manVolt = 0; -unsigned char CW; -unsigned char Richtungswechsel; - -// PID-Reglerparameter -//float Kp = 5.15; -//float Ki = 0.02; -//float Kd = 2273.47; -float Kp = 0; -float Ki = 0.010; -float Kd = 0; -float I_ALPHA = 0.0; -float I_old = 0; -int Pout; -int Iout; -int Dout; - -int error; // Fehler zwischen Soll- und Istwert (Regeldifferenz e) - -// Steuervariablen -bool OFFSET_ON = true; -bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren -bool PID = false; // PID-Regelung ein-/ausschalten -unsigned char PID_DELAY = 0; // Verzögerung zwischen PID-Regelungen in ms -unsigned int Imax = 217; // Maximalstrom in mA -bool PRINT_DEBUG = true; // Debug-Ausgaben ein-/ausschalten -bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten -bool ARDUINO_PLOTTER = false; - -// Variablen für PID-Berechnungen -float integral = 0; // Integralterm -int derivative; // Differenzialterm -int previousError = 0; // Vorheriger Fehler -unsigned long lastTime = 0; -unsigned long currentTime; -unsigned long timeChange; - -// Initialisierung der verschiedenen Timer -unsigned long timer1; -unsigned long timer2; -unsigned long PIDtimer; -unsigned long logTimer; - -float RotSpeed; // Rotationsgeschwindigkeit -unsigned int MilliAmpPerSecond = 25; // Milliampere pro Sekunde -unsigned int maxChange; // Maximale Änderung pro Schleifendurchlauf - -char serBuffer[4]; // Buffer für serielle Eingaben - -void setup() { - - for (int i = 0; i < MEAS_SAMPLES; i++) { - measTorque[i - 1] = 0; - measAngle[i - 1] = 0; - measSetpoint[i - 1] = 0; - measMillis[i - 1] = 0; - measVolt[i - 1] = 0; - } - - analogWriteResolution(12); - analogReadResolution(12); - - Serial.begin(115200); // Beginne serielle Kommunikation - - prg.safeShutdown(5); // Sicheres Herunterfahren des Ausgangs - //prg.AutoDecog(); - - // Initialisiere Timer mit der aktuellen Zeit - timer1 = timer2 = PIDtimer = logTimer = millis(); - - // Lese den initialen Winkel - angle = prg.getAngle(); - lastAngle = angle; -} - -void Datalogger() { - if (LOGGING) { - if (millis() - logTimer >= MEAS_TIME) { - logTimer = millis(); - logCounter++; - - measTorque[logCounter] = actTorque; - measAngle[logCounter] = angle; - measSetpoint[logCounter] = setpoint; - measMillis[logCounter] = logCounter * MEAS_TIME; - measVolt[logCounter] = prg.getOutVolt(); - } - } -} - -void loop() { - angle = prg.getAngle(); - actTorque = prg.getTorque(true, ALPHA); - // Aktualisiere den analogen Ausgang für den aktuellen Drehmoment - prg.updateActOut(actTorque); - - // Prüfe, ob der Debug-Modus aktiviert ist - DEBUG_TIMER(); // Aktualisierung des Debug-Timers - if (DEBUG_MODE) { - DEBUG_PRINTER(); // Debug Informationen ausgeben - } - - if (ARDUINO_PLOTTER) { - Arduino_Plotter(); - } - - Datalogger(); - - // Verarbeitung der seriellen Schnittstelle - COMMUNICATION_HANDLER(); - - // Rotationsgeschwindigkeit aktualisieren - SPEEDCHECK(); - - - PID_CONTROL(); // Ausführung der PID-Regelung - - // setze isRunning zurück, damit es durch den Interrupt des Drehgebers wieder gesetzt werden kann - prg.isRunning = false; -} - - -void COMMUNICATION_HANDLER() { - // Verarbeite eingehende Befehle von der seriellen Schnittstelle - if (Serial.available() > 0) { - delay(10); - unsigned int SerIn = Serial.read(); - switch (SerIn) { - case 'a': - // Drehwinkel und Drehmoment ausgeben - Serial.print((float)angle / 1000, 1); - Serial.print(";"); - Serial.println((float)actTorque / 1000, 1); - break; - - case 'e': - // safeShutdown ausführen - prg.safeShutdown(10); - break; - - case 't': - // Drehmoment nullen - prg.tareTorque(); - break; - - case 'w': - // Winkel nullen - prg.tareAngle(); - break; - - case 'o': - // Offset für Drehmoment deaktivieren - OFFSET_ON = !OFFSET_ON; - break; - - // Einstellen der Parameter Kp, Ki und Kd - case 'p': - delay(10); - Serial.readBytes(serBuffer, 4); - Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Kp: "); - Serial.println(Kp); - break; - - case 'i': - delay(10); - Serial.readBytes(serBuffer, 4); - Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Ki: "); - Serial.println(Ki); - break; - - case 'd': - delay(10); - Serial.readBytes(serBuffer, 4); - Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Kd: "); - Serial.println(Kd); - break; - - case 'j': - delay(10); - Serial.readBytes(serBuffer, 4); - I_ALPHA = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("I_ALPHA: "); - Serial.println(I_ALPHA); - break; - - case 's': - // Soll-Wert Einstellung - delay(10); - Serial.readBytes(serBuffer, 4); - setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("setpoint: "); - Serial.println(setpoint); - break; - - case 'm': - // Spannungsausgabe auf manuell stellen - manOut = !manOut; - break; - - case 'v': - // Ausgangsspannung einstellen (manOut muss auf true sein) - if (manOut) { - Serial.readBytes(serBuffer, 4); - manVolt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); - //prg.setOutput((float)manVolt * 0.1575, false); - float outVal = (float)manVolt * 0.1575; - prg.ActOut = outVal; - analogWrite(prg.TORQUE_OUT_PIN, outVal); - } - break; - - case 'x': - // Regelung ein-/ausschalten - PID = !PID; - error = 0; - previousError = 0; - integral = 0; - break; - - case 'r': - // PID-Parameter zurücksetzen - error = 0; - previousError = 0; - integral = 0; - derivative = 0; - Pout = 0; - Iout = 0; - Dout = 0; - break; - - case 'f': - // Dynamic Decogging ausführen - prg.DynDecog(prg.ActOut); - break; - - case 'k': - // Einstellen des maximalen Stromanstiegs - delay(10); - Serial.readBytes(serBuffer, 4); - MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); - Serial.print("Max Milliampere per second: "); - Serial.println(MilliAmpPerSecond); - break; - - case 'b': - // DEBUG_MODE switch - DEBUG_MODE = !DEBUG_MODE; - break; - - case 'l': - LOGGING_HANDLER(); - break; - - case 'y': - // Automatic Measurement - timer = millis(); - diff = millis() - timer; - LOGGING_HANDLER(); - PID = true; - while (diff <= 150000) { - angle = prg.getAngle(); - actTorque = prg.getTorque(true, ALPHA); - prg.updateActOut(actTorque); - - Datalogger(); - - /*if (diff > 20000 && diff <= 50000) { - setpoint = 5000; - } - if (diff > 50000 && diff <= 80000) { - setpoint = 10000; - } - if (diff > 80000 && diff <= 110000) { - setpoint = 15000; - } - if (diff > 110000 && diff <= 140000) { - setpoint = 5000; - }*/ - - if (angle > 0 && angle <= 45000) { - setpoint = 10000; - } - if (angle > 45000 && angle <= 65000) { - setpoint = 15000; - } - if (angle > 65000 && angle <= 90000) { - setpoint = 10000; - } - - - SPEEDCHECK(); - - PID_CONTROL(); - - diff = millis() - timer; - - Serial.print("elapsed Time: "); - Serial.print((float)diff / 1000, 3); - Serial.print(" actual Angle: "); - Serial.println((float)angle / 1000, 3); - } - LOGGING_HANDLER(); - PID = false; - break; - - default: - // Unbekannte Befehle bearbeiten - Serial.print("Unbekannter Befehl: "); - Serial.println(SerIn); - break; - } - } -} - -void printData() { - Serial.print("Kp:"); - Serial.print(";"); - Serial.println(Kp, 3); - Serial.print("Ki:"); - Serial.print(";"); - Serial.println(Ki, 3); - Serial.print("Kd:"); - Serial.print(";"); - Serial.println(Kd, 3); - Serial.print("I_ALPHA:"); - Serial.print(";"); - Serial.println(I_ALPHA, 6); - Serial.print("Drehrichtung:"); - Serial.print(";"); - if (CW) { - Serial.println("CW"); - } else { - Serial.println("CCW"); - } - - Serial.println("Zeit in Sekunden;Winkel;Soll-Drehmoment;Ist-Drehmoment;Erregerspannung;Winkel in Grad;Soll-Drehmoment in Nm;Ist-Drehmoment in Nm;Erregerspannung in V"); - for (int i = 0; i < MEAS_SAMPLES; i++) { - if (measMillis[i] > 0) { - Serial.print((float)measMillis[i] / 1000.0, 2); - Serial.print(";"); - Serial.print(measAngle[i]); - Serial.print(";"); - Serial.print(measSetpoint[i]); - Serial.print(";"); - Serial.print(measTorque[i]); - Serial.print(";"); - Serial.print(measVolt[i]); - Serial.print(";"); - Serial.print(abs((float)measAngle[i] / 1000.0), 3); - Serial.print(";"); - Serial.print(abs((float)measSetpoint[i] / 1000.0), 3); - Serial.print(";"); - Serial.print(abs((float)measTorque[i] / 1000.0), 3); - Serial.print(";"); - Serial.println(abs((float)measVolt[i] / 1000.0), 3); - } - } - for (int i = 0; i < MEAS_SAMPLES; i++) { - measTorque[i - 1] = 0; - measAngle[i - 1] = 0; - measSetpoint[i - 1] = 0; - measMillis[i - 1] = 0; - measVolt[i - 1] = 0; - } -} - -void LOGGING_HANDLER() { - // LOGGING switch - LOGGING = !LOGGING; - if (LOGGING) { - logCounter = 0; - } else { - printData() - } -} - -void PID_CONTROL() { - lowerBound = (float)setpoint - ((float)setpoint * tolerance); - upperBound = (float)setpoint + ((float)setpoint * tolerance); - //lowerBound = (float)setpoint - tolerance; - //upperBound = (float)setpoint + tolerance; - // Timing des PID-Reglers - if (millis() - PIDtimer >= PID_DELAY) { - - if (Richtungswechsel) { - integral = 0; - derivative = 0; - Richtungswechsel = 0; - } - - PIDtimer = millis(); // Setze den Timer zurück - // Aktualisierung des Zeitstempels und Berechnung der vergangenen Zeit seit dem letzten Update - currentTime = millis(); // Lese die aktuelle Zeit in Millisekunden - timeChange = currentTime - lastTime; // Zeitdifferenz zum letzten Aufruf - lastTime = currentTime; // Setze den letzten Zeitstempel auf die aktuelle Zeit für den nächsten Durchlauf - - - // Überprüfe, ob die PID-Regelung aktiviert ist, keine manuelle Ausgabe erfolgt, - // das gemessene Drehmoment die Schwelle überschreitet und die Geschwindigkeit hoch genug ist - // ODER das gemessene Drehmoment den Sollwert übersteigt (Antrieb auf Block) - if (PID && !manOut && ((abs(actTorque) >= 5000 || abs(RotSpeed) >= 400) || (abs(actTorque) > setpoint)) && (abs(actTorque) < lowerBound || abs(actTorque) > upperBound)) { - - // Aktuelle Werte auslesen - //actTorque = abs(prg.getTorque(true, 100)); // Aktuelles Drehmoment - //angle = abs(prg.getAngle()); // Aktueller Winkel - - // Berechnung der Regeldifferenz - error = abs(setpoint) - abs(actTorque); // Differenz zwischen Sollwert und tatsächlich gemessenem Drehmoment - - // Berechnung des proportionalen Anteils der PID-Regelung - Pout = Kp * error; // Proportionaler Output basiert auf dem aktuellen Fehler und dem Proportional-Faktor Kp - /* - // Berechnung des integralen Anteils der PID-Regelung - integral += error * ((float)timeChange / 1000.0); // Addition des Fehlers über die Zeit (Integration des Fehlers) - integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden - Iout = Ki * integral; // Berechnung des integralen Output basierend auf dem Integral und dem Integral-Faktor Ki - */ - - // Berechnung des adaptiv gewichteten integralen Anteils - float weight = 1.0 + (1 - exp(-PHI_ALA * abs(error))); // Gewichtungsfaktor abhängig vom Fehler - integral += weight * error * ((float)timeChange / 1000.0); // Anpassung der Integration des Fehlers - integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden - Iout = Ki * integral; - - // Berechnung des differenziellen Anteils der PID-Regelung - derivative = (error - previousError) / ((float)timeChange / 1000.0); // Berechnung der Änderungsrate des Fehlers - Dout = Kd * derivative; // Differenzieller Output basiert auf der Änderungsrate des Fehlers und dem Differenzial-Faktor Kd - - // Berechnung des gesamten PID-Output - int output = Pout + Iout + Dout; // Summe der einzelnen PID-Komponenten - - // Begrenzung des Ausganges, um Cogging zu vermeiden - // Der Strom darf nicht auf 0 sinken, während die Hysteresebremse stillsteht - output = constrain(output, 0, 4095); - - // Begrenzung des Stromanstieges, um Cogging vorzubeugen - //maxChange = ((float)MilliAmpPerSecond * 4095.0 * (float)PID_DELAY) / ((float)Imax * 1000); // Umrechnung des eingegebenen Stromwertes / Sekunde in einen binären Wert, angepasst auf den Ausgang - /*maxChange = 1; - if (abs(output - lastOutput) > maxChange) { - if (output > lastOutput) { - output = lastOutput + maxChange; // Erhöht den Output - } else { - output = lastOutput - maxChange; // Verringert den Output - } - }*/ - - // Den Wert output am Ausgang (an die Hysteresebremse) anlegen - prg.setOutput(output, false); - lastOutput = output; // Aktualisiere den letzten Output-Wert - - // Aktualisierung des vorherigen Fehler - previousError = error; - } - } -} - - -void SPEEDCHECK() { - // Überprüfen der Rotationsgeschwindigkeit - if (millis() - timer2 >= 200) { - timer2 = millis(); - int deltaAngle = angle - lastAngle; - RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde - lastAngle = angle; - - if (RotSpeed > 0) { - if (CW == 0) { - Richtungswechsel = 1; - } - CW = 1; - } else if (RotSpeed < 0) { - if (CW == 1) { - Richtungswechsel = 1; - } - CW = 0; - } - } -} - -void DEBUG_TIMER() { - // Timer für Debug-Ausgaben - if (millis() - timer1 >= 500) { - timer1 = millis(); - PRINT_DEBUG = true; - } else { - PRINT_DEBUG = false; - } -} - -void DEBUG_PRINTER() { - // Debug-Informationen ausgeben - if (PRINT_DEBUG) { - Serial.println("####################"); - Serial.print("Winkel: "); - Serial.println((float)angle / 1000, 3); - Serial.print("Drehmoment: "); - Serial.println((float)actTorque / 1000, 3); - Serial.print("Geschwindigkeit: "); - Serial.println(RotSpeed); - Serial.print("Drehrichtung: "); - Serial.println(CW); - Serial.print("Ausgangsspannung: "); - Serial.println((float)prg.getOutVolt() / 1000, 3); - Serial.print("Setpoint: "); - Serial.println((float)setpoint / 1000, 3); - Serial.print("Error: "); - Serial.println((float)error / 1000, 3); - Serial.print("Kp: "); - Serial.println(Kp, 3); - Serial.print("Ki: "); - Serial.println(Ki, 3); - Serial.print("Kd: "); - Serial.println(Kd, 3); - Serial.print("I_ALPHA:"); - Serial.println(I_ALPHA); - Serial.print("Pout: "); - Serial.println(Pout); - Serial.print("Iout: "); - Serial.println(Iout); - Serial.print("Dout: "); - Serial.println(Dout); - Serial.print("output: "); - Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); - Serial.print("Integral: "); - Serial.println(integral); - Serial.print("PID: "); - Serial.println(PID); - Serial.print("manOut: "); - Serial.println(manOut); - Serial.print("manVolt: "); - Serial.println(manVolt); - Serial.print("Max mA / s (SET): "); - Serial.println(MilliAmpPerSecond); - Serial.print("Max mA / s (ACT): "); - Serial.println((Imax * maxChange * 1000) / (4095 * PID_DELAY)); - Serial.print("maxChange: "); - Serial.println(maxChange); - Serial.print("isRunning: "); - Serial.println(prg.isRunning); - Serial.print("LOGGING: "); - Serial.println(LOGGING); - Serial.println("####################"); - } -} - -void Arduino_Plotter() { - if (PRINT_DEBUG) { - Serial.print("Soll:"); - Serial.print(setpoint); - Serial.print(","); - Serial.print("Ist:"); - Serial.println(abs(actTorque)); - } -} diff --git a/main_v2_clean/main_v2_clean.ino b/main_v2_clean/main_v2_clean.ino deleted file mode 100644 index c1f6b76..0000000 --- a/main_v2_clean/main_v2_clean.ino +++ /dev/null @@ -1,469 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#define DEBUG_PRINT_TIME 500 -#define SPEEDCHECK_TIME 200 -#define PID_DELAY 0 -#define LOG_TIMER_DELAY 100 -#define MEAS_DELAY 0 -#define K_FAKTOR 1.0995380532183 -#define OFFSET 0 -#define ALPHA 0.0008 -#define N_SAMPLES 100 -#define BLOCK_DETECTION_VALUE setpoint -#define ROTATION_DETECTION_VALUE 100 - -#define _getTorque abs(prg.getTorque(false, ALPHA)) -#define _getAngle prg.getAngle() - -struct Timer { - unsigned long startMillis; - unsigned long interval; - bool active; -}; - -void waitStart(Timer &timer, unsigned long interval) { - timer.startMillis = millis(); - timer.interval = interval; - timer.active = true; -} - -bool waitIsOver(Timer &timer) { - if (timer.active && (millis() - timer.startMillis >= timer.interval)) { - timer.active = false; - return true; - } - return false; -} - -Timer debug_timer; -Timer pid_timer; -Timer speedcheck_timer; -Timer log_timer; -Timer meas_timer; - -PRG_342 prg = PRG_342(K_FAKTOR); -Datalogger datalogger = Datalogger(); -PID myPID(0.0, 0.0, 0.0); - -//Messung / Logging -bool LOGGING = false; - -// Deklaration der Variablen für die Steuerung und Messung -int angle; -int lastAnglePID; -int lastAngle; -int actTorque; -//int actTorqueArray[100]; -float filteredTorque = 0; // gefilterter Wert -float filteredSetpoint = 0; -float setpoint; -int oldSetpoint; -static int lastOutput; -float manVolt; -unsigned char CW; -unsigned char Richtungswechsel; -float RotSpeed; -unsigned char BLOCK; -unsigned char BLOCK_SOLVED; - -bool In_Src_Sw = false; - -float Kp = 0.0; -float Ki = 0.1; -float Kd = 0.0; - -/*float Kp = 1.0; -float Ki = 0.333; -float Kd = 1.0;*/ - -float WinkelArray[181]; - -// Steuervariablen -bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren -bool Pon = true; // PID-Regelung ein-/ausschalten -bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten - -// Initialisierung der verschiedenen Timer -unsigned long logtime; - -void setup() { - setAnalogResolution(12); - - - waitStart(debug_timer, DEBUG_PRINT_TIME); - waitStart(pid_timer, PID_DELAY); - waitStart(speedcheck_timer, SPEEDCHECK_TIME); - waitStart(log_timer, LOG_TIMER_DELAY); - waitStart(meas_timer, MEAS_DELAY); - - myPID.setOutputLimits(0, 4095); - myPID.setSampleTime(PID_DELAY); - myPID.setTunings(Kp, Ki, Kd); - - //prg.safeShutdown(3); - - UPDATE_VALUES(); - /*for (int i = 0; i < N_SAMPLES; i++) { - actTorqueArray[i] = actTorque; - }*/ - filteredTorque = actTorque; - - lastAngle = lastAnglePID = angle; - - for (int i = 0; i < 181; i++) { - WinkelArray[i] = -1; - } - - Serial.begin(19200); // Beginne serielle Kommunikation -} - -void loop() { - /*if (waitIsOver(meas_timer)) { - UPDATE_VALUES(); - for (int i = 0; i < N_SAMPLES - 1; i++) { - actTorqueArray[i] = actTorqueArray[i + 1]; - } - actTorqueArray[N_SAMPLES - 1] = actTorque; - actTorque = 0; - for (int j = 0; j < N_SAMPLES; j++) { - actTorque += actTorqueArray[j]; - } - actTorque = (float)actTorque / N_SAMPLES; - waitStart(meas_timer, MEAS_DELAY); - }*/ - UPDATE_VALUES(); - filteredTorque = exponentialFilter(actTorque, filteredTorque, ALPHA); // Verwenden Sie einen geeigneten Alpha-Wert - - COMMUNICATION_HANDLER(); - - if (DEBUG_MODE) { - if (waitIsOver(debug_timer)) { - DEBUG_PRINTER(); - waitStart(debug_timer, DEBUG_PRINT_TIME); - } - } - - if (waitIsOver(speedcheck_timer)) { - SPEEDCHECK(); - - BLOCK = BLOCK_DETECTION(abs(actTorque), abs(RotSpeed)); - - /*if (BLOCK == 0) { - if (BLOCK_SOLVED == 1) { - setpoint = oldSetpoint; - BLOCK_SOLVED = 0; - } else { - oldSetpoint = setpoint; - } - } else if (BLOCK == 1) { - if (BLOCK_SOLVED == 0) { - setpoint = 0; - BLOCK_SOLVED = 1; - } - }*/ - - waitStart(speedcheck_timer, SPEEDCHECK_TIME); - } - - - if (Richtungswechsel) { - myPID.reset(); - Richtungswechsel = 0; - } - - if (!BLOCK) { - // Aktualisieren des Setpoints basierend auf dem Winkel - int index = (float)angle * 0.002; // 0.5 Grad pro Index - if (index >= 0 && index < 181) { - setpoint = WinkelArray[index]; - } - if (In_Src_Sw) { - setpoint = float(prg.getSetPoint(1)) * 5.0; - } - } - - /*if (abs(setpoint) - abs(actTorque) <= 1000) { - myPID.setTunings(Kp, Ki, Kd); - } else { - myPID.setTunings(Kp, Ki, Kd); - }*/ - myPID.setTunings(Kp, Ki, Kd); - - if (lastAnglePID != angle && Pon && !manOut && ((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) { - unsigned int output = myPID.compute(setpoint, actTorque); - prg.setOutput(output); - prg.ActOut = output; - } else if (abs(actTorque) < 1000) { - myPID.reset(); - } - lastAnglePID = angle; - - if (LOGGING) { - if (waitIsOver(log_timer)) { - sendDataToDatalogger(); - waitStart(log_timer, LOG_TIMER_DELAY); - } - } - prg.isRunning == false; -} - -void interpolateWinkelArray() { - int lastSetIndex = -1; - - for (int i = 0; i < 180; i++) { - int start = i; - int k = i + 1; - while (WinkelArray[k] == -1) { - k++; - } - int end = k; - float startTorque = WinkelArray[start]; - float endTorque = WinkelArray[end]; - - for (int j = start + 1; j < end; j++) { - float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start); - WinkelArray[j] = interpolatedTorque; - } - } - - /*for (int i = 0; i < 181; i++) { - if (WinkelArray[i] != 0 || i == 0) { // Sicherstellen, dass der Startwert berücksichtigt wird - if (lastSetIndex != -1) { - int start = lastSetIndex; - int end = i; - float startTorque = WinkelArray[start]; - float endTorque = WinkelArray[end]; - - for (int j = start + 1; j < end; j++) { - float interpolatedTorque = startTorque + ((endTorque - startTorque) / (end - start)) * (j - start); - WinkelArray[j] = interpolatedTorque; - } - } - lastSetIndex = i; - } - }*/ -} - - - -float exponentialFilter(float currentValue, float previousFilteredValue, float alpha) { - return alpha * currentValue + (1.0 - alpha) * previousFilteredValue; -} - -void sendDataToDatalogger() { - datalogger.addData((float)((float)millis() - (float)logtime) / 1000.0, (float)angle / 1000.0, setpoint / 1000.0, actTorque / 1000.0, (float)prg.getOutVolt() / 1000.0, RotSpeed); -} - -unsigned char BLOCK_DETECTION(unsigned int torque, float speed) { - if (torque >= BLOCK_DETECTION_VALUE && speed < ROTATION_DETECTION_VALUE) { - return 1; - } else { - return 0; - } -} - -void setAnalogResolution(unsigned char res) { - analogWriteResolution(res); - analogReadResolution(res); -} - -void UPDATE_VALUES() { - angle = _getAngle; - actTorque = _getTorque; - - prg.updateActOut(actTorque); -} - -void COMMUNICATION_HANDLER() { - if (Serial.available() > 0) { - String command = receiveString(); - parseCommand(command); - Serial.read(); - } -} - -void SPEEDCHECK() { - int deltaAngle = angle - lastAngle; - RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde - lastAngle = angle; - - if (RotSpeed > 0) { - if (CW == 0) { - Richtungswechsel = 1; - } - CW = 1; - } else if (RotSpeed < 0) { - if (CW == 1) { - Richtungswechsel = 1; - } - CW = 0; - } -} - -void DEBUG_PRINTER() { - // Debug-Informationen ausgeben - Serial.println("####################"); - Serial.print("Winkel: "); - Serial.println((float)angle / 1000, 3); - Serial.print("Drehmoment: "); - Serial.println((float)actTorque / 1000, 3); - Serial.print("Geschwindigkeit: "); - Serial.println(RotSpeed); - Serial.print("Drehrichtung: "); - Serial.println(CW); - Serial.print("Ausgangsspannung: "); - Serial.println((float)prg.getOutVolt() / 1000, 3); - Serial.print("Setpoint: "); - Serial.println((float)setpoint / 1000, 3); - Serial.print("Kp: "); - Serial.println(Kp, 3); - Serial.print("Ki: "); - Serial.println(Ki, 3); - Serial.print("Kd: "); - Serial.println(Kd, 3); - Serial.print("output: "); - Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); - Serial.print("Pon: "); - Serial.println(Pon); - Serial.print("manOut: "); - Serial.println(manOut); - Serial.print("manVolt: "); - Serial.println(manVolt); - Serial.print("LOGGING: "); - Serial.println(LOGGING); - Serial.println("####################"); -} - - -String receiveString() { - String received = ""; - unsigned long startTime = millis(); - byte inByte[1]; - while ((char)inByte[0] != '\n' || Serial.available() > 0) { - //c = Serial.read(); - Serial.readBytes(inByte, 1); - received += (char)inByte[0]; - } - //Serial.println(received); - return received; -} - -void parseCommand(const String &command) { - if (command[0] == 'p') { - float tempKp; - setParameter(command, 'Kp', tempKp); - Kp = tempKp; - myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings - datalogger.addConfig("Kp", String(Kp)); // Konvertierung zu String - } else if (command[0] == 'i') { - float tempKi; - setParameter(command, 'Ki', tempKi); - Ki = tempKi; - myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings - datalogger.addConfig("Ki", String(Ki)); // Konvertierung zu String - } else if (command[0] == 'd') { - float tempKd; - setParameter(command, 'Kd', tempKd); - Kd = tempKd; - myPID.setTunings(Kp, Ki, Kd); // Aktualisieren der Tunings - datalogger.addConfig("Kd", String(Kd)); // Konvertierung zu String - } else if (command[0] == 'a') { - Serial.print(angle); - Serial.print(";"); - Serial.print(actTorque); - Serial.print(";"); - Serial.print(prg.getSetPoint(1)); - Serial.print(";"); - Serial.print(setpoint); - Serial.print(";"); - Serial.println(prg.ActOut); - } else if (command[0] == 'b') { - DEBUG_MODE = !DEBUG_MODE; - } else if (command[0] == 'e') { - prg.safeShutdown(5); - } - if (command[0] == 'f') { - prg.DynDecog(prg.ActOut); - } else if (command[0] == 'l') { - LOGGING = !LOGGING; - if (LOGGING) { - if (CW) { - datalogger.addConfig("Drehrichtung", "CW"); - } else { - datalogger.addConfig("Drehrichtung", "CCW"); - } - logtime = millis(); - waitStart(log_timer, LOG_TIMER_DELAY); - } else { - datalogger.logToSerial(); - datalogger.clear(); - } - } else if (command[0] == 'm') { - manOut = !manOut; - } else if (command[0] == 'r') { - myPID.reset(); - } else if (command[0] == 's') { - setParameter(command, 's', setpoint); - for (int i = 0; i < 180; i++) { - WinkelArray[i] = setpoint; - } - } else if (command[0] == 'S') { - In_Src_Sw = !In_Src_Sw; - } else if (command[0] == 't') { - prg.tareTorque(); - } else if (command.startsWith("u") && command.endsWith("u\n")) { - Serial.println(command); // Debug-Ausgabe - for (int i = 0; i < 181; i++) { - WinkelArray[i] = -1; - } - String data = command.substring(1, command.length() - 1); - int lastIndex = 0; - while (lastIndex != -1) { - int index = data.indexOf(';', lastIndex); - String pair = (index == -1) ? data.substring(lastIndex) : data.substring(lastIndex, index); - int commaIndex = pair.indexOf(','); - if (commaIndex != -1) { - int angleIndex = pair.substring(0, commaIndex).toInt(); - float torqueValue = pair.substring(commaIndex + 1).toFloat(); - if (angleIndex >= 0 && angleIndex < 181) { - WinkelArray[angleIndex] = torqueValue; - Serial.print("Set WinkelArray["); - Serial.print(angleIndex); - Serial.print("] to "); - Serial.println(torqueValue); - } - } - lastIndex = (index == -1) ? -1 : index + 1; - } - interpolateWinkelArray(); - } else if (command[0] == 'v') { - if (manOut) { - setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert - manVolt = manVolt * 0.1575; - prg.ActOut = manVolt; - analogWrite(prg.TORQUE_OUT_PIN, manVolt); - } - } else if (command[0] == 'w') { - prg.tareAngle(); - } else if (command[0] == 'x') { - Pon = !Pon; - myPID.reset(); - } -} - - -void setParameter(const String &command, char prefix, float ¶meter) { - int commaIndex = command.indexOf('.'); - if (commaIndex > 1) { - String paramString = command.substring(1, commaIndex) + "." + command.substring(commaIndex + 1); - parameter = paramString.toFloat(); // Konvertierung innerhalb der Funktion - /*Serial.print(prefix); - Serial.print(" gesetzt auf: "); - Serial.println(parameter, 6); // Sechs Dezimalstellen anzeigen*/ - } -} \ No newline at end of file diff --git a/main_v3/main_v3.ino b/main_v3/main_v3.ino deleted file mode 100644 index b183f5c..0000000 --- a/main_v3/main_v3.ino +++ /dev/null @@ -1,623 +0,0 @@ -#include -#define MEAS_SAMPLES 4000 -#define MEAS_TIME 100 -#define K_FAKTOR 1.0995380532183 - -// Initialisiere die PRG_342-Klasse -PRG_342 prg = PRG_342(K_FAKTOR); - -//Messung / Logging -bool LOGGING = false; -int measTorque[MEAS_SAMPLES]; -int measAngle[MEAS_SAMPLES]; -unsigned int measSetpoint[MEAS_SAMPLES]; -unsigned int measMillis[MEAS_SAMPLES]; -unsigned int measVolt[MEAS_SAMPLES]; -unsigned int logCounter; -unsigned long timer; -unsigned long diff; -float ALPHA = 0.001; - -// Deklaration der Variablen für die Steuerung und Messung -int angle = 0; // Aktueller Winkel -int lastAngle = 0; // Letzter gemessener Winkel -int actTorque = 0; // Aktuelles Drehmoment -int setpoint = 0; // Sollwert für Drehmoment -float tolerance = 0.0; -//int tolerance = 500; -float lowerBound; -float upperBound; -static int lastOutput = 0; -unsigned int manVolt = 0; -unsigned char CW; -unsigned char Richtungswechsel; - -// PID-Reglerparameter -//float Kp = 5.15; -//float Ki = 0.02; -//float Kd = 2273.47; -float Kp = 0; -float Ki = 0.010; -float Kd = 0; -float I_ALPHA = 0.0; -float I_old = 0; -int Pout; -int Iout; -int Dout; -float MAX_INTEGRAL; - -int error; // Fehler zwischen Soll- und Istwert (Regeldifferenz e) - -// Steuervariablen -bool OFFSET_ON = true; -bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren -bool PID = false; // PID-Regelung ein-/ausschalten -unsigned long PID_DELAY = 0; // Verzögerung zwischen PID-Regelungen in ms -unsigned int Imax = 217; // Maximalstrom in mA -bool PRINT_DEBUG = true; // Debug-Ausgaben ein-/ausschalten -bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten -bool ARDUINO_PLOTTER = false; - -// Variablen für PID-Berechnungen -float integral = 0; // Integralterm -int derivative; // Differenzialterm -int previousError = 0; // Vorheriger Fehler -unsigned long lastTime = 0; -unsigned long currentTime; -unsigned long timeChange; - -// Initialisierung der verschiedenen Timer -unsigned long timer1; -unsigned long timer2; -unsigned long PIDtimer; -unsigned long logTimer; - -float RotSpeed; // Rotationsgeschwindigkeit -unsigned int MilliAmpPerSecond = 25; // Milliampere pro Sekunde -unsigned int maxChange; // Maximale Änderung pro Schleifendurchlauf - -char serBuffer[4]; // Buffer für serielle Eingaben - -void setup() { - - for (int i = 0; i < MEAS_SAMPLES; i++) { - measTorque[i - 1] = 0; - measAngle[i - 1] = 0; - measSetpoint[i - 1] = 0; - measMillis[i - 1] = 0; - measVolt[i - 1] = 0; - } - - analogWriteResolution(12); - analogReadResolution(12); - - Serial.begin(115200); // Beginne serielle Kommunikation - - prg.safeShutdown(5); // Sicheres Herunterfahren des Ausgangs - //prg.AutoDecog(); - - // Initialisiere Timer mit der aktuellen Zeit - timer1 = timer2 = PIDtimer = logTimer = millis(); - - // Lese den initialen Winkel - angle = prg.getAngle(); - lastAngle = angle; -} - -void Datalogger() { - if (LOGGING) { - if (millis() - logTimer >= MEAS_TIME) { - logTimer = millis(); - logCounter++; - - measTorque[logCounter] = actTorque; - measAngle[logCounter] = angle; - measSetpoint[logCounter] = setpoint; - measMillis[logCounter] = logCounter * MEAS_TIME; - measVolt[logCounter] = prg.getOutVolt(); - } - } -} - -void loop() { - angle = prg.getAngle(); - actTorque = prg.getTorque(true, ALPHA); - // Aktualisiere den analogen Ausgang für den aktuellen Drehmoment - prg.updateActOut(actTorque); - - /* - // Prüfe, ob der Debug-Modus aktiviert ist - DEBUG_TIMER(); // Aktualisierung des Debug-Timers - if (DEBUG_MODE) { - DEBUG_PRINTER(); // Debug Informationen ausgeben - } - - if (ARDUINO_PLOTTER) { - Arduino_Plotter(); - } - - Datalogger(); -*/ - // Verarbeitung der seriellen Schnittstelle - COMMUNICATION_HANDLER(); - - // Rotationsgeschwindigkeit aktualisieren - SPEEDCHECK(); - - - PID_CONTROL(); // Ausführung der PID-Regelung - - // setze isRunning zurück, damit es durch den Interrupt des Drehgebers wieder gesetzt werden kann - prg.isRunning = false; -} - - -void COMMUNICATION_HANDLER() { - // Verarbeite eingehende Befehle von der seriellen Schnittstelle - if (Serial.available() > 0) { - delay(10); - unsigned int SerIn = Serial.read(); - switch (SerIn) { - case 'a': - // Drehwinkel und Drehmoment ausgeben - Serial.print((float)angle / 1000, 1); - Serial.print(";"); - Serial.println((float)actTorque / 1000, 1); - break; - - case 'e': - // safeShutdown ausführen - prg.safeShutdown(10); - break; - - case 't': - // Drehmoment nullen - prg.tareTorque(); - break; - - case 'w': - // Winkel nullen - prg.tareAngle(); - break; - - case 'o': - // Offset für Drehmoment deaktivieren - OFFSET_ON = !OFFSET_ON; - break; - - // Einstellen der Parameter Kp, Ki und Kd - case 'p': - delay(10); - Serial.readBytes(serBuffer, 4); - Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Kp: "); - Serial.println(Kp); - break; - - case 'i': - delay(10); - Serial.readBytes(serBuffer, 4); - Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Ki: "); - Serial.println(Ki); - break; - - case 'd': - delay(10); - Serial.readBytes(serBuffer, 4); - Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("Kd: "); - Serial.println(Kd); - break; - - case 'j': - delay(10); - Serial.readBytes(serBuffer, 4); - I_ALPHA = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("I_ALPHA: "); - Serial.println(I_ALPHA); - break; - - case 's': - // Soll-Wert Einstellung - delay(10); - Serial.readBytes(serBuffer, 4); - setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; - Serial.print("setpoint: "); - Serial.println(setpoint); - break; - - case 'm': - // Spannungsausgabe auf manuell stellen - manOut = !manOut; - break; - - case 'v': - // Ausgangsspannung einstellen (manOut muss auf true sein) - if (manOut) { - Serial.readBytes(serBuffer, 4); - manVolt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); - //prg.setOutput((float)manVolt * 0.1575, false); - float outVal = (float)manVolt * 0.1575; - prg.ActOut = outVal; - analogWrite(prg.TORQUE_OUT_PIN, outVal); - } - break; - - case 'x': - // Regelung ein-/ausschalten - PID = !PID; - error = 0; - previousError = 0; - integral = 0; - break; - - case 'r': - // PID-Parameter zurücksetzen - error = 0; - previousError = 0; - integral = 0; - derivative = 0; - Pout = 0; - Iout = 0; - Dout = 0; - break; - - case 'f': - // Dynamic Decogging ausführen - prg.DynDecog(prg.ActOut); - break; - - case 'k': - // Einstellen des maximalen Stromanstiegs - delay(10); - Serial.readBytes(serBuffer, 4); - MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); - Serial.print("Max Milliampere per second: "); - Serial.println(MilliAmpPerSecond); - break; - - case 'b': - // DEBUG_MODE switch - DEBUG_MODE = !DEBUG_MODE; - break; - - case 'u': - // Open-Loop Test starten - openLoopTest(); - break; - - case 'l': - LOGGING_HANDLER(); - break; - - case 'y': - // Automatic Measurement - timer = millis(); - diff = millis() - timer; - LOGGING_HANDLER(); - PID = true; - while (diff <= 150000) { - angle = prg.getAngle(); - actTorque = prg.getTorque(true, ALPHA); - prg.updateActOut(actTorque); - - Datalogger(); - - /*if (diff > 20000 && diff <= 50000) { - setpoint = 5000; - } - if (diff > 50000 && diff <= 80000) { - setpoint = 10000; - } - if (diff > 80000 && diff <= 110000) { - setpoint = 15000; - } - if (diff > 110000 && diff <= 140000) { - setpoint = 5000; - }*/ - - if (angle > 0 && angle <= 45000) { - setpoint = 10000; - } - if (angle > 45000 && angle <= 65000) { - setpoint = 15000; - } - if (angle > 65000 && angle <= 90000) { - setpoint = 10000; - } - - - SPEEDCHECK(); - - PID_CONTROL(); - - diff = millis() - timer; - - Serial.print("elapsed Time: "); - Serial.print((float)diff / 1000, 3); - Serial.print(" actual Angle: "); - Serial.println((float)angle / 1000, 3); - } - LOGGING_HANDLER(); - PID = false; - break; - - default: - // Unbekannte Befehle bearbeiten - Serial.print("Unbekannter Befehl: "); - Serial.println(SerIn); - break; - } - } -} - -void printData() { - Serial.print("Kp:"); - Serial.print(";"); - Serial.println(Kp, 3); - Serial.print("Ki:"); - Serial.print(";"); - Serial.println(Ki, 3); - Serial.print("Kd:"); - Serial.print(";"); - Serial.println(Kd, 3); - Serial.print("I_ALPHA:"); - Serial.print(";"); - Serial.println(I_ALPHA, 6); - Serial.print("Drehrichtung:"); - Serial.print(";"); - if (CW) { - Serial.println("CW"); - } else { - Serial.println("CCW"); - } - - Serial.println("Zeit in Sekunden;Winkel;Soll-Drehmoment;Ist-Drehmoment;Erregerspannung;Winkel in Grad;Soll-Drehmoment in Nm;Ist-Drehmoment in Nm;Erregerspannung in V"); - for (int i = 0; i < MEAS_SAMPLES; i++) { - if (measMillis[i] > 0) { - Serial.print((float)measMillis[i] / 1000.0, 2); - Serial.print(";"); - Serial.print(measAngle[i]); - Serial.print(";"); - Serial.print(measSetpoint[i]); - Serial.print(";"); - Serial.print(measTorque[i]); - Serial.print(";"); - Serial.print(measVolt[i]); - Serial.print(";"); - Serial.print(abs((float)measAngle[i] / 1000.0), 3); - Serial.print(";"); - Serial.print(abs((float)measSetpoint[i] / 1000.0), 3); - Serial.print(";"); - Serial.print(abs((float)measTorque[i] / 1000.0), 3); - Serial.print(";"); - Serial.println(abs((float)measVolt[i] / 1000.0), 3); - } - } - for (int i = 0; i < MEAS_SAMPLES; i++) { - measTorque[i - 1] = 0; - measAngle[i - 1] = 0; - measSetpoint[i - 1] = 0; - measMillis[i - 1] = 0; - measVolt[i - 1] = 0; - } -} - -void LOGGING_HANDLER() { - // LOGGING switch - LOGGING = !LOGGING; - if (LOGGING) { - logCounter = 0; - } else { - printData(); - } -} - -void PID_CONTROL() { - lowerBound = (float)setpoint - ((float)setpoint * tolerance); - upperBound = (float)setpoint + ((float)setpoint * tolerance); - MAX_INTEGRAL = (4095.0 / Ki) * 0.9; - //lowerBound = (float)setpoint - tolerance; - //upperBound = (float)setpoint + tolerance; - // Timing des PID-Reglers - if (micros() - PIDtimer >= PID_DELAY) { - - if (Richtungswechsel) { - integral = 0; - derivative = 0; - Richtungswechsel = 0; - } - - PIDtimer = micros(); // Setze den Timer zurück - // Aktualisierung des Zeitstempels und Berechnung der vergangenen Zeit seit dem letzten Update - currentTime = millis(); // Lese die aktuelle Zeit in Millisekunden - timeChange = currentTime - lastTime; // Zeitdifferenz zum letzten Aufruf - lastTime = currentTime; // Setze den letzten Zeitstempel auf die aktuelle Zeit für den nächsten Durchlauf - - - // Überprüfe, ob die PID-Regelung aktiviert ist, keine manuelle Ausgabe erfolgt, - // das gemessene Drehmoment die Schwelle überschreitet und die Geschwindigkeit hoch genug ist - // ODER das gemessene Drehmoment den Sollwert übersteigt (Antrieb auf Block) - if (PID && !manOut && ((abs(actTorque) >= 5000 || abs(RotSpeed) >= 400) || (abs(actTorque) > setpoint)) && (abs(actTorque) < lowerBound || abs(actTorque) > upperBound)) { - - // Aktuelle Werte auslesen - //actTorque = abs(prg.getTorque(true, 100)); // Aktuelles Drehmoment - //angle = abs(prg.getAngle()); // Aktueller Winkel - - // Berechnung der Regeldifferenz - error = abs(setpoint) - abs(actTorque); // Differenz zwischen Sollwert und tatsächlich gemessenem Drehmoment - - // Berechnung des proportionalen Anteils der PID-Regelung - Pout = Kp * error; // Proportionaler Output basiert auf dem aktuellen Fehler und dem Proportional-Faktor Kp - /* - // Berechnung des integralen Anteils der PID-Regelung - integral += error * ((float)timeChange / 1000.0); // Addition des Fehlers über die Zeit (Integration des Fehlers) - integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden - Iout = Ki * integral; // Berechnung des integralen Output basierend auf dem Integral und dem Integral-Faktor Ki - */ - - // Berechnung des adaptiv gewichteten integralen Anteils - /*float weight = 1.0 + (1 - exp(-I_ALPHA * abs(error))); // Gewichtungsfaktor abhängig vom Fehler - integral += weight * error * ((float)timeChange / 1000.0); // Anpassung der Integration des Fehlers - integral = constrain(integral, -MAX_INTEGRAL, MAX_INTEGRAL); // Begrenzung des integralen Anteils, um Windup zu vermeiden - Iout = Ki * integral;*/ - - integral += error * (((float)timeChange / 1000.0) * 0.5); // Anpassung der Integration des Fehlers - integral = constrain(integral, -MAX_INTEGRAL, MAX_INTEGRAL); // Begrenzung des integralen Anteils, um Windup zu vermeiden - Iout = Ki * integral; - - // Berechnung des differenziellen Anteils der PID-Regelung - derivative = (error - previousError) / ((float)timeChange / 1000.0); // Berechnung der Änderungsrate des Fehlers - Dout = Kd * derivative; // Differenzieller Output basiert auf der Änderungsrate des Fehlers und dem Differenzial-Faktor Kd - - // Berechnung des gesamten PID-Output - int output = Pout + Iout + Dout; // Summe der einzelnen PID-Komponenten - - // Begrenzung des Ausganges, um Cogging zu vermeiden - // Der Strom darf nicht auf 0 sinken, während die Hysteresebremse stillsteht - output = constrain(output, 0, 4095); - - // Begrenzung des Stromanstieges, um Cogging vorzubeugen - //maxChange = ((float)MilliAmpPerSecond * 4095.0 * (float)PID_DELAY) / ((float)Imax * 1000); // Umrechnung des eingegebenen Stromwertes / Sekunde in einen binären Wert, angepasst auf den Ausgang - /*maxChange = 1; - if (abs(output - lastOutput) > maxChange) { - if (output > lastOutput) { - output = lastOutput + maxChange; // Erhöht den Output - } else { - output = lastOutput - maxChange; // Verringert den Output - } - }*/ - - // Den Wert output am Ausgang (an die Hysteresebremse) anlegen - prg.setOutput(output, false); - lastOutput = output; // Aktualisiere den letzten Output-Wert - - // Aktualisierung des vorherigen Fehler - previousError = error; - } - } -} - -void openLoopTest() { - LOGGING = true; // Logging aktivieren - logCounter = 0; // Log-Zähler zurücksetzen - - // Setze die Spannung für die Stufenänderung - int stepVoltage = 3000; // Beispielwert für die Stufenänderung (kann angepasst werden) - prg.setOutput(stepVoltage, false); - unsigned long startTime = millis(); - unsigned long duration = 50000; // Messdauer in Millisekunden (z.B. 10 Sekunden) - - // Messe die Systemantwort für die festgelegte Dauer - while (millis() - startTime < duration) { - angle = prg.getAngle(); - actTorque = prg.getTorque(true, ALPHA); - prg.updateActOut(actTorque); - Datalogger(); - } - - // Setze die Ausgangsspannung wieder auf Null - prg.setOutput(0, false); - LOGGING = false; // Logging deaktivieren - printData(); // Ausgabe der gemessenen Daten -} - - -void SPEEDCHECK() { - // Überprüfen der Rotationsgeschwindigkeit - if (millis() - timer2 >= 200) { - timer2 = millis(); - int deltaAngle = angle - lastAngle; - RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde - lastAngle = angle; - - if (RotSpeed > 0) { - if (CW == 0) { - Richtungswechsel = 1; - } - CW = 1; - } else if (RotSpeed < 0) { - if (CW == 1) { - Richtungswechsel = 1; - } - CW = 0; - } - } -} - -void DEBUG_TIMER() { - // Timer für Debug-Ausgaben - if (millis() - timer1 >= 500) { - timer1 = millis(); - PRINT_DEBUG = true; - } else { - PRINT_DEBUG = false; - } -} - -void DEBUG_PRINTER() { - // Debug-Informationen ausgeben - if (PRINT_DEBUG) { - Serial.println("####################"); - Serial.print("Winkel: "); - Serial.println((float)angle / 1000, 3); - Serial.print("Drehmoment: "); - Serial.println((float)actTorque / 1000, 3); - Serial.print("Geschwindigkeit: "); - Serial.println(RotSpeed); - Serial.print("Drehrichtung: "); - Serial.println(CW); - Serial.print("Ausgangsspannung: "); - Serial.println((float)prg.getOutVolt() / 1000, 3); - Serial.print("Setpoint: "); - Serial.println((float)setpoint / 1000, 3); - Serial.print("Error: "); - Serial.println((float)error / 1000, 3); - Serial.print("Kp: "); - Serial.println(Kp, 3); - Serial.print("Ki: "); - Serial.println(Ki, 3); - Serial.print("Kd: "); - Serial.println(Kd, 3); - Serial.print("I_ALPHA:"); - Serial.println(I_ALPHA); - Serial.print("Pout: "); - Serial.println(Pout); - Serial.print("Iout: "); - Serial.println(Iout); - Serial.print("Dout: "); - Serial.println(Dout); - Serial.print("output: "); - Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); - Serial.print("Integral: "); - Serial.println(integral); - Serial.print("PID: "); - Serial.println(PID); - Serial.print("manOut: "); - Serial.println(manOut); - Serial.print("manVolt: "); - Serial.println(manVolt); - Serial.print("Max mA / s (SET): "); - Serial.println(MilliAmpPerSecond); - Serial.print("Max mA / s (ACT): "); - Serial.println((Imax * maxChange * 1000) / (4095 * (PID_DELAY / 1000.0))); - Serial.print("maxChange: "); - Serial.println(maxChange); - Serial.print("isRunning: "); - Serial.println(prg.isRunning); - Serial.print("LOGGING: "); - Serial.println(LOGGING); - Serial.println("####################"); - } -} - -void Arduino_Plotter() { - if (PRINT_DEBUG) { - Serial.print("Soll:"); - Serial.print(setpoint); - Serial.print(","); - Serial.print("Ist:"); - Serial.println(abs(actTorque)); - } -} diff --git a/main_v4/main_v4.ino b/main_v4/main_v4.ino deleted file mode 100644 index 2474286..0000000 --- a/main_v4/main_v4.ino +++ /dev/null @@ -1,57 +0,0 @@ -#include -#include - -#define PIN_INPUT A0 -#define PIN_OUTPUT DAC0 -#define K_FAKTOR 1.0995380532183 -#define ALPHA 1 - -PRG_342 prg = PRG_342(K_FAKTOR); - -//Define Variables we'll be connecting to -double Setpoint, Input, Output; -int angle; -int lastAngle; -unsigned long timer1; - -//Specify the links and initial tuning parameters -double Kp = 0, Ki = 0.04, Kd = 0; -PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); - -void setup() {prg.setOutput(4095*0.5, false); - delay(3000); - prg.setOutput(0, false); - delay(2000); - Serial.begin(115200); - - angle = prg.getAngle(); - Input = prg.getTorque(true, ALPHA); - prg.updateActOut(Input); - Setpoint = 15000; - - //turn the PID on - myPID.SetOutputLimits(0, 4095); - myPID.SetMode(AUTOMATIC); -} - -void loop() { - angle = prg.getAngle(); - Input = abs(prg.getTorque(true, ALPHA)); - prg.updateActOut(Input); - - if (SPEEDCHECK() >= 400) { - myPID.Compute(); - } - - prg.setOutput(Output, false); -} - -float SPEEDCHECK() { - // Überprüfen der Rotationsgeschwindigkeit - if (millis() - timer1 >= 200) { - timer1 = millis(); - int deltaAngle = angle - lastAngle; - lastAngle = angle; - return abs(deltaAngle * 5); // Umrechnen in Winkel pro Sekunde - } -} \ No newline at end of file