Hallo Zusammen
Ich will die Geschwindigkeit des Getriebemotors wie die Geschwindigkeit des Schrittmotors regeln.
Zuerst wird die Geschwindigkeit des Schrittmotors mit Drehencoder festgelegt. Der Geschwindigkeit des Getriebemotors wird mit PID-Regler und 2 Drehgeber und Arduino Mega 2560 angesteuert. Mit Drehgeber 1 werde ich die Geschwindigkeit des Schrittmotors als Sollgeschwindigkeit des Getriebemotor und mit Drehgeber 2 die Istgeschwindigkeit des Getriebemotor messen.
Code:
#include <AccelStepper.h>
#include <Encoder.h>
#include <PID_v1.h>
#define encoderA_1 18 // Pin A encoder(Drehgeber) 1
#define encoderA_2 19 // Pin A encoder(Drehgeber) 2
#define DIR 10 // DIR motor Driver
#define PWM_OUTPUT 11 // PWM motor Driver
#define MESSINTERVALL 1000
#define ppr 100 // Encoder 100p/r
#define r_roller 31 //Radius in mm Durchmesser 6,2cm Encoder Roller Rad
// PID Regler Definierung
double Setpoint, Input, Output; //Setpoint: Sollwert, Input: Istwert,
double Kp=0.2, Ki=0.2, Kd=0.1; //PID Parameter
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
Encoder Encod(2, 3);
AccelStepper stepper(1,9,8); //( ,9 (PUL+), 8(DIR))
volatile unsigned long isr_impulse_1 = 0; // gezählte Impulse encoder 1
volatile unsigned long isr_letzterImpuls_1=0; // wann der letzte Impuls encoder 1 gezählt wurde
volatile unsigned long isr_impulse_2 = 0; // gezählte Impulse encoder 2
volatile unsigned long isr_letzterImpuls_2=0; // wann der letzte Impuls encoder 2 gezählt wurde
static unsigned long zeit_1;
static unsigned long letzterImpuls_1; // Zeit des letzten Impulses zum Zeitpunkt der letzten Messwertausgabe
unsigned long impulse_1;
float frequenz_1;
float rps_1;
float geschwindigkeit_1;
static unsigned long zeit_2;
static unsigned long letzterImpuls_2; // Zeit des letzten Impulses zum Zeitpunkt der letzten Messwertausgabe
unsigned long impulse_2;
float frequenz_2;
float rps_2;
float geschwindigkeit_2;
void setup()
{
Serial.begin(115200);
stepper.setMaxSpeed(320);
stepper.setAcceleration(20000);
pinMode (PWM_OUTPUT, OUTPUT);
pinMode (DIR, OUTPUT);
digitalWrite(DIR, HIGH);
pinMode (encoderA_1, INPUT_PULLUP);
pinMode (encoderA_2, INPUT_PULLUP);
//turn the PID on
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(0,255);
myPID.SetSampleTime(1);
attachInterrupt(digitalPinToInterrupt(encoderA_1), encodermessung_1, RISING);
attachInterrupt(digitalPinToInterrupt(encoderA_2), encodermessung_2, RISING);
}
float read_speed_encoder1()
{
static unsigned long ersteMessung_1; // Zeit der ersten Messwertausgabe
static unsigned long letzteMessung_1; // Zeit der letzten Messwertausgabe
static unsigned long Time_1;
geschwindigkeit_1=0.0;
ersteMessung_1=millis(); // Aktuelle Zeit in Millisekunden
if (ersteMessung_1 -letzteMessung_1 >= MESSINTERVALL) // Ist das Messintervall abgelaufen?
{
Time_1 = ersteMessung_1 -letzteMessung_1;
letzteMessung_1 =ersteMessung_1;
noInterrupts(); // Interrupts sperren
// encoder1
impulse_1 =isr_impulse_1; // volatile Variable auslesen
isr_impulse_1=0; // valatile Variable zurücksetzen
zeit_1=isr_letzterImpuls_1 -letzterImpuls_1;
letzterImpuls_1=isr_letzterImpuls_1;
interrupts(); // Interrupts wieder zulassen
//encoder1
if (impulse_1>0)
{
rps_1= impulse_1/(float)(zeit_1*ppr)*1000L;
geschwindigkeit_1 = rps_1 *2*r_roller* PI;
}
// else {
// frequenz_1=0;
// impulse_1=0;
// geschwindigkeit_1=0;
// rps_1=0;
// }
}
return geschwindigkeit_1;
}
float read_speed_encoder2()
{
static unsigned long ersteMessung_2; // Zeit der ersten Messwertausgabe
static unsigned long letzteMessung_2; // Zeit der letzten Messwertausgabe
static unsigned long Time_2;
geschwindigkeit_2=0.0;
ersteMessung_2=millis(); // Aktuelle Zeit in Millisekunden
if (ersteMessung_2 -letzteMessung_2 >= MESSINTERVALL) // Ist das Messintervall abgelaufen?
{
Time_2 = ersteMessung_2 -letzteMessung_2;
letzteMessung_2=ersteMessung_2;
noInterrupts(); // Interrupts sperren
//encoder2
impulse_2 =isr_impulse_2; // volatile Variable auslesen
isr_impulse_2=0; // valatile Variable zurücksetzen
zeit_2=isr_letzterImpuls_2 -letzterImpuls_2;
letzterImpuls_2=isr_letzterImpuls_2;
interrupts(); // Interrupts wieder zulassen
if (impulse_2>0)
{
rps_2= impulse_2/(float)(zeit_2*ppr)*1000L;
geschwindigkeit_2 = rps_2 *2*r_roller* PI;
}
// else {
// frequenz_2=0;
// impulse_2=0;
// geschwindigkeit_2=0;
// rps_2=0;
// }
}
return geschwindigkeit_2;
}
void report(void) // Ausgaben Serieller Monitor
{
static uint32_t last=0;
uint32_t now = millis();
const uint32_t interval = 1000;
if(now - last >= interval ){
last= now;
Serial.print("V_1: ");Serial.print(read_speed_encoder1(),4);//Serial.print(geschwindigkeit_1,4);
Serial.print('\t');
Serial.print("V1 in m/min: "); Serial.print((read_speed_encoder1()/16.666666666667), 4);
Serial.print('\t');
Serial.print("V_2: ");Serial.print(read_speed_encoder2(),4);//Serial.print(geschwindigkeit_2,4);
Serial.print('\t');
Serial.print("V2 in m/min: "); Serial.println((read_speed_encoder2()/16.666666666667), 4);
}
}
long old_position = -999;
void loop()
{
unsigned long new_position = Encod.read();
if (new_position != old_position) {
old_position = new_position;
if(new_position> 320) { new_position=320; }
if(new_position<0) { new_position=0; }
}
stepper.move(new_position);
stepper.run();
Setpoint = read_speed_encoder1()*2,8125; //Umrechnung von der Gescwindigkeit in Pul
Input = read_speed_encoder2()*2,8125; //Umrechnung von der Gescwindigkeit in Pul
if (myPID.Compute()) {
analogWrite(PWM_OUTPUT, Output);
digitalWrite(DIR,HIGH);
Serial.print("Output: ");Serial.println(Output);
}
report();
}
void encodermessung_1()
{
isr_impulse_1++;
isr_letzterImpuls_1=millis();
}
void encodermessung_2()
{
isr_impulse_2++;
isr_letzterImpuls_2=millis();
}
Das Problem der Getriebemotor läuft nicht. Das was kommt raus:
11:27:06.316 -> V_1: 22.5612 V1 in m/min: 0.0000 V_2: nan V2 in m/min: 0.0000
11:27:07.188 -> Output: 0.00
11:27:07.302 -> V_1: 22.5395 V1 in m/min: 0.0000 V_2: nan V2 in m/min: 0.0000
11:27:08.187 -> Output: 0.00
11:27:08.306 -> V_1: 22.6487 V1 in m/min: 0.0000 V_2: nan V2 in m/min: 0.0000
11:27:09.186 -> Output: 0.00
11:27:09.312 -> V_1: 22.7590 V1 in m/min: 0.0000 V_2: nan V2 in m/min: 0.0000
11:27:10.186 -> Output: 0.00
kann jemand mir weiterhelfen.
Vielen Dank im Voraus