From ddedc0c72a4c292da1bf5898835f5e48a2903c4d Mon Sep 17 00:00:00 2001 From: Musab Erdem Date: Thu, 18 Jul 2024 12:02:17 +0200 Subject: [PATCH] Komplette Umstellung auf v2 clean --- main/main_v2_clean.ino | 469 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 469 insertions(+) create mode 100644 main/main_v2_clean.ino diff --git a/main/main_v2_clean.ino b/main/main_v2_clean.ino new file mode 100644 index 0000000..c1f6b76 --- /dev/null +++ b/main/main_v2_clean.ino @@ -0,0 +1,469 @@ +#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