Libraries angepasst, Limits eingefügt, Testsoftware für Messungen angelegt.

main/main.ino

@@ -1,7 +1,7 @@
 //#include <prg_342.h>
-#include <Libraries/PRG_342/prg_342.h>
+#include <prg_342.h>
-#include <Libraries/datalogger/Datalogger.h>
+#include <Datalogger.h>
-#include <Libraries/pid/pid.h>
+#include <pid.h>
 #include <string>
 #include <math.h>
 
@@ -58,6 +58,8 @@ int angle;
 int lastAnglePID;
 int lastAngle;
 int actTorque;
+int upperLimit = 0;
+int lowerLimit = -0;
 //int actTorqueArray[100];
 float filteredTorque = 0;  // gefilterter Wert
 float filteredSetpoint = 0;
@@ -194,7 +196,7 @@ void loop() {
   }*/
   myPID.setTunings(Kp, Ki, Kd);
 
-  if (lastAnglePID != angle && Pon && !manOut && ((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) {
+  if (lastAnglePID != angle && Pon && !manOut && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((abs(actTorque) >= 1000 || abs(RotSpeed) >= 400) || abs(actTorque) > setpoint)) {
     unsigned int output = myPID.compute(setpoint, actTorque);
     prg.setOutput(output);
     prg.ActOut = output;
@@ -212,6 +214,15 @@ void loop() {
   prg.isRunning == false;
 }
 
+bool checkLimits(float setpoint, int actVal, int upperLimit, int lowerLimit) {
+    int error = setpoint - actVal;
+    if (error <= upperLimit && error >= lowerLimit) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
 void interpolateWinkelArray() {
   int lastSetIndex = -1;
 

testsoftware/testsoftware.ino

@@ -0,0 +1,480 @@
+//#include <prg_342.h>
+#include <prg_342.h>
+#include <Datalogger.h>
+#include <pid.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 upperLimit = 0;
+int lowerLimit = -0;
+//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 && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((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;
+}
+
+bool checkLimits(float setpoint, int actVal, int upperLimit, int lowerLimit) {
+    int error = setpoint - actVal;
+    if (error <= upperLimit && error >= lowerLimit) {
+      return true;
+    } else {
+      return 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*/
+  }
+}