File content as of revision 0:b72d86a7b005:

/**
*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);
    }
}