Tricks zur Steuerung von Gleichstrommotoren

Apps und Onlinedienste

Arduino-IDE

Über dieses Projekt

Steuerung von Geschwindigkeit und Richtung von Gleichstrommotoren durch PID-Regler und PWM-Ausgänge

Einführung

Bei fast allen verfügbaren Projekten möchten die Entwickler die Motordrehzahl und -richtung gemeinsam steuern, bevorzugen jedoch hauptsächlich das direkte Senden von PWM an DC-Motoren, sogar über einen Motorsteuerkreis. Eine solche Methode schlägt jedoch immer fehl, wenn Sie die Geschwindigkeit genau nach Ihren Wünschen anpassen müssen, da Zwillingsbrüder als "Reibung" und "Trägheit" bezeichnet werden.

(Bitte geben Sie Zwillingen niemals die Schuld. Was auch immer und wann immer Sie etwas tun möchten, mit oder ohne etwas, Zwillinge kommen sofort und ergreifen Maßnahmen, nur um Ihnen zu helfen, alles unter Kontrolle zu halten. Während Inertia die Dinge „nachdenken“ lässt, bevor sie Maßnahmen ergreifen, Reibung begrenzt ihre Beschleunigung und Geschwindigkeit und "Leistung" ist "nichts", wenn sie nicht unter voller Kontrolle ist.)

Wenn Sie also versuchen, die Drehzahl eines Motors direkt zu steuern, indem Sie Ihren Eingang als PWM-Signal an den Ausgang senden, wird die tatsächliche Drehzahl niemals Ihren Sollwert erreichen und es wird ein erheblicher Unterschied (Fehler) auftreten, wie auf dem Bild oben zu sehen ist. Hier brauchen wir einen anderen Weg, der als „PID-Steuerung“ bezeichnet wird.

PID-Regler

Was ist PID-Steuerung? Stellen Sie sich vor, wie Sie Ihr Auto fahren sollen:Um aus dem Stillstand losfahren zu können, müssen Sie das Gaspedal stärker durchtreten als während einer normalen Fahrt. Während der Fahrt mit (fast) konstanter Geschwindigkeit müssen Sie das Gaspedal nicht zu stark durchtreten, sondern können den Geschwindigkeitsverlust bei Bedarf einfach ausgleichen. Außerdem geben Sie es leicht frei, wenn die Beschleunigung höher ist als Ihre Bedürfnisse. So geht auch "effizientes Fahren".

Der PID-Regler macht also genau dasselbe:Der Regler liest die Differenz „Fehlersignal (e)“ zwischen Sollwert und Ist-Ausgang. Es hat 3 verschiedene Komponenten, die als „Proportional“, „Integral“ und „Derivat“ bezeichnet werden; Der Name des Controllers steht also nach dem ersten Buchstaben jedes einzelnen. Die proportionale Komponente definiert einfach die Steigung (Beschleunigung) des Reglerausgangs in Bezug auf das tatsächliche Fehlersignal. Der Integralteil summiert Fehlersignale rechtzeitig, um den Endfehler zu minimieren. Und die Ableitungskomponente überwacht die Beschleunigung des Fehlersignals und nimmt eine „Anpassung“ vor. Ich werde hier keine weiteren und längeren Details angeben, bitte suchen Sie im Internet nach weiteren Informationen, wenn Sie interessiert sind.

In meinem Arduino-Programm wird der PID-Regler als die unten gezeigte Funktion geschrieben:

float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd){ float P, I, D; /* Basisformel:U =_Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); */ P =_Kp * _E; /* Proportionale Komponente */ I =_Kp * 0,5 * _Ki * (_E+_Eprev) * _dT; /* Integralkomponente */ D =_Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Komponente */ return (P+I+D);} 

Dann wird der endgültige Ausgangswert einfach durch Addieren des aktuellen Ausgangswerts und des Ausgangs des PID-Reglers bestimmt. Dies ist der folgende Abschnitt des Hauptprogramms zusammen mit der Berechnung des Fehlersignals und des PID-Reglerausgangs:

/* Fehlersignal, PID-Reglerausgang und Endausgang (PWM) zum Motor */E =RPMset - RPM;float cPID =controllerPID(E, Eprev, dT, Kp, Ki, Kd);if ( RPMset ==0 ) OutputRPM =0;sonst OutputRPM =OutputRPM + cPID; if ( AusgabeRPM <_minRPM ) AusgabeRPM =_minRPM;  

DC-Motor-Versorgungsschaltung

Natürlich wird es nie empfohlen, einen Gleichstrommotor direkt vom Ausgang eines Arduino oder einer ähnlichen Steuerplatine zu betreiben. Gleichstrommotoren benötigen im Vergleich zu denen, die nicht von den Ausgängen von Controllerkarten geliefert werden können, eine erhebliche Strommenge. Sie müssen also Relaisspulen ansteuern. Aber hier kommt ein weiteres Problem auf:Relais haben mechanische Teile und können mittel- oder langfristig ausfallen. Wir brauchen hier eine weitere Komponente, Transistoren.

Tatsächlich werden Gleichstrommotoren durch Strom und nicht durch Spannung angetrieben. Nach diesem Prinzip habe ich mich für Transistoren entschieden. Sie müssen jedoch den richtigen Transistor auswählen, der dem Motorstrom standhält. Lassen Sie den Motor zunächst direkt laufen, indem Sie ihn an die Stromversorgung anschließen, und messen Sie den Strom bei den maximalen Betriebsbedingungen oder beziehen Sie sich auf die Herstellerspezifikationen.

Danach entschied ich mich, vier BC307A-PNP-Bipolartransistoren auf einer „Brücke“ zu verwenden, um die Stromrichtung durch die Motorspulen zu bestimmen (eigentlich würde ein NPN-BC337-Set besser funktionieren, da es deutlich höheren Kollektorströmen standhalten kann, aber ich tat es nicht habe sie zur Zeit nicht).

Da Motorströme durch den Emitter-Kollektor-Pfad des Transistors geleitet werden sollten, müssen Transistoren mit ungefähr gleichen DC-Stromverstärkungskoeffizienten (hfe) verwendet werden. Um dies zu überprüfen, können Sie die folgende Schaltung verwenden und Transistoren sammeln, die Ihnen ungefähr den gleichen Stromwert auf dem Amperemeter liefern. Um diese vorläufigen Schaltungen zu entwerfen, müssen Sie Folgendes berücksichtigen:

• Suche nach „Basis-Emitter-Anspannung ” (VBEon ) des Transistors. Dies ist die Mindestspannung, die an die Basis zum Einschalten des Transistors angelegt werden muss.

• Finden Sie eine typische „DC-Stromverstärkung ” (hfe ) des Transistors um den Kollektorstrom in der Nähe des Motorstroms. Normalerweise ist es das Verhältnis zwischen Kollektorstrom (IC ) und Basisstrom (IB ), hfe =IC / IB .

• Suchen Sie nach „Maximaler kontinuierlicher Kollektorstrom ” von Transistoren (ICmax ). Der Gleichstrom des Motors sollte diesen Wert in absoluten Werten niemals überschreiten. Ich kann BC307 verwenden, da der von mir verwendete Motor 70 mA benötigt, während der Transistor ICmax (abs) =100 mA hat.

Jetzt können Sie den mit der Basis zu verbindenden Widerstandswert bestimmen:Zuerst müssen Sie die Begrenzungen des Ausgangs Ihrer Controllerkarte berücksichtigen und versuchen, den Basisstrom so gering wie möglich zu halten (daher wird empfohlen, die DC-Stromverstärkung der Transistoren als maximal zu wählen Nehmen Sie die Nennspannung des Ausgangs auf der Controllerplatine als „Triggerspannung ” (VT ) und ermitteln Sie den erforderlichen Basisstrom (IBreq ) durch Teilen des Motorstroms (IM ) zu Gleichstromverstärkung (hfe ) des Transistors:IBreq =IM / hfe .

Bestimmen Sie dann die Spannung, die über den Widerstand fallen soll (VR ), durch Subtraktion von Basis-Emitter-Einschaltspannung (VBEon ) von Triggerspannung :VR =VT - VBEon .

Teilen Sie schließlich die Spannung, die über den Widerstand abfällt (VR ) zum erforderlichen Basisstrom (IBreq ) um den Widerstandswert zu finden (R ):R =VR / IBreq .

[ Kombinierte Formulierung:R =( VT - VBEon ) * hfe / IM ]

In meinem Fall:

• Motorstrom:IM =70 mA

• BC307A Parameter:ICmax =100 mA, hfe =140 (ich habe ungefähr gemessen), VBEon =0,62 V

• Triggerspannung:VT =3,3 V (PWM-Ausgang von Arduino Due)

• R =5360 Ohm (Also entschied ich mich, 4900 Ohm von einem 2K2 und 2K7 zu verwenden, um eine vollständige Abdeckung des Drehzahlbereichs zu gewährleisten und die Schaltung saugt nur ~0,6 mA vom PWM-Ausgang - ein geeignetes Design.)

Umkehren der Richtung und wichtige Hinweise

Um die Richtung eines Gleichstrommotors umzukehren, reicht es aus, den Stromfluss umzukehren. Dazu können wir einfach eine Brückenschaltung mit vier Transistorsätzen erstellen. Auf dem Schaltplan; PWM-Ausgang#2 aktiviert T1A und T1B, während PWM-Ausgang#3 T2A und T2B aktiviert, sodass der durch den Motor fließende Strom geändert wird.

Aber hier müssen wir noch einen anderen Aspekt berücksichtigen:Elektromotoren saugen beim Start einen kurzzeitigen Anlaufstrom, der deutlich höher ist als der Nennstrom, den Sie im Normal-/Dauerbetrieb ablesen (Hersteller geben nur Nennströme an). Der Anlaufstrom kann bei Motoren mit kleiner Leistung ca. 130 % des Nennwertes betragen und erhöht sich je nach Motorleistung. Wenn Sie den Motor also direkt von einer Spannungsquelle speisen und die Polarität während des Betriebs sofort umkehren, zieht der Motor extrem viel Strom, da er nicht vollständig gestoppt wird. Dies kann schließlich zum Durchbrennen der Stromquelle oder zum Durchbrennen der Motorspulen führen. Dies ist für Motoren mit sehr kleiner Leistung möglicherweise nicht so wichtig und spürbar, wird jedoch wichtig, wenn die Leistungsstufen erhöht werden, an denen Sie arbeiten. Aber wenn Sie den Motor über einen Transistor oder eine Reihe von Transistoren (wie das Darlington-Paar) einschalten, haben Sie kein solches Problem, da Transistoren den Strom bereits begrenzen.

Jedenfalls habe ich mir eine kleine Programmroutine überlegt:Wenn die Richtungsauswahl während der Fahrt geändert wird, fährt das Programm zunächst beide Befehlsausgänge auf Null und wartet auf den Motor bis zum vollständigen Stopp. Dann beendet es seine Aufgabe und gibt die gesamte Kontrolle an die Hauptroutine zurück.

if ( Direction !=prevDirection ) { /* Killen beider PWM-Ausgänge zum Motor */ analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); /* Warten bis die Motordrehzahl sinkt */ do { RPM =60*(float)readFrequency(_chSpeedRead,4)/_DiscSlots; } while (RPM> _minRPM); }  

Schnelles Lesen

Bei meiner Anwendung habe ich einen billigen HC-020K-Geschwindigkeitssensor verwendet. Es sendet Impulse in Höhe seiner Versorgungsspannung und das Datenblatt sagt, dass die Versorgungsspannung 5 V beträgt. Mein Board ist jedoch Arduino Due und kann es nicht akzeptieren. Also habe ich es direkt über den 3,3-V-Ausgang von Due mit Strom versorgt, und ja, es hat funktioniert. Und die folgende Funktion wird geschrieben, um die Frequenz und den HC-020K-Ausgang zu lesen.

int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed) { pinMode(_DI_FrequencyCounter_Pin,INPUT); Byte _DigitalRead, _DigitalRead_Previous =0; unsigned long _Time =0, _Time_Init; float _Frequenz =0; if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); else { _Time_Init =micros(); do {_DigitalRead =digitalRead(_DI_FrequencyCounter_Pin); if ( (_DigitalRead_Previous==1) &&(_DigitalRead==0) ) _Frequency++; _DigitalRead_Previous =_DigitalRead; _Zeit =Mikros(); } while ( _Time <(_Time_Init + (1000000/_ReadingSpeed)) ); } zurück (_ReadingSpeed ​​* _Frequency); }  

Beachten Sie, dass das Rad des HC-020K 20 Schlitze hat. Die gelesene Frequenz muss einfach durch 20 geteilt werden, um die Umdrehungen pro Sekunde als Frequenz zu erhalten. Dann sollte das Ergebnis mit 60 multipliziert werden, um RPM zu erhalten.

RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; 

Grafische Nachbesserungen

Um die Eingaben und Ergebnisse anzuzeigen, habe ich ein Makeblock Me TFT-LCD verwendet und die Funktion CommandToTFT() geschrieben, um Befehle an dieses zu senden. Der eigentliche Grund für diese Funktion besteht darin, den seriellen Verbindungspunkt bei Bedarf nur in einer einzigen Zeile im Programm zu ändern.

Die Funktionen Cartesian_Setup(), Cartesian_ClearPlotAreas() und Cartesian_Line() werden geschrieben, um den grafischen Plotbereich vorzubereiten, den Plotbereich zu löschen, wenn das Ende der horizontalen Achse erreicht wird (hier ist es "Zeit") bzw. Grafiken zu zeichnen. Weitere Informationen finden Sie im Makeblock Me TFT LCD-Handbuch, wenn Sie sich hier für grafische Funktionen interessieren, da ich sie hier nicht erläutern werde, da sie eigentlich nicht im Rahmen dieses Blogs liegen.

Ende

Hier kopiere ich das Programm mit und ohne grafische Funktionen separat, damit Sie die Implementierung der Geschwindigkeits- und Richtungssteuerung einzeln oder mit Grafiken überprüfen können. Außerdem finden Sie auf den Codes weitere Erklärungen zu programmierten Funktionen.

Schließlich können Sie die Drehzahl eines Gleichstrommotors normalerweise nicht unter 10-20% seiner Nenndrehzahl verringern, selbst wenn keine Last vorhanden ist. Es ist jedoch möglich, die PID-Regelung für einen unbelasteten DC-Motor nach dem Start auf fast 5 % zu reduzieren.

Bearbeiten (25.02.2018): Wenn Sie NPN-Transistoren anstelle von PNP verwenden möchten, müssen Sie bei beiden Typen auch den Rückstromfluss berücksichtigen. Beim Auslösen (eingeschaltet) fließt bei PNP-Transistoren Strom vom Emitter zum Kollektor, bei NPN-Typen ist es jedoch umgekehrt (vom Kollektor zum Emitter). Daher sind PNP-Transistoren als E(+) C(-) polarisiert und für NPN sollte es C(+) E(-) sein.

Code

• Code mit grafischen Retuschen
• Code ohne grafische Retuschen

Code mit grafischen RetuschenArduino

/* ########################################### ## Farbkonstanten für Makeblock Me TFT LCD########################################## ###### */#define _BLACK 0#define _RED 1#define _GREEN 2#define _BLUE 3#define _YELLOW 4#define _CYAN 5#define _PINK 6#define _WHITE 7/* ######## ####################################### E/A-Zuweisungen###### ######################################## */int _chSpeedSet =A0, // Speed setpoint _chKp =A1, // Proportionaler Koeffizientenwert für PID-Regler _chKi =A2, // Integraler Koeffizientenwert für PID-Regler _chKd =A3, // Differenzialer Koeffizientenwert für PID-Regler _chMotorCmdCCW =3, // PWM-Ausgang zum Motor für Zähler- Drehung im Uhrzeigersinn _chMotorCmdCW =2, // PWM-Ausgang zum Motor für Drehung im Uhrzeigersinn _chSpeedRead =24, // Geschwindigkeitsmessung _chDirection =25; // Richtungswahlanzeige/* ########################################### #### Andere Konstanten ############################################ ### */#define _minRPM 0 // Minimale Drehzahl um Richtungswechsel einzuleiten#define _maxRPM 6000 // Maximale Drehzahlbegrenzung#define _Tmax 90 // Maximale Zeitbegrenzung für die grafische Darstellung#define _DiscSlots 20 // Anzahl der Slots auf der Indexdisk/ * ############################################### Globale Variablen ############################################### */String Cartesian_SetupDetails;boolean Direction, prevDirection;// Alarm Settingsfloat RALL=500.0, RAL=1000.0, RAH=4000.0, RAHH=4500.0;float Seconds=0.0, prevSeconds=0.0, prevRPM=0.0, prevRPMset=0.0, RPM=0.0, RPMset=0.0, AusgangRPM=0.0, Kp=0.0, Ki=0.0, Kd=0.0, Kpmax=2.0, Kimax=1.0, Kdmax=1.0, E=0.0, Eprev=0.0, dT=1.0;/* ###### ######################################### CommandToTFT(TFTCmd) Befehlsfunktion für Makeblock Me TFT-LCD-Eingabeparameter:(String) TFTCmd :Befehlsstring###################################### ######### */void CommandToTFT (String TFTCmd) {/* Serielle Verbindung für die Anzeige verwendet */ Serial1.println (TFTCmd); delay(5);}/* ########### Ende von CommandToTFT() ########### *//* ########### ################################## *//* ############ ################################### Kartesisches_Setup(Xmin, Xmax, Ymin, Ymax, Fenster_X1, Fenster_Y1, Fenster_X2 , Window_Y2, MinDashQty, ColorF, ColorX, ColorY) Kartesische XY-Achsen-Zeichnungsfunktion für Makeblock Me TFT LCD Eingabeparameter:(float) Xmin, Xmax, Ymin, Ymax :Achsenbereichswerte (int) Window_X1, Window_Y1___:Obere linke Ecke von Grafikfenster (int) Window_X2, Window_Y2___:Untere rechte Ecke des Grafikfensters (int) MinDashQty_____________:Anzahl Striche auf kürzester Achse (int) ColorB, ColorX, ColorY :Zeichenfarben für Rahmen, X-Achse und Y-Achse verwendet externe Funktion CommandToTFT().############################################ ### */String Cartesian_Setup( float Xmin, float Xmax, float Ymin, float Ymax, int Window_X1, int Window_Y1, int Window_X2, int Window_Y2, int MinDashQty, int ColorF, int ColorX, int ColorY ){ /* Bildschirmbeschränkungen * / const int Anzeige AuflösungX =319, DisplayResolutionY =239; /* Titelzeichenfolgen begrenzen */ String XminTxt; if (abs(Xmin)>=1000000000) XminTxt ="X=" + String (Xmin/1000000000) + "G"; sonst if (abs(Xmin)>=1000000) XminTxt ="X=" + String (Xmin/1000000) + "M"; else if (abs(Xmin)>=1000) XminTxt ="X=" + String (Xmin/1000) + "K"; sonst XminTxt ="X=" + String (Xmin); Zeichenfolge XmaxTxt; if (abs(Xmax)>=1000000000) XmaxTxt ="X=" + String (Xmax/1000000000) + "G"; sonst if (abs(Xmax)>=1000000) XmaxTxt ="X=" + String (Xmax/1000000) + "M"; sonst if (abs(Xmax)>=1000) XmaxTxt ="X=" + String (Xmax/1000) + "K"; sonst XmaxTxt ="X=" + String (Xmax); Zeichenfolge YminTxt; if (abs(Ymin)>=1000000000) YminTxt ="Y=" + String (Ymin/1000000000) + "G"; sonst if (abs(Ymin)>=1000000) YminTxt ="Y=" + String (Ymin/1000000) + "M"; else if (abs(Ymin)>=1000) YminTxt ="Y=" + String (Ymin/1000) + "K"; sonst YminTxt ="Y=" + String (Ymin); Zeichenfolge YmaxTxt; if (abs(Ymax)>=10000000000) YmaxTxt ="Y=" + String (Ymax/1000000000) + "G"; sonst if (abs(Ymax)>=1000000) YmaxTxt ="Y=" + String (Ymax/1000000) + "M"; sonst if (abs(Ymax)>=1000) YmaxTxt ="Y=" + String (Ymax/1000) + "K"; sonst YmaxTxt ="Y=" + String (Ymax); /* Grenzen */ int XminPx =Window_X1+1; int XmaxPx =Window_X2-1; int YmaxPx =Window_Y1+1; int YminPx =Window_Y2-1; /* Ursprung */ int UrsprungX =XminPx + (int)( (XmaxPx - XminPx) * abs(Xmin) / (abs(Xmax)+abs(Xmin))); int OriginY =YmaxPx + (int)((YminPx – YmaxPx) * abs(Ymax)/(abs(Ymax)+abs(Ymin))); /* Frame */ CommandToTFT ( "BOX(" + String(Window_X1) + "," + String(Window_Y1)+ "," + String(Window_X2) + "," + String(Window_Y2)+ "," + String( FarbeF) + ");" ); /* X-Achse */ CommandToTFT ( "PL(" + String(Window_X1+1) + "," + String(OriginY) + "," + String(Window_X2-1) + "," + String(OriginY) + " ," + String(ColorX) + ");" ); /* Y-Achse */ CommandToTFT ( "PL(" + String(OriginX) + "," + String(Window_Y1+1) + "," + String(OriginX) + "," + String(Window_Y2-1) + " ," + String(ColorY) + ");" ); /* Striche:Die minimale Anzahl von Strichen wird durch "MinDashQty" angegeben und wird auf der kürzesten Achsenseite in Bezug auf den Ursprung gestrichelt. Auf den anderen Abschnitten sind die zu markierenden Striche unter Berücksichtigung des Verhältnisses zur kürzesten Achsseite zu bestimmen. */ /* Gestrichelt */ int XlengthLeft =abs(XminPx-OriginX); int XlengthRight =abs(XmaxPx-UrsprungX); int YlengthLower =abs(YminPx-UrsprungY); int YlengthUpper =abs(YmaxPx-UrsprungY); int XlengthLeft_Mod, XlengthRight_Mod, YlengthLower_Mod, YlengthUpper_Mod; if (XlengthLeft<=1) XlengthLeft_Mod=32767; sonst XlengthLeft_Mod=XlengthLeft; if (XlengthRight<=1) XlengthRight_Mod=32767; sonst XlengthRight_Mod=XlengthRight; if (YlengthLower<=1) YlengthLower_Mod=32767; else YlengthLower_Mod=YlengthLower; if (YlengthUpper<=1) YlengthUpper_Mod=32767; else YlengthUpper_Mod=YlengthUpper; int MinAxisLength =min ( min (XlengthLeft_Mod,XlengthRight_Mod), min (YlengthLower_Mod,YlengthUpper_Mod) ); int XdashesLeft =MinDashQty * XlengthLeft / MinAxisLength; int XdashesRight =MinDashQty * XlengthRight / MinAxisLength; int YdashesLower =MinDashQty * YlengthLower / MinAxisLength; int YdashesUpper =MinDashQty * YlengthUpper / MinAxisLength; int DashingInterval=2; // Min.Intervall btw.dashes /* X-Dash L */ DashingInterval =(int) (XlengthLeft / XdashesLeft); if (!(DashingInterval<2)) for (int i=OriginX; i>=XminPx; i-=DashingInterval) CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + " ," + String(i) + "," + String(OriginY+2) + "," + String(ColorX) + ");" ); /* X-Dash R */ DashingInterval =(int) (XlengthRight / XdashesRight); if (!(DashingInterval<2)) for (int i=OriginX; i<=XmaxPx; i+=DashingInterval) CommandToTFT ( "PL(" + String(i) + "," + String(OriginY-2) + ", " + String(i) + "," + String(OriginY+2) + "," + String(ColorX) + ");" ); /* Y-Dash-L */ DashingInterval =(int) (YlengthLower / YdashesLower); if (!(DashingInterval<2)) for (int i=OriginY; i<=YminPx; i+=DashingInterval) CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + ", " + String(OriginX+2) + "," + String(i) + "," + String(ColorY) + ");" ); /* Y-Dash-U */ DashingInterval =(int) (YlengthUpper / YdashesUpper); if (!(DashingInterval<2)) for (int i=OriginY; i>=YmaxPx; i-=DashingInterval) CommandToTFT ( "PL(" + String(OriginX-2) + "," + String(i) + " ," + String(OriginX+2) + "," + String(i) + "," + String(ColorY) + ");" ); /* Berechnen von Koordinaten zur Anzeige der Achsenendpunktwerte */ int XminTxtX =Window_X1 - (int)(XminTxt.length()*6) - 1, XminTxtY =OriginY, XmaxTxtX =Window_X2 + 1, XmaxTxtY =OriginY, YminTxtX =OriginX, YminTxtY =Window_Y2 + 1, YmaxTxtX =OriginX, YmaxTxtY =Window_Y1 - 12 - 1; /* Kontrollen:Wenn eine Koordinate -1 ist, soll sie die Anzeigegrenzen überschreiten und der entsprechende Wert soll nicht angezeigt werden */ if (XminTxtX<0) XminTxtX =-1; if ((XminTxtY-12) <0 ) XminTxtY =-1; if ( (XmaxTxtX+6*XmaxTxt.length())> DisplayResolutionX ) XmaxTxtX =-1; if ( (XmaxTxtY+12)> DisplayResolutionY ) XmaxTxtY =-1; if ( (YminTxtX+6*YminTxt.length())> DisplayResolutionX ) YminTxtX =-1; if ( (YminTxtY+12)> DisplayResolutionY ) YminTxtY =-1; if ( (YmaxTxtX+6*YmaxTxt.length())> DisplayResolutionX ) YmaxTxtX =-1; wenn (YmaxTxtY<0) YmaxTxtY =-1; /* Bereichsgrenzentitel */ if ( ( XminTxtX !=-1 ) &&( XminTxtY !=-1 ) ) CommandToTFT( "DS12(" + String(XminTxtX) + "," + String(XminTxtY) + ",'" + String(XminTxt) + "'," + String(ColorX) + ");" ); if ( ( XmaxTxtX !=-1 ) &&( XmaxTxtY !=-1 ) ) CommandToTFT( "DS12(" + String(XmaxTxtX) + "," + String(XmaxTxtY) + ",'" + String(XmaxTxt) + " '," + String(ColorX) + ");" ); if ( ( YminTxtX !=-1 ) &&( YminTxtY !=-1 ) ) CommandToTFT( "DS12(" + String(YminTxtX) + "," + String(YminTxtY) + ",'" + String(YminTxt) + " '," + String(ColorY) + ");" ); if ( ( YmaxTxtX !=-1 ) &&( YmaxTxtY !=-1 ) ) CommandToTFT( "DS12(" + String(YmaxTxtX) + "," + String(YmaxTxtY) + ",'" + String(YmaxTxt) + " '," + String(ColorY) + ");" ); /* Rückgabewert String Cartesian_Setup() gibt eine String-Pack-Grafikkonfiguration im folgenden Format zurück:"" String beginnt mit '<' und endet mit '>' . Jeder Wert wird durch ',' begrenzt */ /* Initialize */ String Cartesian_SetupDetails ="<"; Cartesian_SetupDetails +=( String(Xmin) + "," ); Cartesian_SetupDetails +=( String(Xmax) + "," ); Cartesian_SetupDetails +=( String(Ymin) + "," ); Cartesian_SetupDetails +=( String(Ymax) + "," ); Cartesian_SetupDetails +=( String(Window_X1) + "," ); Cartesian_SetupDetails +=( String(Window_Y1) + "," ); Cartesian_SetupDetails +=( String(Window_X2) + "," ); Cartesian_SetupDetails +=( String(Window_Y2) + "," ); /* Close-Out */ Cartesian_SetupDetails +=">";return Cartesian_SetupDetails;}/* ########### Ende von Cartesian_Setup() ########### *// * ################################################ * //* ############################################### Cartesian_ClearPlotAreas(Descriptor, Color) Plot Area Reset/Clear Funktion für Makeblock Me TFT LCD Eingabeparameter:(String) Descriptor:Setup Descriptor - zurückgegeben von Cartesian_Setup() (int) Color______:Farbe die zum Füllen des Plotbereichs verwendet wird Verwendet die externe Funktion CommandToTFT ().############################################## */void Cartesian_ClearPlotAreas(String Descriptor, int Color){ int X1,Y1,X2,Y2; /* Grenzkoordinaten für Plotbereiche */ /* Extrahieren von Werten aus dem Deskriptor */ /* L[0] L[1] L[2] L[3] W[0] W[1] W[2] W[3 ] */ /* Xmin Xmax Ymin Ymax Fenster_X1 Fenster_Y1 Fenster_X2 Fenster_Y2 */ float L[4]; int W[4]; /* Im Deskriptor gespeicherte Werte */ int j=0; /* Zähler */ String D_Str =""; for (int i=1; i<=(Descriptor.length()-1); i++) if ( Descriptor[i] ==',' ) { if (j<4) L[j]=D_Str.toFloat( ); sonst W[j-4]=D_Str.toInt(); D_Str=""; j++; } else D_Str +=Deskriptor[i]; /* Ursprung */ int UrsprungX =(W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / ( abs(L[1])+abs(L[0]))); int OriginY =(W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3 ])+abs(L[2])) ); /* Plotbereiche löschen */ //Area.1 :X+ Y+ X1 =OriginX + 2; Y1 =W[1] + 1; X2 =W[2] - 1; Y2 =UrsprungY - 2; CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + String(X2) + "," + String(Y2) + "," + String(Color) + ");" ); //Area.2 :X- Y+ X1 =W[0] + 1; Y1 =W[1] + 1; X2 =UrsprungX-2; Y2 =UrsprungY - 2; CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + String(X2) + "," + String(Y2) + "," + String(Color) + ");" ); //Bereich.3 :X- Y- X1 =W[0] + 1; Y1 =UrsprungY + 2; X2 =UrsprungX-2; Y2 =W[3] - 1; CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + String(X2) + "," + String(Y2) + "," + String(Color) + ");" ); //Area.4 :X+ Y- X1 =OriginX + 2; Y1 =UrsprungY + 2; X2 =W[2] - 1; Y2 =W[3] - 1; CommandToTFT ( "BOXF(" + String(X1) + "," + String(Y1) + "," + String(X2) + "," + String(Y2) + "," + String(Color) + ");" );} /* ########### Ende von Cartesian_ClearPlotAreas() ########### *//* ############ ############################################# *//* # ############################################## Kartesische_Leitung(Xp, Yp, X, Y, Deskriptor, Farbe) Kartesische Linienfunktion für Makeblock Me TFT LCD Eingabeparameter:(int) Xp, Yp_____:Vorherige Plotkoordinaten - y-Wert vs x (int) X, Y_______:Aktuelle Plotkoordinaten - y-Wert vs x (String) Descriptor :Setup Descriptor - zurückgegeben von Cartesian_Setup() (int) Color______:Markierungsfarbe für die (x,y) Verwendet die externe Funktion CommandToTFT().############## ################################## */void Cartesian_Line(float Xp, float Yp, float X, float Y , String Descriptor, int Color){ /* Extrahieren von Werten aus Descriptor */ /* L[0] L[1] L[2] L[3] W[0] W[1] W[2] W[3] */ /* Xmin Xmax Ymin Ymax Window_X1 Window_Y1 Window_X2 Window_Y2 */ float L[4 ]; int W[4]; /* Im Deskriptor gespeicherte Werte */ int j=0; /* Zähler */ String D_Str =""; for (int i=1; i<=(Descriptor.length()-1); i++) if ( Descriptor[i] ==',' ) { if (j<4) L[j]=D_Str.toFloat( ); sonst W[j-4]=D_Str.toInt(); D_Str=""; j++; } else D_Str +=Deskriptor[i]; /* Ursprung */ int UrsprungX =(W[0]+1) + (int)( ( (W[2]-1) - (W[0]+1) ) * abs(L[0]) / ( abs(L[1])+abs(L[0]))); int OriginY =(W[1]+1) + (int)( ( (W[3]-1) - (W[1]+1) ) * abs(L[3]) / (abs(L[3 ])+abs(L[2])) ); int XminPx =W[0] + 1; int XmaxPx =W[2] - 1; int YmaxPx =W[1] + 1; int YminPx =W[3] - 1; wenn (Y>L[3]) Y=L[3]; wenn (Y
=(OriginX-2) ) &&( DispXp <=(OriginX+2) ) ) || ( ( DispYp>
=(OriginY-2) ) &&( DispYp <=(OriginY+2) ) ) || ( ( DispX>=(OriginX-2) ) &&( DispX <=(OriginX+2) ) ) || ( ( DispY>=(OriginY-2) ) &&( DispY <=(OriginY+2) ) ) )) CommandToTFT( "PL(" + String(DispXp) + "," + String(DispYp) + "," + String(DispX) + "," + String(DispY) + "," + String(Color ) + ");" );}/* ########### Ende der kartesischen_Zeile() ########### *//* ######## ####################################### *//* ####### ######################################## readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) Frequenzmessfunktion Eingang Parameter:(int) _DI_FrequencyCounter_Pin :Zu lesender digitaler Pin (float) _ReadingSpeed____________:Benutzerdefinierte Lesegeschwindigkeit zwischen 0...10 (Hinweis.1) Hinweis.1:_ReadingSpeed ​​ist ein Wert, der angibt, wie lange die Änderungen gezählt werden sollen. It cannot be 0(zero), negative values or a value greater than 10. When _ReadingSpeed changed, 1 second shall be divided by this value to calculate required counting duration. For example; - _ReadingSpeed =0.1 -> input shall be counted during 10 seconds (=1/0.1) - _ReadingSpeed =0.5 -> input shall be counted during 2 seconds (=1/0.5) - _ReadingSpeed =2.0 -> input shall be counted during 0.5 seconds (=1/2) - _ReadingSpeed =4.0 -> input shall be counted during 0.25 seconds (=1/4) Importantly note that, increasing of _ReadingSpeed is a disadvantage especially on lower frequencies (generally below 100 Hz) since counting error increases up to 20%~40% by decreasing frequency.############################################### */int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed){ pinMode(_DI_FrequencyCounter_Pin,INPUT); byte _DigitalRead, _DigitalRead_Previous =0; unsigned long _Time =0, _Time_Init; float _Frequency =0; if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); else { _Time_Init =micros(); do { _DigitalRead =digitalRead(_DI_FrequencyCounter_Pin); if ( (_DigitalRead_Previous==1) &&(_DigitalRead==0) ) _Frequency++; _DigitalRead_Previous =_DigitalRead; _Time =micros(); } while ( _Time <(_Time_Init + (1000000/_ReadingSpeed)) ); } return (_ReadingSpeed * _Frequency);}/* ########### End of readFrequency() ########### *//* ############################################## *//* ############################################### controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) PID Controller Function Input Parameters:(float) RangeMin:Minimum limit for output (float) RangeMax:Maximum limit for output (float) _E_____:Current error signal (float) _Eprev :Previous error signal (float) _dT____:Time difference as seconds (float) _Kp____:Proportional coefficient (float) _Ki____:Integral coefficient (float) _Kp____:Derivative coefficient Adjustment procedure:1. Set Kp=0, Ki=0, Kd=0. 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note critical gain Kc =Kp. 3. Adjust final coefficients as follows. for P-control only :Kp =0.50*Kc for PI-control only :Kp =0.45*Kc, Ki =1.2/Pc for PID-control :Kp =0.60*Kc, Ki =2.0/Pc, Kd=Pc/8 4. Fine tuning could be done by slightly changing each coefficient.############################################### */ float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd){ float P, I, D; /* Base Formula:U =_Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); */ P =_Kp * _E; /* Proportional Component */ I =_Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ D =_Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ return (P+I+D);}/* ########### End of controllerPID() ########### *//* ############################################## *//* ############################################### Setup############################################### */void setup(){ Serial1.begin(9600); Serial1.println("CLS(0);");delay(20); analogReadResolution(12); pinMode(_chDirection,INPUT); // Direction selector reading pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn // Initial killing the PWM outputs to motor analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); // Initial reading for direction selection Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW prevDirection=Direction; // The section below prepares TFT LCD // Cartesian_Setup(Xmin, Xmax, Ymin, Ymax, Window_X1, Window_Y1, Window_X2, Window_Y2, MinDashQty, ColorF, ColorX, ColorY) Cartesian_SetupDetails =Cartesian_Setup(0, _Tmax, _minRPM, _maxRPM, 20, 20, 220, 120, 10, 0, 7, 7); CommandToTFT("DS12(250,10,'Dir:CW '," + String(_WHITE) + ");"); CommandToTFT("DS12(250,25,'____ Set'," + String(_YELLOW) + ");"); CommandToTFT("DS12(250,40,'____ RPM'," + String(_GREEN) + ");"); /* Alarm Values */ CommandToTFT("DS12(250,55,'AHH:" + String(RAHH) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,70,'AH :" + String(RAH) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,85,'AL :" + String(RAL) + "'," + String(_WHITE) + ");"); CommandToTFT("DS12(250,100,'ALL:"+ String(RALL) + "'," + String(_WHITE) + ");"); /* Alarm Window */ CommandToTFT("BOX(240,55,319,115," + String(_WHITE) + ");"); /* Alarm Lamps */ CommandToTFT("BOX(240,55,248,70," + String(_WHITE) + ");"); CommandToTFT("BOX(240,70,248,85," + String(_WHITE) + ");"); CommandToTFT("BOX(240,85,248,100," + String(_WHITE) + ");"); CommandToTFT("BOX(240,100,248,115," + String(_WHITE) + ");");}/* ############################################### Loop############################################### */void loop(){ // Initialization Time:Necessary for PID controller. int InitTime =micros(); // X-Axis Auto-Reset for Graphing if ( Seconds> 90.0 ) { Seconds =0.0; Cartesian_ClearPlotAreas(Cartesian_SetupDetails,0); } // Reading Inputs /* Controller Coefficients */ Kp =Kpmax * (float)analogRead(_chKp) / 4095; Ki =Kimax * (float)analogRead(_chKi) / 4095; Kd =Kdmax * (float)analogRead(_chKd) / 4095; /* Direction Selector */ Direction =digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ /* Actual RPM and RPM Setpoint Note that maximum selectable RPM is 5000. */ RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; RPMset =5000 * (float)analogRead(_chSpeedSet) / 4095; // Calculations and Actions /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ E =RPMset - RPM; float cPID =controllerPID(E, Eprev, dT, Kp, Ki, Kd); if ( RPMset ==0 ) OutputRPM =0; else OutputRPM =OutputRPM + cPID; if ( OutputRPM <_minRPM ) OutputRPM =_minRPM; if ( OutputRPM> _maxRPM ) OutputRPM =_maxRPM; /* Changing Direction when inverted Note that no any graphical indication is performed on this function. */ if ( Direction !=prevDirection ) { /* Killing both of the PWM outputs to motor */ analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); /* Wait until motor speed decreases */ do { RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } while ( RPM> _minRPM ); } // Writing Outputs if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); // Graphing /* Indicating Direction */ if (Direction==HIGH) CommandToTFT("DS12(280,10,'CCW '," + String(_WHITE) + ");"); else CommandToTFT("DS12(280,10,'CW '," + String(_WHITE) + ");"); /* Plotting Curve */ Cartesian_Line(prevSeconds, prevRPMset, Seconds, RPMset, Cartesian_SetupDetails, _YELLOW); Cartesian_Line(prevSeconds, prevRPM, Seconds, RPM, Cartesian_SetupDetails, _GREEN); /* Indicating values of RPM Setpoint, PID Controller Coefficients, Error Signal, PID Controller Output and Final RPM Output (PWM) */ CommandToTFT( "DS12(20,150,'Set:" + String(RPMset) + " rpm " + "RPM:" + String(RPM) + " rpm '," + String(_WHITE) + ");"); CommandToTFT( "DS12(20,170,'Kp=" + String(Kp) + " " + "Ki=" + String(Ki) + " " + "Kd=" + String(Kd) + " " + "dT=" + String(dT*1000) + " ms '," + String(_WHITE) + ");"); CommandToTFT( "DS12(20,190,'e=" + String(E) + " " + "cPID=" + String(cPID) + " " + "RPMout=" + String(OutputRPM) + " '," + String(_WHITE) + ");"); /* Resetting Alarm Lamps */ CommandToTFT("BOXF(241,56,247,69," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,71,247,84," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,86,247,99," + String(_BLACK) + ");"); CommandToTFT("BOXF(241,101,247,114," + String(_BLACK) + ");"); /* Activating Necessary Alarm Lamps */ if (RPM>=RAHH) CommandToTFT("BOXF(241,56,247,69," + String(_RED) + ");"); if ((RPM>=RAH)&&(RPMRALL)&&(RPM<=RAL)) CommandToTFT("BOXF(241,86,247,99," + String(_RED) + ");"); if (RPM<=RALL) CommandToTFT("BOXF(241,101,247,114," + String(_RED) + ");"); // Storing Values generated on previous cycle Eprev =E; prevRPMset =RPMset; prevRPM =RPM; prevSeconds =Seconds; prevDirection =Direction; // Calculating control application cycle time and passed Seconds dT =float ( micros() - InitTime ) / 1000000.0; Seconds+=dT; }

Code without Graphical Touch-UpsArduino

/* ############################################### I/O Assignments############################################### */int _chSpeedSet =A0, // Speed setpoint _chKp =A1, // Proportional coefficient reading for PID controller _chKi =A2, // Integral coefficient reading for PID controller _chKd =A3, // Derivative coefficient reading for PID controller _chMotorCmdCCW =3, // PWM output to motor for counter-clockwise turn _chMotorCmdCW =2, // PWM output to motor for clockwise turn _chSpeedRead =24, // Speed reading _chDirection =25; // Direction selector reading/* ############################################### Other Constants ############################################### */#define _minRPM 0 // Minimum RPM to initiate direction changing#define _maxRPM 6000 // Maximum RPM limit#define _DiscSlots 20 // Qty of slots on Index Disc/* ############################################### Global Variables############################################### */boolean Direction, prevDirection;float RPM=0.0, RPMset=0.0, OutputRPM=0.0, Kp=0.0, Ki=0.0, Kd=0.0, Kpmax=2.0, Kimax=1.0, Kdmax=1.0, E=0.0, Eprev=0.0, dT=1.0;/* ############################################### readFrequency(_DI_FrequencyCounter_Pin, _ReadingSpeed) Frequency Reading Function Input Parameters:(int) _DI_FrequencyCounter_Pin :Digital pin to be read (float) _ReadingSpeed____________:Custom reading speed between 0...10 (Note.1) Note.1:_ReadingSpeed is a value to specify how long shall the changes be counted. It cannot be 0(zero), negative values or a value greater than 10. When _ReadingSpeed changed, 1 second shall be divided by this value to calculate required counting duration. For example; - _ReadingSpeed =0.1 -> input shall be counted during 10 seconds (=1/0.1) - _ReadingSpeed =0.5 -> input shall be counted during 2 seconds (=1/0.5) - _ReadingSpeed =2.0 -> input shall be counted during 0.5 seconds (=1/2) - _ReadingSpeed =4.0 -> input shall be counted during 0.25 seconds (=1/4) Importantly note that, increasing of _ReadingSpeed is a disadvantage especially on lower frequencies (generally below 100 Hz) since counting error increases up to 20%~40% by decreasing frequency.############################################### */int readFrequency(int _DI_FrequencyCounter_Pin, float _ReadingSpeed){ pinMode(_DI_FrequencyCounter_Pin,INPUT); byte _DigitalRead, _DigitalRead_Previous =0; unsigned long _Time =0, _Time_Init; float _Frequency =0; if ( (_ReadingSpeed<=0) || (_ReadingSpeed>10) ) return (-1); else { _Time_Init =micros(); do { _DigitalRead =digitalRead(_DI_FrequencyCounter_Pin); if ( (_DigitalRead_Previous==1) &&(_DigitalRead==0) ) _Frequency++; _DigitalRead_Previous =_DigitalRead; _Time =micros(); } while ( _Time <(_Time_Init + (1000000/_ReadingSpeed)) ); } return (_ReadingSpeed * _Frequency);}/* ########### End of readFrequency() ########### *//* ############################################## *//* ############################################### controllerPID(RangeMin, RangeMax, _E, _Eprev, _dT, _Kp, _Ki, _Kd) PID Controller Function Input Parameters:(float) RangeMin:Minimum limit for output (float) RangeMax:Maximum limit for output (float) _E_____:Current error signal (float) _Eprev :Previous error signal (float) _dT____:Time difference as seconds (float) _Kp____:Proportional coefficient (float) _Ki____:Integral coefficient (float) _Kp____:Derivative coefficient Adjustment procedure:1. Set Kp=0, Ki=0, Kd=0. 2. Start to increase Kp until the system oscillates at fixed period (Pc) and note critical gain Kc =Kp. 3. Adjust final coefficients as follows. for P-control only :Kp =0.50*Kc for PI-control only :Kp =0.45*Kc, Ki =1.2/Pc for PID-control :Kp =0.60*Kc, Ki =2.0/Pc, Kd=Pc/8 4. Fine tuning could be done by slightly changing each coefficient.############################################### */ float controllerPID(float _E, float _Eprev, float _dT, float _Kp, float _Ki, float _Kd){ float P, I, D; /* Base Formula:U =_Kp * ( _E + 0.5*(1/_Ki)*(_E+_Eprev)*_dT + _Kd*(_E-_Eprev)/_dT ); */ P =_Kp * _E; /* Proportional Component */ I =_Kp * 0.5 * _Ki * (_E+_Eprev) * _dT; /* Integral Component */ D =_Kp * _Kd * (_E-_Eprev) / _dT; /* Derivative Component */ return (P+I+D);}/* ########### End of controllerPID() ########### *//* ############################################## *//* ############################################### Setup############################################### */void setup(){ analogReadResolution(12); pinMode(_chDirection,INPUT); // Direction selector reading pinMode(_chMotorCmdCCW,OUTPUT); // PWM output to motor for counter-clockwise turn pinMode(_chMotorCmdCW,OUTPUT); // PWM output to motor for clockwise turn // Initial killing the PWM outputs to motor analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); // Initial reading for direction selection Direction=digitalRead(_chDirection); // HIGH=CCW, LOW=CW prevDirection=Direction;}/* ############################################### Loop############################################### */void loop(){ // Initialization Time:Necessary for PID controller. int InitTime =micros(); // Reading Inputs /* Controller Coefficients */ Kp =Kpmax * (float)analogRead(_chKp) / 4095; Ki =Kimax * (float)analogRead(_chKi) / 4095; Kd =Kdmax * (float)analogRead(_chKd) / 4095; /* Direction Selector */ Direction =digitalRead(_chDirection); /* HIGH=CCW, LOW=CW */ /* Actual RPM and RPM Setpoint Note that maximum selectable RPM is 5000. */ RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; RPMset =5000 * (float)analogRead(_chSpeedSet) / 4095; // Calculations and Actions /* Error Signal, PID Controller Output and Final Output (PWM) to Motor */ E =RPMset - RPM; float cPID =controllerPID(E, Eprev, dT, Kp, Ki, Kd); if ( RPMset ==0 ) OutputRPM =0; else OutputRPM =OutputRPM + cPID; if ( OutputRPM <_minRPM ) OutputRPM =_minRPM; if ( OutputRPM> _maxRPM ) OutputRPM =_maxRPM; /* Changing Direction when inverted */ if ( Direction !=prevDirection ) { /* Killing both of the PWM outputs to motor */ analogWrite(_chMotorCmdCCW,0); analogWrite(_chMotorCmdCW,0); /* Wait until motor speed decreases */ do { RPM =60 * (float)readFrequency(_chSpeedRead,4) / _DiscSlots; } while ( RPM> _minRPM ); } // Writing Outputs if (Direction==HIGH) analogWrite(_chMotorCmdCCW,(int)(255*OutputRPM/_maxRPM)); else analogWrite(_chMotorCmdCW, (int)(255*OutputRPM/_maxRPM)); // Storing Values generated on previous cycle Eprev =E; prevDirection =Direction; // Calculating control application cycle time and passed Seconds dT =float ( micros() - InitTime ) / 1000000.0;}

Schaltpläne