Diff: regler.cpp

Revision:

0:b72d86a7b005

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/regler.cpp	Tue May 31 03:06:27 2016 +0000
@@ -0,0 +1,116 @@
+/**
+*Funktionen des P-Regleralgorithmus
+*
+*CPP-Datei regler_func.cpp
+*
+*@version: 01.11.2015
+*@author: Gustav Grether & Julian Mueller
+*/
+
+
+#include "regler.h"
+
+//Dateisystem zum speichern der Reglerparameter
+LocalFileSystem local("local");
+
+int regler_sollPx=64; //Sollwerte fuer Kantenpixel
+int e;//Regeldifferenz
+float regler_kp=1.0; //zwischenspeichern Proportinalverstaerkung Regler
+float k0=250.0;//Proportionalverstaerkung Regler bei kp=1.0
+
+float f;//Stellgroesse in Frequenz [Hz]
+float fmax=2500.0; //max. Frequenz [Hz], Stellgroessenbegrenzung
+
+int T;//Stellgroesse als Periodendauer [us]
+int Tmin=floor(1.0/fmax*1000000.0); //Stellgroessenbegrenzung als Periodendauer
+
+void regler_setup()
+{
+    //Einlesen der Reglerparamter aus Dateien
+    //Einlesen des Soll Pixels
+    FILE* inFileSoll = fopen ("/local/sollwert.txt","r");
+    if(inFileSoll!=NULL) {//Datei geoeffnet
+        regler_sollPx = fgetc(inFileSoll);
+    } else {//Datei konnte nicht geoeffnet werden
+        regler_sollPx=64;
+    }
+    if(regler_sollPx<40 || regler_sollPx>90) {
+        //SollPx nicht in realistischem Bereich, evtl. Fehler in Datei
+        regler_sollPx=64;
+    }
+    fclose(inFileSoll);
+
+    //Einlesen von kp
+    FILE* inFilekp = fopen ("/local/kp.txt","r");
+    if(inFileSoll!=NULL) {//Datei geoeffnet
+    fscanf (inFilekp, "%f", &regler_kp);}
+    else {//Datei konnte nicht geoeffnet werden
+        regler_kp=1.0;
+    }
+    if(regler_kp<0.4 || regler_kp>3.0) {
+        //kp nicht in realistischem Bereich, evtl. Fehler in Datei
+        regler_kp=1.0;
+    }
+    
+    fclose(inFilekp);
+}
+
+void regler_end()
+{
+    FILE* outFileSoll = fopen("/local/sollwert.txt","w");
+    fputc(regler_sollPx, outFileSoll);
+    fclose(outFileSoll);
+
+    FILE* outFilekp = fopen("/local/kp.txt","w");
+    fprintf(outFilekp,"%.1f",regler_kp);
+    fclose(outFilekp);
+}
+
+void regler_sollPx_set(int sollPx)
+{
+    regler_sollPx=sollPx;
+}
+
+int regler_sollPx_get()
+{
+    return regler_sollPx;
+}
+
+void regler_kp_set(float kp)
+{
+    regler_kp=kp;
+}
+
+float regler_kp_get()
+{
+    return regler_kp;
+}
+
+
+void control(int edgePx)
+{
+    e=regler_sollPx-edgePx;
+
+    if(e==0) {//keine Motorbewegung
+        if(mtr_period_get()!=0) {
+            T=0;
+            mtr_period_set(T);
+        }
+
+    } else {
+        if(e<0) { //fahre in +y
+            mtr_dir_set(1);
+        } else if(e>0) { //fahre in -y
+            mtr_dir_set(0);
+        }
+        f=k0*regler_kp*abs(e);//Stellgroesse in [Hz]
+
+        if(f>fmax) { //Stellgroessenbegrenzung,maximale Geschwindigkeit
+            T=Tmin;//Stellgroesse in [us]
+            lcd.locate(0,1);
+        } else {
+            T=floor(1.0/f*1000000.0); //Stellgroesse in [us]
+        }
+        mtr_period_set(T);
+    }
+}
\ No newline at end of file