588 lines
17 KiB
C++
588 lines
17 KiB
C++
#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));
|
|
}
|
|
}
|