Synchronisierung.
This commit is contained in:
292
main/main.ino
292
main/main.ino
@@ -1,9 +1,293 @@
|
||||
void setup() {
|
||||
// put your setup code here, to run once:
|
||||
#include <prg_342.h>
|
||||
|
||||
PRG_342 prg = PRG_342();
|
||||
|
||||
int angle = 0;
|
||||
int lastAngle = 0;
|
||||
int actTorque = 0;
|
||||
int setpoint = 0;
|
||||
double lastOutput;
|
||||
|
||||
float Kp = 0.0;
|
||||
float Ki = 0.0;
|
||||
float Kd = 0.0;
|
||||
int Pout;
|
||||
int Iout;
|
||||
int Dout;
|
||||
|
||||
int error;
|
||||
|
||||
bool OFFSET_ON = true;
|
||||
bool manOut = false;
|
||||
|
||||
bool PID = false;
|
||||
unsigned char PID_DELAY = 1; //in ms
|
||||
unsigned int Imax = 217; //in mA
|
||||
bool PRINT_DEBUG = false;
|
||||
bool DEBUG_MODE = false;
|
||||
|
||||
// Define variables for PID calculations
|
||||
float integral = 0;
|
||||
int derivative;
|
||||
int previousError = 0;
|
||||
unsigned long lastTime = 0;
|
||||
unsigned long currentTime;
|
||||
unsigned long timeChange;
|
||||
|
||||
unsigned long timer1;
|
||||
unsigned long timer2;
|
||||
unsigned long PIDtimer;
|
||||
|
||||
float RotSpeed;
|
||||
unsigned int MilliAmpPerSecond = 160;
|
||||
unsigned int maxChange;
|
||||
|
||||
char serBuffer[4];
|
||||
|
||||
void setup() {
|
||||
analogWriteResolution(12); // Set the resolution to 12 bits (0-4095)
|
||||
analogReadResolution(12);
|
||||
|
||||
Serial.begin(115200);
|
||||
|
||||
prg.safeShutdown(5);
|
||||
|
||||
timer1 = timer2 = PIDtimer = millis();
|
||||
angle = prg.getAngle();
|
||||
lastAngle = angle;
|
||||
}
|
||||
|
||||
void COMMUNICATION_HANDLER() {
|
||||
if (Serial.available() > 0) {
|
||||
delay(10);
|
||||
unsigned int SerIn = Serial.read();
|
||||
switch (SerIn) {
|
||||
case 'a':
|
||||
Serial.print((float)prg.getAngle() / 1000, 1);
|
||||
Serial.print(";");
|
||||
Serial.println((float)prg.getTorque(false, 10) / 1000, 1);
|
||||
break;
|
||||
|
||||
case 'e':
|
||||
prg.safeShutdown(10);
|
||||
break;
|
||||
|
||||
case 't':
|
||||
prg.tareTorque();
|
||||
prg.tareAngle();
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
OFFSET_ON = !OFFSET_ON;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
delay(10);
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
Kp = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
|
||||
break;
|
||||
|
||||
case 'i':
|
||||
delay(10);
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
Ki = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
delay(10);
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
Kd = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
|
||||
break;
|
||||
|
||||
case 's':
|
||||
delay(10);
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
setpoint = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
|
||||
Serial.print("setpoint: ");
|
||||
Serial.println(setpoint);
|
||||
break;
|
||||
|
||||
case 'm':
|
||||
manOut = !manOut;
|
||||
break;
|
||||
|
||||
case 'v':
|
||||
if (manOut) {
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
unsigned int volt = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]) * 0.001;
|
||||
prg.setOutput(40.95 * volt, false);
|
||||
}
|
||||
break;
|
||||
|
||||
case 'x':
|
||||
PID = !PID;
|
||||
error = 0;
|
||||
previousError = 0;
|
||||
integral = 0;
|
||||
break;
|
||||
|
||||
case 'r':
|
||||
error = 0;
|
||||
previousError = 0;
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
Pout = 0;
|
||||
Iout = 0;
|
||||
Dout = 0;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
prg.DynDecog(prg.ActOut);
|
||||
break;
|
||||
|
||||
case 'k':
|
||||
delay(10);
|
||||
Serial.readBytes(serBuffer, 4);
|
||||
MilliAmpPerSecond = ((serBuffer[0] << 24) | (serBuffer[1] << 16) | (serBuffer[2] << 8) | serBuffer[3]);
|
||||
Serial.print("Max Milliampere per second: ");
|
||||
Serial.println(MilliAmpPerSecond);
|
||||
break;
|
||||
|
||||
case 'b':
|
||||
DEBUG_MODE = !DEBUG_MODE;
|
||||
break;
|
||||
|
||||
default:
|
||||
Serial.println(SerIn);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PID_CONTROL() {
|
||||
// Get current time
|
||||
currentTime = millis();
|
||||
timeChange = currentTime - lastTime;
|
||||
lastTime = currentTime;
|
||||
|
||||
// Read the current value from the sensor
|
||||
actTorque = abs(prg.getTorque(true, 10));
|
||||
angle = abs(prg.getAngle());
|
||||
|
||||
// Calculate error
|
||||
error = setpoint - actTorque;
|
||||
|
||||
// Proportional term
|
||||
Pout = Kp * error;
|
||||
|
||||
// Integral term
|
||||
integral += error * (float)timeChange / 1000.0; // Convert timeChange to seconds
|
||||
// Limit the integral term to prevent windup
|
||||
integral = constrain(integral, -100000, 100000);
|
||||
Iout = Ki * integral;
|
||||
|
||||
// Derivative term
|
||||
derivative = (error - previousError) / ((float)timeChange / 1000.0); // Convert timeChange to seconds
|
||||
Dout = Kd * derivative;
|
||||
|
||||
// Calculate total output
|
||||
int output = Pout + Iout + Dout;
|
||||
|
||||
// Constrain output
|
||||
output = constrain(output, 10, 4095);
|
||||
|
||||
// Implement a smooth change to avoid sudden jumps
|
||||
static int lastOutput = 0;
|
||||
//int maxChange = 10; // Max change per loop iteration (adjust as needed)
|
||||
maxChange = (MilliAmpPerSecond * 4095 * PID_DELAY) / (Imax * 1000);
|
||||
if (abs(output - lastOutput) > maxChange) {
|
||||
if (output > lastOutput) {
|
||||
output = lastOutput + maxChange;
|
||||
} else {
|
||||
output = lastOutput - maxChange;
|
||||
}
|
||||
}
|
||||
|
||||
// Write the PID output to the actuator
|
||||
prg.setOutput(output, false);
|
||||
lastOutput = output;
|
||||
|
||||
// Update previous error
|
||||
previousError = error;
|
||||
}
|
||||
|
||||
void SPEEDCHECK() {
|
||||
if (millis() - timer2 >= 200) {
|
||||
timer2 = millis();
|
||||
int deltaAngle = prg.getAngle() - lastAngle;
|
||||
RotSpeed = deltaAngle * 5.0; // Convert to speed per second
|
||||
lastAngle = prg.getAngle();
|
||||
}
|
||||
}
|
||||
|
||||
void DEBUG_TIMER() {
|
||||
if (millis() - timer1 >= 500) {
|
||||
timer1 = millis();
|
||||
PRINT_DEBUG = true;
|
||||
} else {
|
||||
PRINT_DEBUG = false;
|
||||
}
|
||||
}
|
||||
|
||||
void DEBUG_PRINTER() {
|
||||
if (PRINT_DEBUG) {
|
||||
Serial.println("####################");
|
||||
Serial.print("Winkel: ");
|
||||
Serial.println((float)prg.getAngle() / 1000, 3);
|
||||
Serial.print("Drehmoment: ");
|
||||
Serial.println((float)prg.getTorque() / 1000, 3);
|
||||
Serial.print("Geschwindigkeit: ");
|
||||
Serial.println(RotSpeed);
|
||||
Serial.print("Ausgangsspannung: ");
|
||||
Serial.println((float)prg.getOutVolt() / 1000, 3);
|
||||
Serial.print("Setpoint: ");
|
||||
Serial.println((float)setpoint / 1000, 3);
|
||||
Serial.print("Error: ");
|
||||
Serial.println((float)error / 1000, 3);
|
||||
Serial.print("Kp: ");
|
||||
Serial.println(Kp, 3);
|
||||
Serial.print("Ki: ");
|
||||
Serial.println(Ki, 3);
|
||||
Serial.print("Kd: ");
|
||||
Serial.println(Kd, 3);
|
||||
Serial.print("Pout: ");
|
||||
Serial.println(Pout);
|
||||
Serial.print("Iout: ");
|
||||
Serial.println(Iout);
|
||||
Serial.print("Dout: ");
|
||||
Serial.println(Dout);
|
||||
Serial.print("Integral: ");
|
||||
Serial.println(integral);
|
||||
Serial.print("PID: ");
|
||||
Serial.println(PID);
|
||||
Serial.print("Max mA / s (SET): ");
|
||||
Serial.println(MilliAmpPerSecond);
|
||||
Serial.print("Max mA / s (ACT): ");
|
||||
Serial.println((Imax * maxChange * 1000) / (4095 * PID_DELAY));
|
||||
Serial.print("maxChange: ");
|
||||
Serial.println(maxChange);
|
||||
Serial.print("isRunning: ");
|
||||
Serial.println(prg.isRunning);
|
||||
Serial.println("####################");
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
if (DEBUG_MODE) {
|
||||
DEBUG_TIMER();
|
||||
DEBUG_PRINTER();
|
||||
}
|
||||
COMMUNICATION_HANDLER();
|
||||
SPEEDCHECK();
|
||||
|
||||
}
|
||||
if (millis() - PIDtimer >= PID_DELAY) { // Small delay for stability
|
||||
PIDtimer = millis();
|
||||
|
||||
if ((PID && !manOut && abs(prg.getTorque()) >= 2000 && abs(RotSpeed) >= 400) || (abs(prg.getTorque()) > setpoint)) {
|
||||
PID_CONTROL();
|
||||
}
|
||||
}
|
||||
|
||||
prg.updateActOut();
|
||||
prg.isRunning = false;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user