Synchronisierung.

Libraries/datalogger/Datalogger.cpp

@@ -0,0 +1,51 @@
+#include "Datalogger.h"
+
+Datalogger::Datalogger() {}
+
+void Datalogger::addData(const std::map<String, String>& data) {
+    DataEntry entry;
+    entry.values = data;
+    dataEntries.push_back(entry);
+}
+
+void Datalogger::addConfig(String parameter, String value) {
+    Config config = { parameter, value };
+    configs.push_back(config);
+}
+
+void Datalogger::logToSerial() {
+    Serial.println("Configurations:");
+    for (Config config : configs) {
+        Serial.print(config.parameter);
+        Serial.print("\t");
+        Serial.println(config.value);
+    }
+
+    Serial.println("\nData:");
+
+    if (!dataEntries.empty()) {
+        // Print headers
+        bool firstEntry = true;
+        for (const auto& entry : dataEntries) {
+            if (firstEntry) {
+                for (const auto& pair : entry.values) {
+                    Serial.print(pair.first);
+                    Serial.print("\t");
+                }
+                Serial.println();
+                firstEntry = false;
+            }
+            // Print values
+            for (const auto& pair : entry.values) {
+                Serial.print(pair.second);
+                Serial.print("\t");
+            }
+            Serial.println();
+        }
+    }
+}
+
+void Datalogger::clear() {
+    dataEntries.clear();
+    configs.clear();
+}

Libraries/datalogger/Datalogger.h

@@ -0,0 +1,31 @@
+#ifndef DATALOGGER_H
+#define DATALOGGER_H
+
+#include <Arduino.h>
+#include <vector>
+#include <map>
+
+struct DataEntry {
+    std::map<String, String> values;
+};
+
+struct Config {
+    String parameter;
+    String value;
+};
+
+class Datalogger {
+public:
+    Datalogger();
+
+    void addData(const std::map<String, String>& data);
+    void addConfig(String parameter, String value);
+    void logToSerial();
+    void clear();
+
+private:
+    std::vector<DataEntry> dataEntries;
+    std::vector<Config> configs;
+};
+
+#endif // DATALOGGER_H

Libraries/datalogger/example.ino

@@ -0,0 +1,45 @@
+#include <Arduino.h>
+#include "Datalogger.h"
+
+Datalogger logger;
+
+void setup() {
+    Serial.begin(9600);
+
+    // Beispielkonfigurationen hinzufügen
+    logger.addConfig("Sample Rate", "1Hz");
+    logger.addConfig("Sensor Type", "Temperature");
+
+    // Beispieldaten hinzufügen
+    std::map<String, String> data1;
+    data1["Time"] = "0.1";
+    data1["Input"] = "1";
+    data1["Output"] = "5";
+    logger.addData(data1);
+
+    std::map<String, String> data2;
+    data2["Time"] = "0.2";
+    data2["Input"] = "2";
+    data2["Output"] = "10";
+    logger.addData(data2);
+
+    std::map<String, String> data3;
+    data3["Time"] = "0.3";
+    data3["Input"] = "3";
+    data3["Output"] = "15";
+    logger.addData(data3);
+
+    // Daten und Konfigurationen über die serielle Schnittstelle ausgeben
+    logger.logToSerial();
+
+    // Inhalte leeren
+    logger.clear();
+
+    // Überprüfen, ob die Inhalte geleert wurden
+    Serial.println("After clearing:");
+    logger.logToSerial();
+}
+
+void loop() {
+    // Hier könnte weitere Logik stehen
+}

Libraries/datalogger/pid.cpp

@@ -0,0 +1,71 @@
+#include "PID.h"
+#include <Arduino.h>
+
+PID::PID(float kp, float ki, float kd) {
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+    this->minOutput = 0;
+    this->maxOutput = 255;
+    this->lastOutput = 0;
+    this->sampleTime = 100; // Default sample time in milliseconds
+    this->lastTime = millis();
+    this->integral = 0;
+    this->lastInput = 0;
+}
+
+void PID::setTunings(float kp, float ki, float kd) {
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+}
+
+void PID::setOutputLimits(float min, float max) {
+    this->minOutput = min;
+    this->maxOutput = max;
+}
+
+void PID::setSampleTime(int sampleTime) {
+    this->sampleTime = sampleTime;
+}
+
+void PID::reset() {
+    this->integral = 0;
+    this->lastInput = 0;
+    this->lastTime = millis();
+}
+
+float PID::compute(float setpoint, float input) {
+    unsigned long now = millis();
+    unsigned long timeChange = now - lastTime;
+
+    if (timeChange >= sampleTime) {
+        // Calculate error
+        float error = setpoint - input;
+
+        // Proportional term
+        float Pout = kp * error;
+
+        // Integral term
+        integral += (ki * error * timeChange / 1000.0);
+        integral = constrain(integral, minOutput, maxOutput);
+
+        // Derivative term
+        float derivative = (input - lastInput) / (timeChange / 1000.0);
+        float Dout = kd * derivative;
+
+        // Compute output
+        float output = Pout + integral - Dout;
+        output = constrain(output, minOutput, maxOutput);
+
+        // Remember some variables for next time
+        lastInput = input;
+        lastTime = now;
+
+        lastOutput = output
+
+        return output;
+    }
+
+    return lastOutput;
+}

Libraries/datalogger/pid.h

@@ -0,0 +1,32 @@
+#ifndef PID_H
+#define PID_H
+
+class PID {
+public:
+    PID(float kp, float ki, float kd);
+
+    void setTunings(float kp, float ki, float kd);
+    void setOutputLimits(float min, float max);
+    void setSampleTime(int sampleTime);
+
+    void reset();
+
+    float compute(float setpoint, float input);
+
+private:
+    float kp;
+    float ki;
+    float kd;
+
+    float minOutput;
+    float maxOutput;
+    float lastOutput;
+
+    int sampleTime;
+    unsigned long lastTime;
+
+    float integral;
+    float lastInput;
+};
+
+#endif

alpha/alpha.ino

@@ -0,0 +1,113 @@
+#define encoderPinA 22  // Pin für das Signal A
+#define encoderPinB 23  // Pin für das Signal B
+
+volatile int encoderPos = 0;  // Variable zur Speicherung der relativen Position
+int lastEncoded = 0;          // Variable zur Speicherung des letzten Encodierten Zustands
+
+int SerIn = 0;
+float torque = 0;
+float offset = 0;
+float setpoint = 0;
+unsigned int valA1 = 0;
+unsigned int valA0 = 0;
+
+void setup() {
+  // Initialize the DAC pin
+  analogWrite(DAC0, 0);
+  pinMode(DAC0, OUTPUT);
+  analogWrite(DAC1, 0);
+  pinMode(DAC1, OUTPUT);
+
+  pinMode(A0, INPUT);
+  pinMode(A1, INPUT);
+
+  pinMode(encoderPinA, INPUT);
+  pinMode(encoderPinB, INPUT);
+  attachInterrupt(digitalPinToInterrupt(encoderPinA), updateEncoder, CHANGE);
+  attachInterrupt(digitalPinToInterrupt(encoderPinB), updateEncoder, CHANGE);
+
+  analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
+  analogReadResolution(12);
+
+  Serial.begin(9600);
+
+  //delay(10000); // Evtl. vorhandene Spulenspannung senken.
+}
+
+void loop() {
+  if (Serial.available() > 0) {
+    delay(100);
+    SerIn = Serial.read();
+    if (SerIn == 't') {
+      tare();
+    } else {
+      SerIn = (SerIn - '0') * 10;
+      analogWrite(DAC1, SerIn * 40.95);
+      Serial.println(SerIn * 0.26);
+    }
+  }
+
+
+
+  torque = getActTorque();
+  for (int i = 0; i < 9; i++) {
+    torque += getActTorque();
+  }
+  torque = torque * 0.1;
+
+  Serial.print("torque: ");
+  Serial.println(abs(torque + offset), 1);
+  analogWrite(DAC0, abs(torque + offset) * 81.9);
+
+
+
+  // Beispiel: Ausgabe der relativen Position
+  Serial.print("Relative Position: ");
+  Serial.println(encoderPos);
+
+  // Berechnung des relativen Winkels
+  float angle = (encoderPos / 20000.0) * 360.0;
+  Serial.print("Relative Angle: ");
+  Serial.println(angle, 1);  // Ausgabe mit einer Nachkommastelle
+
+  delay(1000);
+}
+
+void updateEncoder() {
+  int MSB = digitalRead(encoderPinA);  // Most Significant Bit
+  int LSB = digitalRead(encoderPinB);  // Least Significant Bit
+
+  int encoded = (MSB << 1) | LSB;          // Kombinieren der beiden Bits
+  int sum = (lastEncoded << 2) | encoded;  // Kombinieren mit dem letzten Zustand
+
+  // Bestimmen der Richtungsänderung und Aktualisierung der Position
+  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderPos++;
+  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderPos--;
+
+  lastEncoded = encoded;  // Aktualisieren des letzten Zustands
+}
+
+unsigned int VoltToInt(unsigned int voltage) {
+  return (voltage - 550.0) * 1.86136364;  // (x - 550) * 4095/2200
+}
+
+float getActTorque() {
+  valA0 = analogRead(A0);
+  return (valA0 - 2047.5) * 0.024420024;  // (x - 2047.5) * 100/4095
+  //return ((4095 * 1.00) - 2047.5) * 0.024420024;
+}
+
+float getSetPoint() {
+  return (analogRead(A1) * 0.002442002);  //x * 10/4095
+}
+
+void tare() {
+  torque = getActTorque();
+  for (int i = 0; i < 99999; i++) {
+    torque += getActTorque();
+  }
+  torque = torque * 0.00001;
+  offset = 0 - torque;
+
+  encoderPos = 0;
+}

debug/debug.ino

@@ -0,0 +1,46 @@
+#include <prg_342.h>
+
+// Initialisiere die PRG_342-Klasse
+PRG_342 prg = PRG_342();  // Maximale Änderung pro Schleifendurchlauf
+
+char serBuffer[4];  // Buffer für serielle Eingaben
+
+void setup() {
+  analogWriteResolution(12);
+  analogReadResolution(12);
+
+  Serial.begin(115200);  // Beginne serielle Kommunikation
+
+  prg.safeShutdown(5);  // Sicheres Herunterfahren des Ausgangs
+}
+
+void loop() {
+  for (int i = 1; i < 10; i ++) {
+    Serial.println("##########");
+    Serial.print("Beginne mit ");
+    Serial.print(i);
+    Serial.println(" mA / s.");
+
+    prg.setOutput(0, 4095 * 0.5, i);
+    prg.setOutput(4095 * 0.5, 0, i);
+    Serial.println("Pause: 5 Sekunden");
+    delay(1000);
+    Serial.println("Pause: 4 Sekunden");
+    delay(1000);
+    Serial.println("Pause: 3 Sekunden");
+    delay(1000);
+    Serial.println("Pause: 2 Sekunden");
+    delay(1000);
+    Serial.println("Pause: 1 Sekunde");
+    delay(1000);
+
+    Serial.print("Vorgang mit ");
+    Serial.print(i);
+    Serial.println(" mA / s abgeschlossen.");
+    Serial.println("##########");
+    Serial.println("Entgnetisierung läuft");
+    prg.safeShutdown(5);  // Sicheres Herunterfahren des Ausgangs
+    Serial.println("Nächster Test in 3 Sekunden.");
+    delay(3000);
+  }
+}

main/main.ino

@@ -1,9 +1,293 @@
-void setup() {
+#include <prg_342.h>
-  // put your setup code here, to run once:
 
+PRG_342 prg = PRG_342();
+
+int angle = 0;
+int lastAngle = 0;
+int actTorque = 0;
+int setpoint = 0;
+double lastOutput;
+
+float Kp = 0.0;
+float Ki = 0.0;
+float Kd = 0.0;
+int Pout;
+int Iout;
+int Dout;
+
+int error;
+
+bool OFFSET_ON = true;
+bool manOut = false;
+
+bool PID = false;
+unsigned char PID_DELAY = 1;  //in ms
+unsigned int Imax = 217;      //in mA
+bool PRINT_DEBUG = false;
+bool DEBUG_MODE = false;
+
+// 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];
+
+void setup() {
+  analogWriteResolution(12);  // Set the resolution to 12 bits (0-4095)
+  analogReadResolution(12);
+
+  Serial.begin(115200);
+
+  prg.safeShutdown(5);
+
+  timer1 = timer2 = PIDtimer = millis();
+  angle = prg.getAngle();
+  lastAngle = angle;
+}
+
+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;
+    }
+  }
+}
+
+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();
+  }
+}
+
+void DEBUG_TIMER() {
+  if (millis() - timer1 >= 500) {
+    timer1 = millis();
+    PRINT_DEBUG = true;
+  } else {
+    PRINT_DEBUG = false;
+  }
+}
+
+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("####################");
+  }
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+  if (DEBUG_MODE) {
-
+    DEBUG_TIMER();
+    DEBUG_PRINTER();
+  }
+  COMMUNICATION_HANDLER();
+  SPEEDCHECK();
+
+  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();
+    }
+  }
+
+  prg.updateActOut();
+  prg.isRunning = false;
 }

main_v2/main_v2.ino

@@ -0,0 +1,587 @@
+#include <prg_342.h>
+#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));
+  }
+}

main_v2_clean/main_v2_clean.ino

@@ -0,0 +1,469 @@
+#include <prg_342.h>
+#include <Datalogger.h>
+#include <pid.h>
+#include <ArduinoJson.h>
+#include <string>
+#include <math.h>
+
+#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 &parameter) {
+  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*/
+  }
+}

main_v3/main_v3.ino

@@ -0,0 +1,623 @@
+#include <prg_342.h>
+#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));
+  }
+}

main_v4/main_v4.ino

@@ -0,0 +1,57 @@
+#include <PID_v1.h>
+#include <prg_342.h>
+
+#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
+  }
+}

tuning/tuning.ino

@@ -0,0 +1,90 @@
+#include <pidautotuner.h>
+#include <prg_342.h>
+
+void setup() {
+
+  PRG_342 prg = PRG_342();
+  analogWriteResolution(12);
+  analogReadResolution(12);
+  prg.safeShutdown(5);  // Sicheres Herunterfahren des Ausgangs
+
+  Serial.begin(115200);
+
+  Serial.println("Starte in 5 Sekunden.");
+
+  delay(5000);
+
+  Serial.println("Start.");
+
+  PIDAutotuner tuner = PIDAutotuner();
+
+  // Set the target value to tune to
+  // This will depend on what you are tuning. This should be set to a value within
+  // the usual range of the setpoint. For low-inertia systems, values at the lower
+  // end of this range usually give better results. For anything else, start with a
+  // value at the middle of the range.
+  tuner.setTargetInputValue(5000);
+
+  // Set the loop interval in microseconds
+  // This must be the same as the interval the PID control loop will run at
+  tuner.setLoopInterval(1);
+
+  // Set the output range
+  // These are the minimum and maximum possible output values of whatever you are
+  // using to control the system (Arduino analogWrite, for example, is 0-255)
+  tuner.setOutputRange(0, 4095 * 0.5);
+
+  // Set the Ziegler-Nichols tuning mode
+  // Set it to either PIDAutotuner::ZNModeBasicPID, PIDAutotuner::ZNModeLessOvershoot,
+  // or PIDAutotuner::ZNModeNoOvershoot. Defaults to ZNModeNoOvershoot as it is the
+  // safest option.
+  tuner.setZNMode(PIDAutotuner::ZNModeBasicPID);
+
+  // This must be called immediately before the tuning loop
+  // Must be called with the current time in microseconds
+  tuner.startTuningLoop(micros());
+
+  // Run a loop until tuner.isFinished() returns true
+  long microseconds;
+  while (!tuner.isFinished()) {
+
+    // This loop must run at the same speed as the PID control loop being tuned
+    long prevMicroseconds = microseconds;
+    microseconds = micros();
+
+    // Get input value here (temperature, encoder position, velocity, etc)
+    double input = prg.getTorque(false, 10);
+
+    // Call tunePID() with the input value and current time in microseconds
+    double output = tuner.tunePID(input, microseconds);
+
+    // Set the output - tunePid() will return values within the range configured
+    // by setOutputRange(). Don't change the value or the tuning results will be
+    // incorrect.
+    prg.setOutput(output, false);
+
+    // This loop must run at the same speed as the PID control loop being tuned
+    while (micros() - microseconds < 1) delayMicroseconds(1);
+  }
+
+  // Turn the output off here.
+  prg.setOutput(0);
+
+  // Get PID gains - set your PID controller's gains to these
+  double kp = tuner.getKp();
+  double ki = tuner.getKi();
+  double kd = tuner.getKd();
+
+  Serial.println("#####");
+  Serial.print("Kp: ");
+  Serial.println(kp);
+  Serial.print("Ki: ");
+  Serial.println(ki);
+  Serial.print("Kd: ");
+  Serial.println(kd);
+  Serial.println("#####");
+  Serial.println("Abgeschlossen");
+}
+
+void loop() {
+}