From d62c0e58a9dca13cede017821a48f79162d5711a Mon Sep 17 00:00:00 2001 From: Musab Erdem Date: Thu, 18 Jul 2024 11:57:11 +0200 Subject: [PATCH] Synchronisierung. --- Libraries/datalogger/Datalogger.cpp | 51 +++ Libraries/datalogger/Datalogger.h | 31 ++ Libraries/datalogger/example.ino | 45 ++ Libraries/datalogger/pid.cpp | 71 ++++ Libraries/datalogger/pid.h | 32 ++ alpha/alpha.ino | 113 +++++ debug/debug.ino | 46 ++ main/main.ino | 292 ++++++++++++- main_v2/main_v2.ino | 587 ++++++++++++++++++++++++++ main_v2_clean/main_v2_clean.ino | 469 +++++++++++++++++++++ main_v3/main_v3.ino | 623 ++++++++++++++++++++++++++++ main_v4/main_v4.ino | 57 +++ tuning/tuning.ino | 90 ++++ 13 files changed, 2503 insertions(+), 4 deletions(-) create mode 100644 Libraries/datalogger/Datalogger.cpp create mode 100644 Libraries/datalogger/Datalogger.h create mode 100644 Libraries/datalogger/example.ino create mode 100644 Libraries/datalogger/pid.cpp create mode 100644 Libraries/datalogger/pid.h create mode 100644 alpha/alpha.ino create mode 100644 debug/debug.ino create mode 100644 main_v2/main_v2.ino create mode 100644 main_v2_clean/main_v2_clean.ino create mode 100644 main_v3/main_v3.ino create mode 100644 main_v4/main_v4.ino create mode 100644 tuning/tuning.ino diff --git a/Libraries/datalogger/Datalogger.cpp b/Libraries/datalogger/Datalogger.cpp new file mode 100644 index 0000000..dff04e6 --- /dev/null +++ b/Libraries/datalogger/Datalogger.cpp @@ -0,0 +1,51 @@ +#include "Datalogger.h" + +Datalogger::Datalogger() {} + +void Datalogger::addData(const std::map& 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(); +} diff --git a/Libraries/datalogger/Datalogger.h b/Libraries/datalogger/Datalogger.h new file mode 100644 index 0000000..660eb2a --- /dev/null +++ b/Libraries/datalogger/Datalogger.h @@ -0,0 +1,31 @@ +#ifndef DATALOGGER_H +#define DATALOGGER_H + +#include +#include +#include + +struct DataEntry { + std::map values; +}; + +struct Config { + String parameter; + String value; +}; + +class Datalogger { +public: + Datalogger(); + + void addData(const std::map& data); + void addConfig(String parameter, String value); + void logToSerial(); + void clear(); + +private: + std::vector dataEntries; + std::vector configs; +}; + +#endif // DATALOGGER_H diff --git a/Libraries/datalogger/example.ino b/Libraries/datalogger/example.ino new file mode 100644 index 0000000..eed2747 --- /dev/null +++ b/Libraries/datalogger/example.ino @@ -0,0 +1,45 @@ +#include +#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 data1; + data1["Time"] = "0.1"; + data1["Input"] = "1"; + data1["Output"] = "5"; + logger.addData(data1); + + std::map data2; + data2["Time"] = "0.2"; + data2["Input"] = "2"; + data2["Output"] = "10"; + logger.addData(data2); + + std::map 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 +} \ No newline at end of file diff --git a/Libraries/datalogger/pid.cpp b/Libraries/datalogger/pid.cpp new file mode 100644 index 0000000..c665373 --- /dev/null +++ b/Libraries/datalogger/pid.cpp @@ -0,0 +1,71 @@ +#include "PID.h" +#include + +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; +} diff --git a/Libraries/datalogger/pid.h b/Libraries/datalogger/pid.h new file mode 100644 index 0000000..07c28fa --- /dev/null +++ b/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 diff --git a/alpha/alpha.ino b/alpha/alpha.ino new file mode 100644 index 0000000..5cebd8f --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/debug/debug.ino b/debug/debug.ino new file mode 100644 index 0000000..e8d9957 --- /dev/null +++ b/debug/debug.ino @@ -0,0 +1,46 @@ +#include + +// 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); + } +} \ No newline at end of file diff --git a/main/main.ino b/main/main.ino index e20cfc7..59f1ecf 100644 --- a/main/main.ino +++ b/main/main.ino @@ -1,9 +1,293 @@ -void setup() { - // put your setup code here, to run once: +#include +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(); -} \ No newline at end of file + 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; +} diff --git a/main_v2/main_v2.ino b/main_v2/main_v2.ino new file mode 100644 index 0000000..7403613 --- /dev/null +++ b/main_v2/main_v2.ino @@ -0,0 +1,587 @@ +#include +#define MEAS_SAMPLES 4000 +#define MEAS_TIME 100 +#define K_FAKTOR 1.0995380532183 + +// Initialisiere die PRG_342-Klasse +PRG_342 prg = PRG_342(K_FAKTOR); + +//Messung / Logging +bool LOGGING = false; +int measTorque[MEAS_SAMPLES]; +int measAngle[MEAS_SAMPLES]; +unsigned int measSetpoint[MEAS_SAMPLES]; +unsigned int measMillis[MEAS_SAMPLES]; +unsigned int measVolt[MEAS_SAMPLES]; +unsigned int logCounter; +unsigned long timer; +unsigned long diff; +float ALPHA = 0.0008; + +// Deklaration der Variablen für die Steuerung und Messung +int angle = 0; // Aktueller Winkel +int lastAngle = 0; // Letzter gemessener Winkel +int actTorque = 0; // Aktuelles Drehmoment +int setpoint = 0; // Sollwert für Drehmoment +float tolerance = 0.0; +//int tolerance = 500; +float lowerBound; +float upperBound; +static int lastOutput = 0; +unsigned int manVolt = 0; +unsigned char CW; +unsigned char Richtungswechsel; + +// PID-Reglerparameter +//float Kp = 5.15; +//float Ki = 0.02; +//float Kd = 2273.47; +float Kp = 0; +float Ki = 0.010; +float Kd = 0; +float I_ALPHA = 0.0; +float I_old = 0; +int Pout; +int Iout; +int Dout; + +int error; // Fehler zwischen Soll- und Istwert (Regeldifferenz e) + +// Steuervariablen +bool OFFSET_ON = true; +bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren +bool PID = false; // PID-Regelung ein-/ausschalten +unsigned char PID_DELAY = 0; // Verzögerung zwischen PID-Regelungen in ms +unsigned int Imax = 217; // Maximalstrom in mA +bool PRINT_DEBUG = true; // Debug-Ausgaben ein-/ausschalten +bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten +bool ARDUINO_PLOTTER = false; + +// Variablen für PID-Berechnungen +float integral = 0; // Integralterm +int derivative; // Differenzialterm +int previousError = 0; // Vorheriger Fehler +unsigned long lastTime = 0; +unsigned long currentTime; +unsigned long timeChange; + +// Initialisierung der verschiedenen Timer +unsigned long timer1; +unsigned long timer2; +unsigned long PIDtimer; +unsigned long logTimer; + +float RotSpeed; // Rotationsgeschwindigkeit +unsigned int MilliAmpPerSecond = 25; // Milliampere pro Sekunde +unsigned int maxChange; // Maximale Änderung pro Schleifendurchlauf + +char serBuffer[4]; // Buffer für serielle Eingaben + +void setup() { + + for (int i = 0; i < MEAS_SAMPLES; i++) { + measTorque[i - 1] = 0; + measAngle[i - 1] = 0; + measSetpoint[i - 1] = 0; + measMillis[i - 1] = 0; + measVolt[i - 1] = 0; + } + + analogWriteResolution(12); + analogReadResolution(12); + + Serial.begin(115200); // Beginne serielle Kommunikation + + prg.safeShutdown(5); // Sicheres Herunterfahren des Ausgangs + //prg.AutoDecog(); + + // Initialisiere Timer mit der aktuellen Zeit + timer1 = timer2 = PIDtimer = logTimer = millis(); + + // Lese den initialen Winkel + angle = prg.getAngle(); + lastAngle = angle; +} + +void Datalogger() { + if (LOGGING) { + if (millis() - logTimer >= MEAS_TIME) { + logTimer = millis(); + logCounter++; + + measTorque[logCounter] = actTorque; + measAngle[logCounter] = angle; + measSetpoint[logCounter] = setpoint; + measMillis[logCounter] = logCounter * MEAS_TIME; + measVolt[logCounter] = prg.getOutVolt(); + } + } +} + +void loop() { + angle = prg.getAngle(); + actTorque = prg.getTorque(true, ALPHA); + // Aktualisiere den analogen Ausgang für den aktuellen Drehmoment + prg.updateActOut(actTorque); + + // Prüfe, ob der Debug-Modus aktiviert ist + DEBUG_TIMER(); // Aktualisierung des Debug-Timers + if (DEBUG_MODE) { + DEBUG_PRINTER(); // Debug Informationen ausgeben + } + + if (ARDUINO_PLOTTER) { + Arduino_Plotter(); + } + + Datalogger(); + + // Verarbeitung der seriellen Schnittstelle + COMMUNICATION_HANDLER(); + + // Rotationsgeschwindigkeit aktualisieren + SPEEDCHECK(); + + + PID_CONTROL(); // Ausführung der PID-Regelung + + // setze isRunning zurück, damit es durch den Interrupt des Drehgebers wieder gesetzt werden kann + prg.isRunning = false; +} + + +void COMMUNICATION_HANDLER() { + // Verarbeite eingehende Befehle von der seriellen Schnittstelle + if (Serial.available() > 0) { + delay(10); + unsigned int SerIn = Serial.read(); + switch (SerIn) { + case 'a': + // Drehwinkel und Drehmoment ausgeben + Serial.print((float)angle / 1000, 1); + Serial.print(";"); + Serial.println((float)actTorque / 1000, 1); + break; + + case 'e': + // safeShutdown ausführen + prg.safeShutdown(10); + break; + + case 't': + // Drehmoment nullen + prg.tareTorque(); + break; + + case 'w': + // Winkel nullen + prg.tareAngle(); + break; + + case 'o': + // Offset für Drehmoment deaktivieren + OFFSET_ON = !OFFSET_ON; + break; + + // Einstellen der Parameter Kp, Ki und Kd + case 'p': + delay(10); + Serial.readBytes(serBuffer, 4); + Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Kp: "); + Serial.println(Kp); + break; + + case 'i': + delay(10); + Serial.readBytes(serBuffer, 4); + Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Ki: "); + Serial.println(Ki); + break; + + case 'd': + delay(10); + Serial.readBytes(serBuffer, 4); + Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Kd: "); + Serial.println(Kd); + break; + + case 'j': + delay(10); + Serial.readBytes(serBuffer, 4); + I_ALPHA = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("I_ALPHA: "); + Serial.println(I_ALPHA); + break; + + case 's': + // Soll-Wert Einstellung + delay(10); + Serial.readBytes(serBuffer, 4); + setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("setpoint: "); + Serial.println(setpoint); + break; + + case 'm': + // Spannungsausgabe auf manuell stellen + manOut = !manOut; + break; + + case 'v': + // Ausgangsspannung einstellen (manOut muss auf true sein) + if (manOut) { + Serial.readBytes(serBuffer, 4); + manVolt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); + //prg.setOutput((float)manVolt * 0.1575, false); + float outVal = (float)manVolt * 0.1575; + prg.ActOut = outVal; + analogWrite(prg.TORQUE_OUT_PIN, outVal); + } + break; + + case 'x': + // Regelung ein-/ausschalten + PID = !PID; + error = 0; + previousError = 0; + integral = 0; + break; + + case 'r': + // PID-Parameter zurücksetzen + error = 0; + previousError = 0; + integral = 0; + derivative = 0; + Pout = 0; + Iout = 0; + Dout = 0; + break; + + case 'f': + // Dynamic Decogging ausführen + prg.DynDecog(prg.ActOut); + break; + + case 'k': + // Einstellen des maximalen Stromanstiegs + delay(10); + Serial.readBytes(serBuffer, 4); + MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); + Serial.print("Max Milliampere per second: "); + Serial.println(MilliAmpPerSecond); + break; + + case 'b': + // DEBUG_MODE switch + DEBUG_MODE = !DEBUG_MODE; + break; + + case 'l': + LOGGING_HANDLER(); + break; + + case 'y': + // Automatic Measurement + timer = millis(); + diff = millis() - timer; + LOGGING_HANDLER(); + PID = true; + while (diff <= 150000) { + angle = prg.getAngle(); + actTorque = prg.getTorque(true, ALPHA); + prg.updateActOut(actTorque); + + Datalogger(); + + /*if (diff > 20000 && diff <= 50000) { + setpoint = 5000; + } + if (diff > 50000 && diff <= 80000) { + setpoint = 10000; + } + if (diff > 80000 && diff <= 110000) { + setpoint = 15000; + } + if (diff > 110000 && diff <= 140000) { + setpoint = 5000; + }*/ + + if (angle > 0 && angle <= 45000) { + setpoint = 10000; + } + if (angle > 45000 && angle <= 65000) { + setpoint = 15000; + } + if (angle > 65000 && angle <= 90000) { + setpoint = 10000; + } + + + SPEEDCHECK(); + + PID_CONTROL(); + + diff = millis() - timer; + + Serial.print("elapsed Time: "); + Serial.print((float)diff / 1000, 3); + Serial.print(" actual Angle: "); + Serial.println((float)angle / 1000, 3); + } + LOGGING_HANDLER(); + PID = false; + break; + + default: + // Unbekannte Befehle bearbeiten + Serial.print("Unbekannter Befehl: "); + Serial.println(SerIn); + break; + } + } +} + +void printData() { + Serial.print("Kp:"); + Serial.print(";"); + Serial.println(Kp, 3); + Serial.print("Ki:"); + Serial.print(";"); + Serial.println(Ki, 3); + Serial.print("Kd:"); + Serial.print(";"); + Serial.println(Kd, 3); + Serial.print("I_ALPHA:"); + Serial.print(";"); + Serial.println(I_ALPHA, 6); + Serial.print("Drehrichtung:"); + Serial.print(";"); + if (CW) { + Serial.println("CW"); + } else { + Serial.println("CCW"); + } + + Serial.println("Zeit in Sekunden;Winkel;Soll-Drehmoment;Ist-Drehmoment;Erregerspannung;Winkel in Grad;Soll-Drehmoment in Nm;Ist-Drehmoment in Nm;Erregerspannung in V"); + for (int i = 0; i < MEAS_SAMPLES; i++) { + if (measMillis[i] > 0) { + Serial.print((float)measMillis[i] / 1000.0, 2); + Serial.print(";"); + Serial.print(measAngle[i]); + Serial.print(";"); + Serial.print(measSetpoint[i]); + Serial.print(";"); + Serial.print(measTorque[i]); + Serial.print(";"); + Serial.print(measVolt[i]); + Serial.print(";"); + Serial.print(abs((float)measAngle[i] / 1000.0), 3); + Serial.print(";"); + Serial.print(abs((float)measSetpoint[i] / 1000.0), 3); + Serial.print(";"); + Serial.print(abs((float)measTorque[i] / 1000.0), 3); + Serial.print(";"); + Serial.println(abs((float)measVolt[i] / 1000.0), 3); + } + } + for (int i = 0; i < MEAS_SAMPLES; i++) { + measTorque[i - 1] = 0; + measAngle[i - 1] = 0; + measSetpoint[i - 1] = 0; + measMillis[i - 1] = 0; + measVolt[i - 1] = 0; + } +} + +void LOGGING_HANDLER() { + // LOGGING switch + LOGGING = !LOGGING; + if (LOGGING) { + logCounter = 0; + } else { + printData() + } +} + +void PID_CONTROL() { + lowerBound = (float)setpoint - ((float)setpoint * tolerance); + upperBound = (float)setpoint + ((float)setpoint * tolerance); + //lowerBound = (float)setpoint - tolerance; + //upperBound = (float)setpoint + tolerance; + // Timing des PID-Reglers + if (millis() - PIDtimer >= PID_DELAY) { + + if (Richtungswechsel) { + integral = 0; + derivative = 0; + Richtungswechsel = 0; + } + + PIDtimer = millis(); // Setze den Timer zurück + // Aktualisierung des Zeitstempels und Berechnung der vergangenen Zeit seit dem letzten Update + currentTime = millis(); // Lese die aktuelle Zeit in Millisekunden + timeChange = currentTime - lastTime; // Zeitdifferenz zum letzten Aufruf + lastTime = currentTime; // Setze den letzten Zeitstempel auf die aktuelle Zeit für den nächsten Durchlauf + + + // Überprüfe, ob die PID-Regelung aktiviert ist, keine manuelle Ausgabe erfolgt, + // das gemessene Drehmoment die Schwelle überschreitet und die Geschwindigkeit hoch genug ist + // ODER das gemessene Drehmoment den Sollwert übersteigt (Antrieb auf Block) + if (PID && !manOut && ((abs(actTorque) >= 5000 || abs(RotSpeed) >= 400) || (abs(actTorque) > setpoint)) && (abs(actTorque) < lowerBound || abs(actTorque) > upperBound)) { + + // Aktuelle Werte auslesen + //actTorque = abs(prg.getTorque(true, 100)); // Aktuelles Drehmoment + //angle = abs(prg.getAngle()); // Aktueller Winkel + + // Berechnung der Regeldifferenz + error = abs(setpoint) - abs(actTorque); // Differenz zwischen Sollwert und tatsächlich gemessenem Drehmoment + + // Berechnung des proportionalen Anteils der PID-Regelung + Pout = Kp * error; // Proportionaler Output basiert auf dem aktuellen Fehler und dem Proportional-Faktor Kp + /* + // Berechnung des integralen Anteils der PID-Regelung + integral += error * ((float)timeChange / 1000.0); // Addition des Fehlers über die Zeit (Integration des Fehlers) + integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden + Iout = Ki * integral; // Berechnung des integralen Output basierend auf dem Integral und dem Integral-Faktor Ki + */ + + // Berechnung des adaptiv gewichteten integralen Anteils + float weight = 1.0 + (1 - exp(-PHI_ALA * abs(error))); // Gewichtungsfaktor abhängig vom Fehler + integral += weight * error * ((float)timeChange / 1000.0); // Anpassung der Integration des Fehlers + integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden + Iout = Ki * integral; + + // Berechnung des differenziellen Anteils der PID-Regelung + derivative = (error - previousError) / ((float)timeChange / 1000.0); // Berechnung der Änderungsrate des Fehlers + Dout = Kd * derivative; // Differenzieller Output basiert auf der Änderungsrate des Fehlers und dem Differenzial-Faktor Kd + + // Berechnung des gesamten PID-Output + int output = Pout + Iout + Dout; // Summe der einzelnen PID-Komponenten + + // Begrenzung des Ausganges, um Cogging zu vermeiden + // Der Strom darf nicht auf 0 sinken, während die Hysteresebremse stillsteht + output = constrain(output, 0, 4095); + + // Begrenzung des Stromanstieges, um Cogging vorzubeugen + //maxChange = ((float)MilliAmpPerSecond * 4095.0 * (float)PID_DELAY) / ((float)Imax * 1000); // Umrechnung des eingegebenen Stromwertes / Sekunde in einen binären Wert, angepasst auf den Ausgang + /*maxChange = 1; + if (abs(output - lastOutput) > maxChange) { + if (output > lastOutput) { + output = lastOutput + maxChange; // Erhöht den Output + } else { + output = lastOutput - maxChange; // Verringert den Output + } + }*/ + + // Den Wert output am Ausgang (an die Hysteresebremse) anlegen + prg.setOutput(output, false); + lastOutput = output; // Aktualisiere den letzten Output-Wert + + // Aktualisierung des vorherigen Fehler + previousError = error; + } + } +} + + +void SPEEDCHECK() { + // Überprüfen der Rotationsgeschwindigkeit + if (millis() - timer2 >= 200) { + timer2 = millis(); + int deltaAngle = angle - lastAngle; + RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde + lastAngle = angle; + + if (RotSpeed > 0) { + if (CW == 0) { + Richtungswechsel = 1; + } + CW = 1; + } else if (RotSpeed < 0) { + if (CW == 1) { + Richtungswechsel = 1; + } + CW = 0; + } + } +} + +void DEBUG_TIMER() { + // Timer für Debug-Ausgaben + if (millis() - timer1 >= 500) { + timer1 = millis(); + PRINT_DEBUG = true; + } else { + PRINT_DEBUG = false; + } +} + +void DEBUG_PRINTER() { + // Debug-Informationen ausgeben + if (PRINT_DEBUG) { + Serial.println("####################"); + Serial.print("Winkel: "); + Serial.println((float)angle / 1000, 3); + Serial.print("Drehmoment: "); + Serial.println((float)actTorque / 1000, 3); + Serial.print("Geschwindigkeit: "); + Serial.println(RotSpeed); + Serial.print("Drehrichtung: "); + Serial.println(CW); + Serial.print("Ausgangsspannung: "); + Serial.println((float)prg.getOutVolt() / 1000, 3); + Serial.print("Setpoint: "); + Serial.println((float)setpoint / 1000, 3); + Serial.print("Error: "); + Serial.println((float)error / 1000, 3); + Serial.print("Kp: "); + Serial.println(Kp, 3); + Serial.print("Ki: "); + Serial.println(Ki, 3); + Serial.print("Kd: "); + Serial.println(Kd, 3); + Serial.print("I_ALPHA:"); + Serial.println(I_ALPHA); + Serial.print("Pout: "); + Serial.println(Pout); + Serial.print("Iout: "); + Serial.println(Iout); + Serial.print("Dout: "); + Serial.println(Dout); + Serial.print("output: "); + Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); + Serial.print("Integral: "); + Serial.println(integral); + Serial.print("PID: "); + Serial.println(PID); + Serial.print("manOut: "); + Serial.println(manOut); + Serial.print("manVolt: "); + Serial.println(manVolt); + Serial.print("Max mA / s (SET): "); + Serial.println(MilliAmpPerSecond); + Serial.print("Max mA / s (ACT): "); + Serial.println((Imax * maxChange * 1000) / (4095 * PID_DELAY)); + Serial.print("maxChange: "); + Serial.println(maxChange); + Serial.print("isRunning: "); + Serial.println(prg.isRunning); + Serial.print("LOGGING: "); + Serial.println(LOGGING); + Serial.println("####################"); + } +} + +void Arduino_Plotter() { + if (PRINT_DEBUG) { + Serial.print("Soll:"); + Serial.print(setpoint); + Serial.print(","); + Serial.print("Ist:"); + Serial.println(abs(actTorque)); + } +} diff --git a/main_v2_clean/main_v2_clean.ino b/main_v2_clean/main_v2_clean.ino new file mode 100644 index 0000000..c1f6b76 --- /dev/null +++ b/main_v2_clean/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 diff --git a/main_v3/main_v3.ino b/main_v3/main_v3.ino new file mode 100644 index 0000000..b183f5c --- /dev/null +++ b/main_v3/main_v3.ino @@ -0,0 +1,623 @@ +#include +#define MEAS_SAMPLES 4000 +#define MEAS_TIME 100 +#define K_FAKTOR 1.0995380532183 + +// Initialisiere die PRG_342-Klasse +PRG_342 prg = PRG_342(K_FAKTOR); + +//Messung / Logging +bool LOGGING = false; +int measTorque[MEAS_SAMPLES]; +int measAngle[MEAS_SAMPLES]; +unsigned int measSetpoint[MEAS_SAMPLES]; +unsigned int measMillis[MEAS_SAMPLES]; +unsigned int measVolt[MEAS_SAMPLES]; +unsigned int logCounter; +unsigned long timer; +unsigned long diff; +float ALPHA = 0.001; + +// Deklaration der Variablen für die Steuerung und Messung +int angle = 0; // Aktueller Winkel +int lastAngle = 0; // Letzter gemessener Winkel +int actTorque = 0; // Aktuelles Drehmoment +int setpoint = 0; // Sollwert für Drehmoment +float tolerance = 0.0; +//int tolerance = 500; +float lowerBound; +float upperBound; +static int lastOutput = 0; +unsigned int manVolt = 0; +unsigned char CW; +unsigned char Richtungswechsel; + +// PID-Reglerparameter +//float Kp = 5.15; +//float Ki = 0.02; +//float Kd = 2273.47; +float Kp = 0; +float Ki = 0.010; +float Kd = 0; +float I_ALPHA = 0.0; +float I_old = 0; +int Pout; +int Iout; +int Dout; +float MAX_INTEGRAL; + +int error; // Fehler zwischen Soll- und Istwert (Regeldifferenz e) + +// Steuervariablen +bool OFFSET_ON = true; +bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren +bool PID = false; // PID-Regelung ein-/ausschalten +unsigned long PID_DELAY = 0; // Verzögerung zwischen PID-Regelungen in ms +unsigned int Imax = 217; // Maximalstrom in mA +bool PRINT_DEBUG = true; // Debug-Ausgaben ein-/ausschalten +bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten +bool ARDUINO_PLOTTER = false; + +// Variablen für PID-Berechnungen +float integral = 0; // Integralterm +int derivative; // Differenzialterm +int previousError = 0; // Vorheriger Fehler +unsigned long lastTime = 0; +unsigned long currentTime; +unsigned long timeChange; + +// Initialisierung der verschiedenen Timer +unsigned long timer1; +unsigned long timer2; +unsigned long PIDtimer; +unsigned long logTimer; + +float RotSpeed; // Rotationsgeschwindigkeit +unsigned int MilliAmpPerSecond = 25; // Milliampere pro Sekunde +unsigned int maxChange; // Maximale Änderung pro Schleifendurchlauf + +char serBuffer[4]; // Buffer für serielle Eingaben + +void setup() { + + for (int i = 0; i < MEAS_SAMPLES; i++) { + measTorque[i - 1] = 0; + measAngle[i - 1] = 0; + measSetpoint[i - 1] = 0; + measMillis[i - 1] = 0; + measVolt[i - 1] = 0; + } + + analogWriteResolution(12); + analogReadResolution(12); + + Serial.begin(115200); // Beginne serielle Kommunikation + + prg.safeShutdown(5); // Sicheres Herunterfahren des Ausgangs + //prg.AutoDecog(); + + // Initialisiere Timer mit der aktuellen Zeit + timer1 = timer2 = PIDtimer = logTimer = millis(); + + // Lese den initialen Winkel + angle = prg.getAngle(); + lastAngle = angle; +} + +void Datalogger() { + if (LOGGING) { + if (millis() - logTimer >= MEAS_TIME) { + logTimer = millis(); + logCounter++; + + measTorque[logCounter] = actTorque; + measAngle[logCounter] = angle; + measSetpoint[logCounter] = setpoint; + measMillis[logCounter] = logCounter * MEAS_TIME; + measVolt[logCounter] = prg.getOutVolt(); + } + } +} + +void loop() { + angle = prg.getAngle(); + actTorque = prg.getTorque(true, ALPHA); + // Aktualisiere den analogen Ausgang für den aktuellen Drehmoment + prg.updateActOut(actTorque); + + /* + // Prüfe, ob der Debug-Modus aktiviert ist + DEBUG_TIMER(); // Aktualisierung des Debug-Timers + if (DEBUG_MODE) { + DEBUG_PRINTER(); // Debug Informationen ausgeben + } + + if (ARDUINO_PLOTTER) { + Arduino_Plotter(); + } + + Datalogger(); +*/ + // Verarbeitung der seriellen Schnittstelle + COMMUNICATION_HANDLER(); + + // Rotationsgeschwindigkeit aktualisieren + SPEEDCHECK(); + + + PID_CONTROL(); // Ausführung der PID-Regelung + + // setze isRunning zurück, damit es durch den Interrupt des Drehgebers wieder gesetzt werden kann + prg.isRunning = false; +} + + +void COMMUNICATION_HANDLER() { + // Verarbeite eingehende Befehle von der seriellen Schnittstelle + if (Serial.available() > 0) { + delay(10); + unsigned int SerIn = Serial.read(); + switch (SerIn) { + case 'a': + // Drehwinkel und Drehmoment ausgeben + Serial.print((float)angle / 1000, 1); + Serial.print(";"); + Serial.println((float)actTorque / 1000, 1); + break; + + case 'e': + // safeShutdown ausführen + prg.safeShutdown(10); + break; + + case 't': + // Drehmoment nullen + prg.tareTorque(); + break; + + case 'w': + // Winkel nullen + prg.tareAngle(); + break; + + case 'o': + // Offset für Drehmoment deaktivieren + OFFSET_ON = !OFFSET_ON; + break; + + // Einstellen der Parameter Kp, Ki und Kd + case 'p': + delay(10); + Serial.readBytes(serBuffer, 4); + Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Kp: "); + Serial.println(Kp); + break; + + case 'i': + delay(10); + Serial.readBytes(serBuffer, 4); + Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Ki: "); + Serial.println(Ki); + break; + + case 'd': + delay(10); + Serial.readBytes(serBuffer, 4); + Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("Kd: "); + Serial.println(Kd); + break; + + case 'j': + delay(10); + Serial.readBytes(serBuffer, 4); + I_ALPHA = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("I_ALPHA: "); + Serial.println(I_ALPHA); + break; + + case 's': + // Soll-Wert Einstellung + delay(10); + Serial.readBytes(serBuffer, 4); + setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001; + Serial.print("setpoint: "); + Serial.println(setpoint); + break; + + case 'm': + // Spannungsausgabe auf manuell stellen + manOut = !manOut; + break; + + case 'v': + // Ausgangsspannung einstellen (manOut muss auf true sein) + if (manOut) { + Serial.readBytes(serBuffer, 4); + manVolt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); + //prg.setOutput((float)manVolt * 0.1575, false); + float outVal = (float)manVolt * 0.1575; + prg.ActOut = outVal; + analogWrite(prg.TORQUE_OUT_PIN, outVal); + } + break; + + case 'x': + // Regelung ein-/ausschalten + PID = !PID; + error = 0; + previousError = 0; + integral = 0; + break; + + case 'r': + // PID-Parameter zurücksetzen + error = 0; + previousError = 0; + integral = 0; + derivative = 0; + Pout = 0; + Iout = 0; + Dout = 0; + break; + + case 'f': + // Dynamic Decogging ausführen + prg.DynDecog(prg.ActOut); + break; + + case 'k': + // Einstellen des maximalen Stromanstiegs + delay(10); + Serial.readBytes(serBuffer, 4); + MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]); + Serial.print("Max Milliampere per second: "); + Serial.println(MilliAmpPerSecond); + break; + + case 'b': + // DEBUG_MODE switch + DEBUG_MODE = !DEBUG_MODE; + break; + + case 'u': + // Open-Loop Test starten + openLoopTest(); + break; + + case 'l': + LOGGING_HANDLER(); + break; + + case 'y': + // Automatic Measurement + timer = millis(); + diff = millis() - timer; + LOGGING_HANDLER(); + PID = true; + while (diff <= 150000) { + angle = prg.getAngle(); + actTorque = prg.getTorque(true, ALPHA); + prg.updateActOut(actTorque); + + Datalogger(); + + /*if (diff > 20000 && diff <= 50000) { + setpoint = 5000; + } + if (diff > 50000 && diff <= 80000) { + setpoint = 10000; + } + if (diff > 80000 && diff <= 110000) { + setpoint = 15000; + } + if (diff > 110000 && diff <= 140000) { + setpoint = 5000; + }*/ + + if (angle > 0 && angle <= 45000) { + setpoint = 10000; + } + if (angle > 45000 && angle <= 65000) { + setpoint = 15000; + } + if (angle > 65000 && angle <= 90000) { + setpoint = 10000; + } + + + SPEEDCHECK(); + + PID_CONTROL(); + + diff = millis() - timer; + + Serial.print("elapsed Time: "); + Serial.print((float)diff / 1000, 3); + Serial.print(" actual Angle: "); + Serial.println((float)angle / 1000, 3); + } + LOGGING_HANDLER(); + PID = false; + break; + + default: + // Unbekannte Befehle bearbeiten + Serial.print("Unbekannter Befehl: "); + Serial.println(SerIn); + break; + } + } +} + +void printData() { + Serial.print("Kp:"); + Serial.print(";"); + Serial.println(Kp, 3); + Serial.print("Ki:"); + Serial.print(";"); + Serial.println(Ki, 3); + Serial.print("Kd:"); + Serial.print(";"); + Serial.println(Kd, 3); + Serial.print("I_ALPHA:"); + Serial.print(";"); + Serial.println(I_ALPHA, 6); + Serial.print("Drehrichtung:"); + Serial.print(";"); + if (CW) { + Serial.println("CW"); + } else { + Serial.println("CCW"); + } + + Serial.println("Zeit in Sekunden;Winkel;Soll-Drehmoment;Ist-Drehmoment;Erregerspannung;Winkel in Grad;Soll-Drehmoment in Nm;Ist-Drehmoment in Nm;Erregerspannung in V"); + for (int i = 0; i < MEAS_SAMPLES; i++) { + if (measMillis[i] > 0) { + Serial.print((float)measMillis[i] / 1000.0, 2); + Serial.print(";"); + Serial.print(measAngle[i]); + Serial.print(";"); + Serial.print(measSetpoint[i]); + Serial.print(";"); + Serial.print(measTorque[i]); + Serial.print(";"); + Serial.print(measVolt[i]); + Serial.print(";"); + Serial.print(abs((float)measAngle[i] / 1000.0), 3); + Serial.print(";"); + Serial.print(abs((float)measSetpoint[i] / 1000.0), 3); + Serial.print(";"); + Serial.print(abs((float)measTorque[i] / 1000.0), 3); + Serial.print(";"); + Serial.println(abs((float)measVolt[i] / 1000.0), 3); + } + } + for (int i = 0; i < MEAS_SAMPLES; i++) { + measTorque[i - 1] = 0; + measAngle[i - 1] = 0; + measSetpoint[i - 1] = 0; + measMillis[i - 1] = 0; + measVolt[i - 1] = 0; + } +} + +void LOGGING_HANDLER() { + // LOGGING switch + LOGGING = !LOGGING; + if (LOGGING) { + logCounter = 0; + } else { + printData(); + } +} + +void PID_CONTROL() { + lowerBound = (float)setpoint - ((float)setpoint * tolerance); + upperBound = (float)setpoint + ((float)setpoint * tolerance); + MAX_INTEGRAL = (4095.0 / Ki) * 0.9; + //lowerBound = (float)setpoint - tolerance; + //upperBound = (float)setpoint + tolerance; + // Timing des PID-Reglers + if (micros() - PIDtimer >= PID_DELAY) { + + if (Richtungswechsel) { + integral = 0; + derivative = 0; + Richtungswechsel = 0; + } + + PIDtimer = micros(); // Setze den Timer zurück + // Aktualisierung des Zeitstempels und Berechnung der vergangenen Zeit seit dem letzten Update + currentTime = millis(); // Lese die aktuelle Zeit in Millisekunden + timeChange = currentTime - lastTime; // Zeitdifferenz zum letzten Aufruf + lastTime = currentTime; // Setze den letzten Zeitstempel auf die aktuelle Zeit für den nächsten Durchlauf + + + // Überprüfe, ob die PID-Regelung aktiviert ist, keine manuelle Ausgabe erfolgt, + // das gemessene Drehmoment die Schwelle überschreitet und die Geschwindigkeit hoch genug ist + // ODER das gemessene Drehmoment den Sollwert übersteigt (Antrieb auf Block) + if (PID && !manOut && ((abs(actTorque) >= 5000 || abs(RotSpeed) >= 400) || (abs(actTorque) > setpoint)) && (abs(actTorque) < lowerBound || abs(actTorque) > upperBound)) { + + // Aktuelle Werte auslesen + //actTorque = abs(prg.getTorque(true, 100)); // Aktuelles Drehmoment + //angle = abs(prg.getAngle()); // Aktueller Winkel + + // Berechnung der Regeldifferenz + error = abs(setpoint) - abs(actTorque); // Differenz zwischen Sollwert und tatsächlich gemessenem Drehmoment + + // Berechnung des proportionalen Anteils der PID-Regelung + Pout = Kp * error; // Proportionaler Output basiert auf dem aktuellen Fehler und dem Proportional-Faktor Kp + /* + // Berechnung des integralen Anteils der PID-Regelung + integral += error * ((float)timeChange / 1000.0); // Addition des Fehlers über die Zeit (Integration des Fehlers) + integral = constrain(integral, -4095000, 4095000); // Begrenzung des integralen Anteils, um Windup zu vermeiden + Iout = Ki * integral; // Berechnung des integralen Output basierend auf dem Integral und dem Integral-Faktor Ki + */ + + // Berechnung des adaptiv gewichteten integralen Anteils + /*float weight = 1.0 + (1 - exp(-I_ALPHA * abs(error))); // Gewichtungsfaktor abhängig vom Fehler + integral += weight * error * ((float)timeChange / 1000.0); // Anpassung der Integration des Fehlers + integral = constrain(integral, -MAX_INTEGRAL, MAX_INTEGRAL); // Begrenzung des integralen Anteils, um Windup zu vermeiden + Iout = Ki * integral;*/ + + integral += error * (((float)timeChange / 1000.0) * 0.5); // Anpassung der Integration des Fehlers + integral = constrain(integral, -MAX_INTEGRAL, MAX_INTEGRAL); // Begrenzung des integralen Anteils, um Windup zu vermeiden + Iout = Ki * integral; + + // Berechnung des differenziellen Anteils der PID-Regelung + derivative = (error - previousError) / ((float)timeChange / 1000.0); // Berechnung der Änderungsrate des Fehlers + Dout = Kd * derivative; // Differenzieller Output basiert auf der Änderungsrate des Fehlers und dem Differenzial-Faktor Kd + + // Berechnung des gesamten PID-Output + int output = Pout + Iout + Dout; // Summe der einzelnen PID-Komponenten + + // Begrenzung des Ausganges, um Cogging zu vermeiden + // Der Strom darf nicht auf 0 sinken, während die Hysteresebremse stillsteht + output = constrain(output, 0, 4095); + + // Begrenzung des Stromanstieges, um Cogging vorzubeugen + //maxChange = ((float)MilliAmpPerSecond * 4095.0 * (float)PID_DELAY) / ((float)Imax * 1000); // Umrechnung des eingegebenen Stromwertes / Sekunde in einen binären Wert, angepasst auf den Ausgang + /*maxChange = 1; + if (abs(output - lastOutput) > maxChange) { + if (output > lastOutput) { + output = lastOutput + maxChange; // Erhöht den Output + } else { + output = lastOutput - maxChange; // Verringert den Output + } + }*/ + + // Den Wert output am Ausgang (an die Hysteresebremse) anlegen + prg.setOutput(output, false); + lastOutput = output; // Aktualisiere den letzten Output-Wert + + // Aktualisierung des vorherigen Fehler + previousError = error; + } + } +} + +void openLoopTest() { + LOGGING = true; // Logging aktivieren + logCounter = 0; // Log-Zähler zurücksetzen + + // Setze die Spannung für die Stufenänderung + int stepVoltage = 3000; // Beispielwert für die Stufenänderung (kann angepasst werden) + prg.setOutput(stepVoltage, false); + unsigned long startTime = millis(); + unsigned long duration = 50000; // Messdauer in Millisekunden (z.B. 10 Sekunden) + + // Messe die Systemantwort für die festgelegte Dauer + while (millis() - startTime < duration) { + angle = prg.getAngle(); + actTorque = prg.getTorque(true, ALPHA); + prg.updateActOut(actTorque); + Datalogger(); + } + + // Setze die Ausgangsspannung wieder auf Null + prg.setOutput(0, false); + LOGGING = false; // Logging deaktivieren + printData(); // Ausgabe der gemessenen Daten +} + + +void SPEEDCHECK() { + // Überprüfen der Rotationsgeschwindigkeit + if (millis() - timer2 >= 200) { + timer2 = millis(); + int deltaAngle = angle - lastAngle; + RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde + lastAngle = angle; + + if (RotSpeed > 0) { + if (CW == 0) { + Richtungswechsel = 1; + } + CW = 1; + } else if (RotSpeed < 0) { + if (CW == 1) { + Richtungswechsel = 1; + } + CW = 0; + } + } +} + +void DEBUG_TIMER() { + // Timer für Debug-Ausgaben + if (millis() - timer1 >= 500) { + timer1 = millis(); + PRINT_DEBUG = true; + } else { + PRINT_DEBUG = false; + } +} + +void DEBUG_PRINTER() { + // Debug-Informationen ausgeben + if (PRINT_DEBUG) { + Serial.println("####################"); + Serial.print("Winkel: "); + Serial.println((float)angle / 1000, 3); + Serial.print("Drehmoment: "); + Serial.println((float)actTorque / 1000, 3); + Serial.print("Geschwindigkeit: "); + Serial.println(RotSpeed); + Serial.print("Drehrichtung: "); + Serial.println(CW); + Serial.print("Ausgangsspannung: "); + Serial.println((float)prg.getOutVolt() / 1000, 3); + Serial.print("Setpoint: "); + Serial.println((float)setpoint / 1000, 3); + Serial.print("Error: "); + Serial.println((float)error / 1000, 3); + Serial.print("Kp: "); + Serial.println(Kp, 3); + Serial.print("Ki: "); + Serial.println(Ki, 3); + Serial.print("Kd: "); + Serial.println(Kd, 3); + Serial.print("I_ALPHA:"); + Serial.println(I_ALPHA); + Serial.print("Pout: "); + Serial.println(Pout); + Serial.print("Iout: "); + Serial.println(Iout); + Serial.print("Dout: "); + Serial.println(Dout); + Serial.print("output: "); + Serial.println((unsigned int)((float)prg.getOutVolt() * 0.1575)); + Serial.print("Integral: "); + Serial.println(integral); + Serial.print("PID: "); + Serial.println(PID); + Serial.print("manOut: "); + Serial.println(manOut); + Serial.print("manVolt: "); + Serial.println(manVolt); + Serial.print("Max mA / s (SET): "); + Serial.println(MilliAmpPerSecond); + Serial.print("Max mA / s (ACT): "); + Serial.println((Imax * maxChange * 1000) / (4095 * (PID_DELAY / 1000.0))); + Serial.print("maxChange: "); + Serial.println(maxChange); + Serial.print("isRunning: "); + Serial.println(prg.isRunning); + Serial.print("LOGGING: "); + Serial.println(LOGGING); + Serial.println("####################"); + } +} + +void Arduino_Plotter() { + if (PRINT_DEBUG) { + Serial.print("Soll:"); + Serial.print(setpoint); + Serial.print(","); + Serial.print("Ist:"); + Serial.println(abs(actTorque)); + } +} diff --git a/main_v4/main_v4.ino b/main_v4/main_v4.ino new file mode 100644 index 0000000..2474286 --- /dev/null +++ b/main_v4/main_v4.ino @@ -0,0 +1,57 @@ +#include +#include + +#define PIN_INPUT A0 +#define PIN_OUTPUT DAC0 +#define K_FAKTOR 1.0995380532183 +#define ALPHA 1 + +PRG_342 prg = PRG_342(K_FAKTOR); + +//Define Variables we'll be connecting to +double Setpoint, Input, Output; +int angle; +int lastAngle; +unsigned long timer1; + +//Specify the links and initial tuning parameters +double Kp = 0, Ki = 0.04, Kd = 0; +PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); + +void setup() {prg.setOutput(4095*0.5, false); + delay(3000); + prg.setOutput(0, false); + delay(2000); + Serial.begin(115200); + + angle = prg.getAngle(); + Input = prg.getTorque(true, ALPHA); + prg.updateActOut(Input); + Setpoint = 15000; + + //turn the PID on + myPID.SetOutputLimits(0, 4095); + myPID.SetMode(AUTOMATIC); +} + +void loop() { + angle = prg.getAngle(); + Input = abs(prg.getTorque(true, ALPHA)); + prg.updateActOut(Input); + + if (SPEEDCHECK() >= 400) { + myPID.Compute(); + } + + prg.setOutput(Output, false); +} + +float SPEEDCHECK() { + // Überprüfen der Rotationsgeschwindigkeit + if (millis() - timer1 >= 200) { + timer1 = millis(); + int deltaAngle = angle - lastAngle; + lastAngle = angle; + return abs(deltaAngle * 5); // Umrechnen in Winkel pro Sekunde + } +} \ No newline at end of file diff --git a/tuning/tuning.ino b/tuning/tuning.ino new file mode 100644 index 0000000..fbf495f --- /dev/null +++ b/tuning/tuning.ino @@ -0,0 +1,90 @@ +#include +#include + +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() { +} \ No newline at end of file