Hallo,
ich möchte gerne einen DC Motor mittels einem PID-Regler auf eine gewünschte Drehzahl regeln. Die Drehzahl wird über Rotary Encoder gemessen.
Ich glaube, habe ich einen Fehler im Code beim Messen der Drehzahl.
Ich wäre dankbar, wenn jemand mir weiterhelfen könnte.
#include <PID_v3.h> //https://github.com/ZHomeSlice/Arduino_PID_Library_v3
// Adjust these variables:
int PulsesPerRevolution = 100;// From encoder How Many Pulses Pur Revolution
int SampleDuration = 30; // in Milliseconds -- How often do you want to trigger the PID Lower the number the more detailed control but the more the arduino processor time is used up
double consKp = 1, consKi = 0.01, consKd = 1;
double Setpoint = 100;// RPM
#define PWMpin 12
#define interruptPin 2
#define motordrive_dir pinMode(10, OUTPUT);digitalWrite(10, LOW);
//Define Variables we'll be connecting to
double Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
// Varables used for Calculations
volatile unsigned long timeX = 1;
unsigned long StallTimer = SampleDuration *3;
volatile int Counts = 1;
double PulsesPerMinute;
volatile unsigned long LastTime;
volatile int PulseCtrX;
int PulseCtr;
unsigned long Counter;
unsigned long Time;
void setup() {
Serial.begin(115200);
Serial.println("Test Tachometer");
PulsesPerMinute = (60 * 1000000) / (PulsesPerRevolution / Counts);// initialize
pinMode(interruptPin, INPUT_PULLUP);
pinMode(PWMpin, OUTPUT);
motordrive_dir;
attachInterrupt(digitalPinToInterrupt(interruptPin), sensorInterrupt, FALLING);
myPID.SetSampleTime(1);
myPID.SetOutputLimits(0, 255); // Standard PWM Range
myPID.SetMode(AUTOMATIC);
}
void loop() {
readRpm();
}
// New version of sensorInterrupt
void sensorInterrupt()
{
static int Ctr;
unsigned long Time;
Ctr++;
if (Ctr >= Counts) { // so we are taking an average of "Counts" readings to use in our calculations
Time = micros();
timeX += (Time - LastTime); // this time is accumulative ovrer those "Counts" readings
LastTime = Time;
PulseCtrX ++; // will usually be 1 unless something else delays the sample
Ctr = 0;
}
}
void readRpm()
{
static unsigned long STime;
if (!PulseCtrX) {
if ((unsigned long)(millis() - STime) >= StallTimer) {
Time = micros();
timeX += (Time - LastTime); // this time is accumulative ovrer those "Counts" readings
LastTime = Time;
PulseCtrX ++; // will usually be 1 unless something else delays the sample
Serial.print(" Stall \t");
//Setpoint +=100;
} else return; // << Added lets not stop interrupts unless we know we are ready (keep other code happy).
}
cli (); // clear interrupts flag
Time = timeX; // Make a copy so if an interrupt occurs timeX can be altered and not affect the results.
timeX = 0;
PulseCtr = PulseCtrX ;
PulseCtrX = 0;
sei (); // set interrupts flag
if (PulseCtr > 0) {
Input = (double) (PulsesPerMinute / (double)(( (unsigned long)Time ) * (unsigned long)PulseCtr)); // double has more percision
// PulseCtr = 0; // set pulse Ctr to zero
AverageCapture(Input);
debug();
myPID.Compute();
analogWrite(PWMpin, Output);
long DSDur = ((long) Time * (long)PulseCtr) - ((long) SampleDuration * 1000L);
Counts -= DSDur * .015;
Counts = max(Counts, max(1, PulsesPerRevolution * .25));
Time = 0; // set time to zero to wait for the next rpm trigger.
Counter += PulseCtr;
PulseCtr = 0; // set pulse Ctr to zero
PulsesPerMinute = (60.0 * 1000000.0) / (double)((double)PulsesPerRevolution / (double)Counts);
STime = millis();
}
}
float AvgArray[100];
int Readings = 0;
void AverageCapture(float in) {
static int Position = 0;
AvgArray[Position] = in;
Position++;
Readings++;
Readings = min (100, Readings); // 100 readings 0-99;
if (Position >= 100)Position = 0;//99 spots
}
float AverageValue() {
float Total = 0;
float Average;
if (!Readings)return (0.0);
for (int Position = 0; Position < Readings; Position++) {
Total += AvgArray[Position];
}
Average = Total / Readings;
return (Average);
}
void debug(){
char S[20];
for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 100; QTimer = millis() ) { // one line Spam Delay at 100 miliseconds
Serial.print("Counts: "); Serial.print(Counts );
// Serial.print(" Target RPM: ");Serial.print(RPM );
// Serial.print(" Counts: "); Serial.print(Counts );
// Serial.print(" time: "); Serial.print(Time );
// Serial.print(" DeltaT: "); Serial.print(Time * PulseCtr);
Serial.print(" PulseCtr: "); Serial.print(PulseCtr );
// Serial.print(" PulsesPerMinute: "); Serial.print(dtostrf(PulsesPerMinute, 6, 1, S));
Serial.print("\t Average: "); Serial.print(dtostrf(AverageValue(), 6, 1, S));
// Serial.print(" Setpoint: "); Serial.print(dtostrf(Setpoint, 6, 1, S) );
Serial.print(" Calculated RPM: "); Serial.print(dtostrf(Input, 6, 1, S));
Serial.print(" Output: "); Serial.print(dtostrf(Output, 6, 2, S));
Serial.print("\t ");
Serial.println();
}
}
DC Motor und Encoder sind wie so verbinden
