57 lines
1.2 KiB
C++
57 lines
1.2 KiB
C++
#include <PID_v1.h>
|
|
#include <prg_342.h>
|
|
|
|
#define PIN_INPUT A0
|
|
#define PIN_OUTPUT DAC0
|
|
#define K_FAKTOR 1.0995380532183
|
|
#define ALPHA 1
|
|
|
|
PRG_342 prg = PRG_342(K_FAKTOR);
|
|
|
|
//Define Variables we'll be connecting to
|
|
double Setpoint, Input, Output;
|
|
int angle;
|
|
int lastAngle;
|
|
unsigned long timer1;
|
|
|
|
//Specify the links and initial tuning parameters
|
|
double Kp = 0, Ki = 0.04, Kd = 0;
|
|
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
|
|
|
|
void setup() {prg.setOutput(4095*0.5, false);
|
|
delay(3000);
|
|
prg.setOutput(0, false);
|
|
delay(2000);
|
|
Serial.begin(115200);
|
|
|
|
angle = prg.getAngle();
|
|
Input = prg.getTorque(true, ALPHA);
|
|
prg.updateActOut(Input);
|
|
Setpoint = 15000;
|
|
|
|
//turn the PID on
|
|
myPID.SetOutputLimits(0, 4095);
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
|
|
void loop() {
|
|
angle = prg.getAngle();
|
|
Input = abs(prg.getTorque(true, ALPHA));
|
|
prg.updateActOut(Input);
|
|
|
|
if (SPEEDCHECK() >= 400) {
|
|
myPID.Compute();
|
|
}
|
|
|
|
prg.setOutput(Output, false);
|
|
}
|
|
|
|
float SPEEDCHECK() {
|
|
// Überprüfen der Rotationsgeschwindigkeit
|
|
if (millis() - timer1 >= 200) {
|
|
timer1 = millis();
|
|
int deltaAngle = angle - lastAngle;
|
|
lastAngle = angle;
|
|
return abs(deltaAngle * 5); // Umrechnen in Winkel pro Sekunde
|
|
}
|
|
} |