musabe24/t2000_arduino_due
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Synchronisierung.

Browse Source
This commit is contained in:
Musab Erdem
2024-07-18 11:57:11 +02:00
parent 7aad80b38e
commit d62c0e58a9
13 changed files with 2503 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
Libraries/datalogger/Datalogger.cpp Normal file
View File

@@ -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();
}

31
Libraries/datalogger/Datalogger.h Normal file
View File

@@ -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

45
Libraries/datalogger/example.ino Normal file
View File

@@ -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
}

71
Libraries/datalogger/pid.cpp Normal file
View File

@@ -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;
}

32
Libraries/datalogger/pid.h Normal file
View File

@@ -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

113
alpha/alpha.ino Normal file
View File

@@ -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;
}

46
debug/debug.ino Normal file
View File

@@ -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);
}
}

292
main/main.ino
View File

@@ -1,9 +1,293 @@
void setup() {
// put your setup code here, to run once:
#include <prg_342.h>
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;
}

587
main_v2/main_v2.ino Normal file
View File

@@ -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));
}
}

469
main_v2_clean/main_v2_clean.ino Normal file
View File

@@ -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*/
}
}

623
main_v3/main_v3.ino Normal file
View File

@@ -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));
}
}

57
main_v4/main_v4.ino Normal file
View File

@@ -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
}
}

90
tuning/tuning.ino Normal file
View File

@@ -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() {
}