regler.cpp

00001 /**
00002 *Funktionen des P-Regleralgorithmus
00003 *
00004 *CPP-Datei regler_func.cpp
00005 *
00006 *@version: 01.11.2015
00007 *@author: Gustav Grether & Julian Mueller
00008 */
00009 
00010 
00011 #include "regler.h"
00012 
00013 //Dateisystem zum speichern der Reglerparameter
00014 LocalFileSystem local("local");
00015 
00016 int regler_sollPx=64; //Sollwerte fuer Kantenpixel
00017 int e;//Regeldifferenz
00018 float regler_kp=1.0; //zwischenspeichern Proportinalverstaerkung Regler
00019 float k0=250.0;//Proportionalverstaerkung Regler bei kp=1.0
00020 
00021 float f;//Stellgroesse in Frequenz [Hz]
00022 float fmax=2500.0; //max. Frequenz [Hz], Stellgroessenbegrenzung
00023 
00024 int T;//Stellgroesse als Periodendauer [us]
00025 int Tmin=floor(1.0/fmax*1000000.0); //Stellgroessenbegrenzung als Periodendauer
00026 
00027 void regler_setup()
00028 {
00029     //Einlesen der Reglerparamter aus Dateien
00030     //Einlesen des Soll Pixels
00031     FILE* inFileSoll = fopen ("/local/sollwert.txt","r");
00032     if(inFileSoll!=NULL) {//Datei geoeffnet
00033         regler_sollPx = fgetc(inFileSoll);
00034     } else {//Datei konnte nicht geoeffnet werden
00035         regler_sollPx=64;
00036     }
00037     if(regler_sollPx<40 || regler_sollPx>90) {
00038         //SollPx nicht in realistischem Bereich, evtl. Fehler in Datei
00039         regler_sollPx=64;
00040     }
00041     fclose(inFileSoll);
00042 
00043     //Einlesen von kp
00044     FILE* inFilekp = fopen ("/local/kp.txt","r");
00045     if(inFileSoll!=NULL) {//Datei geoeffnet
00046     fscanf (inFilekp, "%f", &regler_kp);}
00047     else {//Datei konnte nicht geoeffnet werden
00048         regler_kp=1.0;
00049     }
00050     if(regler_kp<0.4 || regler_kp>3.0) {
00051         //kp nicht in realistischem Bereich, evtl. Fehler in Datei
00052         regler_kp=1.0;
00053     }
00054     
00055     fclose(inFilekp);
00056 }
00057 
00058 void regler_end()
00059 {
00060     FILE* outFileSoll = fopen("/local/sollwert.txt","w");
00061     fputc(regler_sollPx, outFileSoll);
00062     fclose(outFileSoll);
00063 
00064     FILE* outFilekp = fopen("/local/kp.txt","w");
00065     fprintf(outFilekp,"%.1f",regler_kp);
00066     fclose(outFilekp);
00067 }
00068 
00069 void regler_sollPx_set(int sollPx)
00070 {
00071     regler_sollPx=sollPx;
00072 }
00073 
00074 int regler_sollPx_get()
00075 {
00076     return regler_sollPx;
00077 }
00078 
00079 void regler_kp_set(float kp)
00080 {
00081     regler_kp=kp;
00082 }
00083 
00084 float regler_kp_get()
00085 {
00086     return regler_kp;
00087 }
00088 
00089 
00090 void control(int edgePx)
00091 {
00092     e=regler_sollPx-edgePx;
00093 
00094     if(e==0) {//keine Motorbewegung
00095         if(mtr_period_get()!=0) {
00096             T=0;
00097             mtr_period_set(T);
00098         }
00099 
00100     } else {
00101         if(e<0) { //fahre in +y
00102             mtr_dir_set(1);
00103         } else if(e>0) { //fahre in -y
00104             mtr_dir_set(0);
00105         }
00106         f=k0*regler_kp*abs(e);//Stellgroesse in [Hz]
00107 
00108         if(f>fmax) { //Stellgroessenbegrenzung,maximale Geschwindigkeit
00109             T=Tmin;//Stellgroesse in [us]
00110             lcd.locate(0,1);
00111         } else {
00112             T=floor(1.0/f*1000000.0); //Stellgroesse in [us]
00113         }
00114         mtr_period_set(T);
00115     }
00116 }