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

Drehmoment wird nun normal gemittelt, output Variable ist jetzt global, PID Parameter aktualisiert, der Spannungsausgang bleibt erhalten - auch wenn die Regelung pausiert wurde.

Browse Source
This commit is contained in:
Musab Erdem
2024-07-25 16:28:38 +02:00
parent a49ca289e1
commit 34218c251f
5 changed files with 443 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

172
testsoftware/testsoftware.ino
View File

@@ -9,8 +9,9 @@
#define SPEEDCHECK_TIME 200
#define PID_DELAY 0
#define LOG_TIMER_DELAY 100
#define MEAS_DELAY 0
#define K_FAKTOR 1.0995380532183
#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
@@ -121,147 +122,19 @@ void setup() {
WinkelArray[i] = -1;
}
Serial.begin(19200); // Beginne serielle Kommunikation
Serial.begin(115200); // 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
prg.setOutput(2048);
delay(10);
prg.setOutput(2049);
delay(10);
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 && !checkLimits(setpoint, abs(actTorque), upperLimit, lowerLimit) &&((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;
}
bool checkLimits(float setpoint, int actVal, int upperLimit, int lowerLimit) {
int error = setpoint - actVal;
if (error <= upperLimit && error >= lowerLimit) {
return true;
} else {
return 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;
}
@@ -270,14 +143,6 @@ 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);
@@ -298,24 +163,6 @@ void COMMUNICATION_HANDLER() {
}
}
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("####################");
@@ -451,7 +298,6 @@ void parseCommand(const String &command) {
}
lastIndex = (index == -1) ? -1 : index + 1;
}
interpolateWinkelArray();
} else if (command[0] == 'v') {
if (manOut) {
setParameter(command, 'v', manVolt); // Hier wurde 'manVolt' als float deklariert