musabe24/t2000_arduino_due
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
ddedc0c72a4c292da1bf5898835f5e48a2903c4d
t2000_arduino_due/main/main.ino

469 lines
12 KiB
C++
Raw Blame History

#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*/
}
}
Reference in New Issue View Git Blame Copy Permalink