326 lines
8.6 KiB
C++
326 lines
8.6 KiB
C++
//#include <prg_342.h>
|
|
#include <prg_342.h>
|
|
#include <Datalogger.h>
|
|
#include <pid.h>
|
|
#include <string>
|
|
#include <math.h>
|
|
|
|
#define DEBUG_PRINT_TIME 500
|
|
#define SPEEDCHECK_TIME 200
|
|
#define PID_DELAY 0
|
|
#define LOG_TIMER_DELAY 100
|
|
#define MEAS_DELAY 1000
|
|
//#define K_FAKTOR 1.0995380532183
|
|
#define K_FAKTOR 1.00
|
|
#define OFFSET 0
|
|
#define ALPHA 0.0008
|
|
#define N_SAMPLES 100
|
|
#define BLOCK_DETECTION_VALUE setpoint
|
|
#define ROTATION_DETECTION_VALUE 100
|
|
|
|
#define _getTorque abs(prg.getTorque(false, ALPHA))
|
|
#define _getAngle prg.getAngle()
|
|
|
|
struct Timer {
|
|
unsigned long startMillis;
|
|
unsigned long interval;
|
|
bool active;
|
|
};
|
|
|
|
void waitStart(Timer &timer, unsigned long interval) {
|
|
timer.startMillis = millis();
|
|
timer.interval = interval;
|
|
timer.active = true;
|
|
}
|
|
|
|
bool waitIsOver(Timer &timer) {
|
|
if (timer.active && (millis() - timer.startMillis >= timer.interval)) {
|
|
timer.active = false;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Timer debug_timer;
|
|
Timer pid_timer;
|
|
Timer speedcheck_timer;
|
|
Timer log_timer;
|
|
Timer meas_timer;
|
|
|
|
PRG_342 prg = PRG_342(K_FAKTOR);
|
|
Datalogger datalogger = Datalogger();
|
|
PID myPID(0.0, 0.0, 0.0);
|
|
|
|
//Messung / Logging
|
|
bool LOGGING = false;
|
|
|
|
// Deklaration der Variablen für die Steuerung und Messung
|
|
int angle;
|
|
int lastAnglePID;
|
|
int lastAngle;
|
|
int actTorque;
|
|
int upperLimit = 0;
|
|
int lowerLimit = -0;
|
|
//int actTorqueArray[100];
|
|
float filteredTorque = 0; // gefilterter Wert
|
|
float filteredSetpoint = 0;
|
|
float setpoint;
|
|
int oldSetpoint;
|
|
static int lastOutput;
|
|
float manVolt;
|
|
unsigned char CW;
|
|
unsigned char Richtungswechsel;
|
|
float RotSpeed;
|
|
unsigned char BLOCK;
|
|
unsigned char BLOCK_SOLVED;
|
|
|
|
bool In_Src_Sw = false;
|
|
|
|
float Kp = 0.0;
|
|
float Ki = 0.1;
|
|
float Kd = 0.0;
|
|
|
|
/*float Kp = 1.0;
|
|
float Ki = 0.333;
|
|
float Kd = 1.0;*/
|
|
|
|
float WinkelArray[181];
|
|
|
|
// Steuervariablen
|
|
bool manOut = false; // Manuelle Ausgabe aktivieren/deaktivieren
|
|
bool Pon = true; // PID-Regelung ein-/ausschalten
|
|
bool DEBUG_MODE = false; // Debug-Modus ein-/ausschalten
|
|
|
|
// Initialisierung der verschiedenen Timer
|
|
unsigned long logtime;
|
|
|
|
void setup() {
|
|
setAnalogResolution(12);
|
|
|
|
|
|
waitStart(debug_timer, DEBUG_PRINT_TIME);
|
|
waitStart(pid_timer, PID_DELAY);
|
|
waitStart(speedcheck_timer, SPEEDCHECK_TIME);
|
|
waitStart(log_timer, LOG_TIMER_DELAY);
|
|
waitStart(meas_timer, MEAS_DELAY);
|
|
|
|
myPID.setOutputLimits(0, 4095);
|
|
myPID.setSampleTime(PID_DELAY);
|
|
myPID.setTunings(Kp, Ki, Kd);
|
|
|
|
//prg.safeShutdown(3);
|
|
|
|
UPDATE_VALUES();
|
|
/*for (int i = 0; i < N_SAMPLES; i++) {
|
|
actTorqueArray[i] = actTorque;
|
|
}*/
|
|
filteredTorque = actTorque;
|
|
|
|
lastAngle = lastAnglePID = angle;
|
|
|
|
for (int i = 0; i < 181; i++) {
|
|
WinkelArray[i] = -1;
|
|
}
|
|
|
|
Serial.begin(115200); // Beginne serielle Kommunikation
|
|
}
|
|
|
|
void loop() {
|
|
|
|
prg.setOutput(2048);
|
|
delay(10);
|
|
prg.setOutput(2049);
|
|
delay(10);
|
|
|
|
COMMUNICATION_HANDLER();
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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 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;
|
|
}
|
|
} 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*/
|
|
}
|
|
} |