Komplette Umstellung auf v2 clean
This commit is contained in:
692
main/main.ino
692
main/main.ino
@@ -1,293 +1,469 @@
|
||||
#include <prg_342.h>
|
||||
#include <Datalogger.h>
|
||||
#include <pid.h>
|
||||
#include <ArduinoJson.h>
|
||||
#include <string>
|
||||
#include <math.h>
|
||||
|
||||
PRG_342 prg = PRG_342();
|
||||
#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
|
||||
|
||||
int angle = 0;
|
||||
int lastAngle = 0;
|
||||
int actTorque = 0;
|
||||
int setpoint = 0;
|
||||
double lastOutput;
|
||||
#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.0;
|
||||
float Ki = 0.1;
|
||||
float Kd = 0.0;
|
||||
int Pout;
|
||||
int Iout;
|
||||
int Dout;
|
||||
|
||||
int error;
|
||||
/*float Kp = 1.0;
|
||||
float Ki = 0.333;
|
||||
float Kd = 1.0;*/
|
||||
|
||||
bool OFFSET_ON = true;
|
||||
bool manOut = false;
|
||||
float WinkelArray[181];
|
||||
|
||||
bool PID = false;
|
||||
unsigned char PID_DELAY = 1; //in ms
|
||||
unsigned int Imax = 217; //in mA
|
||||
bool PRINT_DEBUG = false;
|
||||
bool DEBUG_MODE = false;
|
||||
// Steuervariablen
|
||||
bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren
|
||||
bool Pon = true; // PID-Regelung ein-/ausschalten
|
||||
bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten
|
||||
|
||||
// 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];
|
||||
// Initialisierung der verschiedenen Timer
|
||||
unsigned long logtime;
|
||||
|
||||
void setup() {
|
||||
analogWriteResolution(12); // Set the resolution to 12 bits (0-4095)
|
||||
analogReadResolution(12);
|
||||
setAnalogResolution(12);
|
||||
|
||||
Serial.begin(115200);
|
||||
|
||||
prg.safeShutdown(5);
|
||||
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);
|
||||
|
||||
timer1 = timer2 = PIDtimer = millis();
|
||||
angle = prg.getAngle();
|
||||
lastAngle = angle;
|
||||
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) {
|
||||
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;
|
||||
}
|
||||
String command = receiveString();
|
||||
parseCommand(command);
|
||||
Serial.read();
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
int deltaAngle = angle - lastAngle;
|
||||
RotSpeed = deltaAngle * 5; // Umrechnen in Winkel pro Sekunde
|
||||
lastAngle = angle;
|
||||
|
||||
void DEBUG_TIMER() {
|
||||
if (millis() - timer1 >= 500) {
|
||||
timer1 = millis();
|
||||
PRINT_DEBUG = true;
|
||||
} else {
|
||||
PRINT_DEBUG = false;
|
||||
if (RotSpeed > 0) {
|
||||
if (CW == 0) {
|
||||
Richtungswechsel = 1;
|
||||
}
|
||||
CW = 1;
|
||||
} else if (RotSpeed < 0) {
|
||||
if (CW == 1) {
|
||||
Richtungswechsel = 1;
|
||||
}
|
||||
CW = 0;
|
||||
}
|
||||
}
|
||||
|
||||
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("####################");
|
||||
}
|
||||
// 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("####################");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (DEBUG_MODE) {
|
||||
DEBUG_TIMER();
|
||||
DEBUG_PRINTER();
|
||||
|
||||
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];
|
||||
}
|
||||
COMMUNICATION_HANDLER();
|
||||
SPEEDCHECK();
|
||||
//Serial.println(received);
|
||||
return received;
|
||||
}
|
||||
|
||||
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();
|
||||
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*/
|
||||
}
|
||||
|
||||
prg.updateActOut();
|
||||
prg.isRunning = false;
|
||||
}
|
||||
@@ -1,587 +0,0 @@
|
||||
#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));
|
||||
}
|
||||
}
|
||||
@@ -1,469 +0,0 @@
|
||||
#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 ¶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*/
|
||||
}
|
||||
}
|
||||
@@ -1,623 +0,0 @@
|
||||
#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));
|
||||
}
|
||||
}
|
||||
@@ -1,57 +0,0 @@
|
||||
#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
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user