kramann.info
© Guido Kramann

Login: Passwort:










Regelungssysteme
1 day_by_day
2 Heizregelkreis
3 Verzoegerungsglieder
4 Laplace
..4.1 Eigenwerte
..4.2 PT1
..4.3 PRegler
..4.4 Scilab
5 Regleroptimierung
..5.1 Guetefunktion
..5.2 Heuristiken
..5.3 Scilab
..5.4 Gradientenverfahren
..5.5 ModifizierteG
..5.6 Gleichstrommotor
..5.7 Stoerverhalten
6 Javaanwendung
..6.1 PIDgeregelterAntrieb
..6.2 RungeKuttaIntegrator
..6.3 Gradientenverfahren
7 Einstellregeln
..7.1 Totzeit
..7.2 Methode1
..7.3 Methode2
..7.4 Scilab
..7.5 Daempfungsgrad
..7.6 Uebung
8 Polvorgabe
9 Beobachter
10 AutonomerHackenprosche
..10.1 Herleitung
..10.2 Scilab
..10.3 Modellerweiterung
..10.4 Scilab
..10.5 Modellgueltigkeit
..10.6 java
11 Stabilitaet
..11.1 Beispiele
..11.2 Nyqusitkriterium
..11.3 Windup
..11.4 Bode
12 Adaptiv
..12.1 Definition
..12.2 Einachser
..12.3 Auswertung
..12.4 Identifikation
..12.5 Regleroptimierung
..12.6 Zustandsregler
..12.7 Beobachter
13 Analyse
..13.1 Linear
..13.2 Nichtlinear
14 Kalmanfilter
15 Ue_04_2014
..15.1 Geschwindigkeit
..15.2 Richtung
..15.3 Gesamtsystem
..15.4 RiccatiUSW
..15.5 TdOT
16 Inverses_Pendel
17 Einachser
..17.1 Mechanik
..17.2 Uebung8
18 Fuzzy
..18.1 Fuzzylogik
..18.2 FuzzyRegler
..18.3 Uebung9
..18.5 Softwareentwicklung
....18.5.1 AgileSoftwareentwicklung
....18.5.2 FuzzyRegler
....18.5.3 Uebung
..18.6 Umsetzung
....18.6.1 FuzzyRegler
....18.6.2 Simulation
....18.6.3 Optimierung
....18.6.4 Uebung
..18.7 Haengependel
....18.7.1 Haengependel
....18.7.2 Simulation
....18.7.3 FuzzyRegler
....18.7.4 Optimierer
....18.7.5 Genetisch
..18.8 Information
..18.9 Energie
21 Beispiel1
98 day_by_day_WS2021_SoSe21
99 day_by_day_SoSe2018
kramann.info
© Guido Kramann

Login: Passwort:




Beispiel 1: Regelung eines Motorpendels

(EN google-translate)

(PL google-translate)

Anordnung

Aufbau des Systems.

Bild 0-1: Aufbau des Systems.

Schematischer Aufbau und Stromlaufplan des Systems.

Bild 0-2: Schematischer Aufbau und Stromlaufplan des Systems.

pendelregler.ogg.zip - Film.

Programmvarianten

REGLER.zip - Programmvarianten
Experiment einfacher P-Regler
int sensor;
int motor;

void setzeMotor(int wert)
{
      if(wert>255)
          wert=255;
      if(wert<-255)    
          wert=-255;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
          analogWrite(9,wert);
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
          analogWrite(9,-wert);
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
          analogWrite(9,0);
      }
}

void setup() 
{
    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
}

void loop() 
{
    sensor = analogRead(0)-300;
    Serial.println(sensor);
    delay(200);
    
    setzeMotor(-sensor);
}

Code 0-1: REGLER001 - Experiment einfacher P-Regler

Experiment einfacher PI-Regler
int sensor;
int motor;
int integral = 0;

void setzeMotor(int wert)
{
      if(wert>255)
          wert=255;
      if(wert<-255)    
          wert=-255;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
          analogWrite(9,wert);
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
          analogWrite(9,-wert);
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
          analogWrite(9,0);
      }
}

void setup() 
{
    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
}

void loop() 
{
    sensor = analogRead(0)-300;
    integral+=sensor;
    Serial.println(sensor);
    delay(200);
    
    setzeMotor(-sensor-integral);
}

Code 0-2: REGLER002 - Experiment einfacher PI-Regler.

Experiment einfacher PID-Regler
int sensor=0;
int sensor_alt=0;
int motor=0;
int integral = 0;
int differenz = 0;
void setzeMotor(int wert)
{
      if(wert>255)
          wert=255;
      if(wert<-255)    
          wert=-255;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
          analogWrite(9,wert);
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
          analogWrite(9,-wert);
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
          analogWrite(9,0);
      }
}

void setup() 
{
    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
}

void loop() 
{
    sensor_alt = sensor;
    sensor = analogRead(0)-300;
    integral+=sensor;
    differenz = 4*(sensor-sensor_alt);
    if(integral>200)
        integral=200;
    if(integral<-200)
        integral=-200;
//    Serial.println(sensor);
    delay(4);
    
    setzeMotor(-2*sensor-integral-differenz);
}

Code 0-3: REGLER004 - Experiment einfacher PID-Regler.

Experiment Sprungantwort
#define ARRSIZE 300

int sensor=0;
int sensor_alt=0;
int motor=0;
int integral = 0;
int differenz = 0;

int ZUSTAND = 0;

int arr[ARRSIZE];
int index=0;

void setzeMotor(int wert)
{
      if(wert>255)
          wert=255;
      if(wert<-255)    
          wert=-255;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
          analogWrite(9,wert);
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
          analogWrite(9,-wert);
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
          analogWrite(9,0);
      }
}

void setup() 
{
    pinMode(12, INPUT); 
    digitalWrite(12,1); //Pullup
  
    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
}

void loopA()
{
            sensor_alt = sensor;
            sensor = analogRead(0)-300;
            integral+=sensor;
            differenz = 4*(sensor-sensor_alt);
            if(integral>200)
                integral=200;
            if(integral<-200)
                integral=-200;
            delay(4);    
            setzeMotor(-2*sensor-integral-differenz);
            if(digitalRead(12)==0)
            {
                index=0;
                ZUSTAND = 1;
            }      
}

void loopB()
{
            setzeMotor(-255);
            arr[index] = analogRead(0)-300;
            index++;
            if(index>=ARRSIZE)
            {
                setzeMotor(0);
                index=0;
                ZUSTAND=2;
            }    
            delay(1);
}

void loopC()
{
            Serial.print(index); //Millisekunden
            Serial.print(" ");
            Serial.println(arr[index]); //Winkel
            index++;
            if(index>=ARRSIZE)
            {
                index=0;
                ZUSTAND=0;
            }    
}

void loop() 
{
    switch(ZUSTAND)
    {
        case 0:
          loopA();
        break;  
        case 1:
          loopB();
        break;
        case 2:
          loopC();
        break;
    }    
}

Code 0-4: REGLER005sprung - Experiment Sprungantwort.

Auswertung

daten.zip - mess3_roh.sce Rohdaten als Scilab-Matrix, mess3c.sce Glättung / zeitl. Ableitung, Totzeit-Annahme: 20ms.
Sprungantwort Bewegungsgeschwindigkeit, Einheiten/Sekunde gegen Zeit in Sekunden, Totzeit nicht dargestellt.

Bild 0-3: Sprungantwort Bewegungsgeschwindigkeit, Einheiten/Sekunde gegen Zeit in Sekunden, Totzeit nicht dargestellt.

System-Identifikation

anzahl_schritte = 400;
delta = ones(2,2)*0.1;
for i=1:1:anzahl_schritte
    pnumber    = 1+ floor(grand(1, 1, "unf", 0, 1.999999999));
    prichtung  = 2*floor(grand(1, 1, "unf", 0, 1.999999999))-1;
    prichtung_index = (prichtung+1)/2 + 1;

    param_neu = param;
    param_neu(pnumber) = param_neu(pnumber) + prichtung*delta(pnumber,prichtung_index);
    fehler_neu = berechneFehler(param_neu);

    if ( fehler_neu <= fehler ) then
        fehler = fehler_neu;
        param  = param_neu;
        delta(pnumber,prichtung_index) = delta(pnumber,prichtung_index)*1.1;
    else
        delta(pnumber,prichtung_index) = delta(pnumber,prichtung_index)*0.9;
    end
end

Code 0-5: Einfacher Optimierungsalgorithmus (modifiziertes Gradientenverfahren) als mögliche Grundlage.

Sensorwert-Verlauf bei der Sprungantwort.

Bild 0-4: Sensorwert-Verlauf bei der Sprungantwort.

PROBLEME: Die Daten, die bei der Sprungantwort geliefert werden sind mit Störungen behaftet. Ursachen könnten sein:

  • Plötzliche Ausreißer: Von den Motorkommutatoren verursachte elektromagnetische Störungen.
  • Stufigkeit: Von der Wandlergeschwindigkeit des AD-Wandlers herrührendes Verharren auf dem zuletzt gewandelten Wert.

Neuer Ansatz FAST PWM:

fpwm = fclock / ( N*(1+TOP) )


Vorteilung N = 1

fclock = 16.000.000 Hz

fpwm = soll 50kHz (maximale Wandlungsfrequenz des AD-Wandlers: 60kHz, etwas drunter bleiben)

Allerdings ist die PWM-Frequenz des Motortreibers auf 5kHz begrenzt, also wird die genommen:
Deshalb:
fpwm = 5kHz

TOP = fclock/(fpwm*N)  - 1 = 16000000 / (5000*1) - 1 = 3199 (relativ klein!)
=> TOP = 3199
Konfiguration von Timer1 mit Mode 14, Seite 123 Datenblatt ATmega32U4


fpwm = fclock / ( N*(1+TOP) ) = 16.000.000 / ( 1*(4999+1) )

Code 0-6: Überlegungen zur neuen Version

ÜBUNGSAUFGABEN
  • Wie sähe der PID-Regler aus dem Quelltext oben in kontinuierlicher Schreibweise aus?
  • Versuche die Übertagungsfunktion der Regelstrecke anhand der Sprungantwort und einer Annahme bzgl. des Typs der Übertragungsfunktion zu bestimmen.
  • Teste verschiedene Verfahren, um den Regler anhand der identifizierten Sprungantwort auszulegen. Überprüfe die Ergebnisse am realen System.
-

VERSION 2.0

-
Veränderte Fragestellung: Welche Regelstrecke liegt zugrunde, wenn sie PID-geregelt diese Rohdaten liefert?

Bild 0-5: Veränderte Fragestellung: Welche Regelstrecke liegt zugrunde, wenn sie PID-geregelt diese Rohdaten liefert?

mess15.sce.zip - Neue Rohdaten.
//5000Hz: Schafft D-Anteil nicht mehr:
#define MOTORMAX 3199
#define MAXARR 1000
#define SOLLSTART -200
volatile long int sensor=0;
volatile long int inx=0;

volatile long int SOLLWERT=SOLLSTART;


//Für ADW:
volatile unsigned int akku, akku2;

volatile bool toggle = false;

volatile unsigned int ZEITSCHRIITE = 0;

volatile long int e=0,e_alt=0,e_diff=0,e_integ=0,stellsignal=0;
//volatile long int P  = 40;
volatile long int P  = 1500;
volatile long int I  = 4;
volatile long int D  = 1500;

volatile long int HAFT=0;

int arr[MAXARR];
int index=0;
int ZUSTAND=0;
//dt = 0.0002; //5000Hz == 0.0002s
void initTimer1()
{
    //Timer1, Mode 14 WGM13..0 1110
    //Vorteilung 1, CS12..0  001 
    TCCR1B = (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10) ;
    TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (1<<WGM11) | (0<<WGM10);
    //TOP=ICR1=3199
    //N=1
    //fpwm = fclock / ( N*(1+TOP) ) = 16000000/3200==5000Hz    
    
    ICR1 = MOTORMAX;
  
    DDRB |= (1<<PB5);  //ist D9 / OC1A
    
    //ISR aufgrund Überlauf Timer 1:
    TIMSK1 |= (1<<OCIE1A);  
    //TIMSK1 |= (1<<ICIE1);  //Match mit ICR1
    
    //Interrupts aktivieren
    sei(); 
}

void initADW()
{
    //A0 ist ADC7 auf dem Arduino Micro:
    //MUX:000111
  
    //Vorteilung bei 16Mhz soll 60kHz ergeben, deshalb: 16000000/60000 = 266,67
    //Nächster Wert: 256, also 16000000/256 = 62500Hz (o.k.)
  
    //ADLAR==1 => oberen 8 Bit komplett in ADCH, unteren 2 in oberen von ADCL
    //ADEN==1 aktiv
    //ADSC==1 start conversion / start 1st bei free running
    //ADTS3..0 ==0 free running mode
    
    //REFS1..0 00==VREF intern , extern ausgeschaltet
    //ADHSM 1==High speed , mehr Stromverbrauch
    
    //ADTS3..0    Koppeln an Timer 1 Overflow: 0110
    ADCSRA = (1<<ADEN) | (1<<ADSC) | (0<<ADATE) | (0<<ADIF) | (0<<ADIE) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);  
    ADCSRB = (1<<ADHSM) | (0<<ACME) | (0<<MUX5) | (0<<ADTS3) | (1<<ADTS2) | (1<<ADTS1) | (0<<ADTS0);
    ADMUX = (1<<REFS1) | (0<<REFS0) | (1<<ADLAR) | (0<<MUX4) | (0<<MUX3) | (1<<MUX2) | (1<<MUX1) | (1<<MUX0);  
}

void setzeMotor(long int wert) // +/- 3199
{
      if(wert>MOTORMAX)
          wert=MOTORMAX;
      if(wert<-MOTORMAX)    
          wert=-MOTORMAX;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
          analogWrite(9,wert);
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
          analogWrite(9,-wert);
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
          analogWrite(9,0);
      }
}

void setup() 
{    
    initADW();  
    
    initTimer1();

    pinMode(12,INPUT); //Taster
    digitalWrite(12,1);

    pinMode(11,OUTPUT); //Lautsprecher

    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
}

void aktualisiereSensorwert()
{    
    akku2 = ADCL; //muss erst ausgelesen werden, damit ADCL verfügbar ist
    akku  = ADCH;
    ADCSRA |= (1<<ADSC);  //Neue Wandlung anfordern, ist (hoffentlich) fertig bei nächstem Durchlauf    
    sensor = (akku<<2) | (akku2>>6);            
    sensor-=608;
}

void loopA()
{
    aktualisiereSensorwert();
    
    e_alt = e;
    e = SOLLWERT-sensor;
    e_integ+=e;
    e_diff = (e-e_alt);

    if(e_integ>MOTORMAX)
        e_integ = MOTORMAX;
    if(e_integ<-MOTORMAX)
        e_integ = -MOTORMAX;
    //P-Regler:
    //stellsignal = P*e;
    //PI-Regler:
    //stellsignal = P*e + I*e_integ;
    //PID-Regler:
/*    
    if(sensor>0)
        HAFT=-1500;
    else if(sensor<0)    
        HAFT=1500;
    else
        HAFT=0;    
*/    
    stellsignal = (P*e)/100 + (D*e_diff)/100 + (I*e_integ)/100 + HAFT;     
    setzeMotor(stellsignal);

    //if(ZEITSCHRIITE%100==0)
    //    Serial.println(sensor);
    
    if(digitalRead(12)==0)
    {
           index=0;
           ZUSTAND=1;
           SOLLWERT = 0;
    }
}

void loopB()
{
    aktualisiereSensorwert();
    
    e_alt = e;
    e = SOLLWERT-sensor;
    e_integ+=e;
    e_diff = (e-e_alt);

    if(e_integ>MOTORMAX)
        e_integ = MOTORMAX;
    if(e_integ<-MOTORMAX)
        e_integ = -MOTORMAX;
    stellsignal = (P*e)/100 + (D*e_diff)/100 + (I*e_integ)/100 + HAFT;     
    setzeMotor(stellsignal);

    if(ZEITSCHRIITE%10==0)        
    {
       arr[index]=sensor;
       index++;
    }  
       if(index>=MAXARR)
       {
            ZUSTAND=2;
            index=0;
            setzeMotor(0);             
            SOLLWERT = SOLLSTART;
       }
}

void loopC()
{
       Serial.print(index);
       Serial.print(" ");
       Serial.println(arr[index]);
       index++;
       if(index>=MAXARR)
       {
            ZUSTAND=0;
            index=0;
            SOLLWERT=SOLLSTART;
       }
}

ISR(TIMER1_COMPA_vect)  //Ein Durchgang
{

    switch(ZUSTAND)  
    {
        case 0:
            loopA();
        break;
        case 1:
            loopB();
        break;
        case 2:
            loopC();
        break;
    }

    if(toggle)
        digitalWrite(11,1);
    else
        digitalWrite(11,0);
    
    toggle=!toggle;
    ZEITSCHRIITE++;
    ZEITSCHRIITE%=100;
    //theoretisch überflüssig, da an Timer 1 Overflow gekoppelt.
}

void loop() 
{
    //leer, alles passiert zeitlich getaktet in der ISR (Interrupt Service Routine)
}

Code 0-7: Zugrunde liegendes Programm.

-

VERSION 3.0

-
  • P-geregeltes System.
  • Sprung von -300 nach 0 (Abweichung Sensorwert)
  • Idee: P-geregeltes System identifizieren
Sprungantwort des P-geregelten Systems,

Bild 0-6: Sprungantwort des P-geregelten Systems,

mess25.sce.zip - Scilab-Daten
Regler012sprung.zip - Arduino-Projekt

//5000Hz: Schafft D-Anteil nicht mehr:
//TEST mit 50000Hz !!!!:
//5000Hz
//#define MOTORMAX 3199
//50000Hz
//#define MOTORMAX 319
//20000Hz
#define MOTORMAX 799
#define MAXARR 200

volatile int arr[MAXARR];
volatile unsigned int ZEITSCHRITTE = 0;

int ZUSTAND=0;
//dt = 0.0002; //5000Hz == 0.0002s

void setzeMotor(long int wert) // +/- 3199
{
      if(wert>MOTORMAX)
          wert=MOTORMAX;
      if(wert<-MOTORMAX)    
          wert=-MOTORMAX;
          
      if(wert>0)    
      {
          digitalWrite(7,1);
          digitalWrite(8,0);
//          analogWrite(9,wert);
          OCR1A = wert;
      }    
      else if(wert<0)
      {
          digitalWrite(7,0);
          digitalWrite(8,1);
//          analogWrite(9,-wert);
          OCR1A = -wert;
      }
      else
      {
          digitalWrite(7,0);
          digitalWrite(8,0);
//          analogWrite(9,0);
          OCR1A = 0;
      }
}

#include "MODULE.h"
#include "LOOP.h"
LOOP* loops[3] = {&loopA,&loopB,&loopC};

void setup() 
{    
    pinMode(12,INPUT); //Taster
    digitalWrite(12,1);

    pinMode(11,OUTPUT); //Lautsprecher

    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(7,0);
    digitalWrite(8,0);
    Serial.begin(9600);
    
    adw.start();
    timer1.start();
        
}

ISR(TIMER1_COMPA_vect)  //Ein Durchgang
{
    loops[ZUSTAND]->exec();
  
    if(ZEITSCHRITTE%10==0)
        digitalWrite(11,1);
    if(ZEITSCHRITTE%10==5)
        digitalWrite(11,0);

    pinMode(12,INPUT); //Taster
    digitalWrite(12,1);

    
    ZEITSCHRITTE++;
    ZEITSCHRITTE%=500;  //0..499 == 0.01 Sekunde
    //theoretisch überflüssig, da an Timer 1 Overflow gekoppelt.
}

void loop() 
{
    //leer, alles passiert zeitlich getaktet in der ISR (Interrupt Service Routine)
}

Code 0-8: Hauptprogramm

class TIMER1
{
   public:
     TIMER1()
     {
     }
     
     void start()
     {
        //Timer1, Mode 14 WGM13..0 1110
        //Vorteilung 1, CS12..0  001 
        TCCR1B = (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (0<<CS11) | (1<<CS10) ;
        TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (1<<WGM11) | (0<<WGM10);
        //TOP=ICR1=3199
        //N=1
        //fpwm = fclock / ( N*(1+TOP) ) = 16000000/3200==5000Hz    
    
        ICR1 = MOTORMAX;
  
        DDRB |= (1<<PB5);  //ist D9 / OC1A
    
        //ISR aufgrund Überlauf Timer 1:
        TIMSK1 |= (1<<OCIE1A);  
        //TIMSK1 |= (1<<ICIE1);  //Match mit ICR1
    
        //Interrupts aktivieren
        sei();            
     }
} timer1;

class ADW
{
    public:
        long int sensor=0;
        unsigned int akku, akku2;
    
        ADW()
        {
        }

        void start()
        {
            //A0 ist ADC7 auf dem Arduino Micro:
            //MUX:000111
  
            //Vorteilung bei 16Mhz soll 60kHz ergeben, deshalb: 16000000/60000 = 266,67
            //Nächster Wert: 256, also 16000000/256 = 62500Hz (o.k.)
  
            //ADLAR==1 => oberen 8 Bit komplett in ADCH, unteren 2 in oberen von ADCL
            //ADEN==1 aktiv
            //ADSC==1 start conversion / start 1st bei free running
            //ADTS3..0 ==0 free running mode
    
            //REFS1..0 00==VREF intern , extern ausgeschaltet
            //ADHSM 1==High speed , mehr Stromverbrauch
    
            //ADTS3..0    Koppeln an Timer 1 Overflow: 0110
            ADCSRA = (1<<ADEN) | (1<<ADSC) | (0<<ADATE) | (0<<ADIF) | (0<<ADIE) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);  
            ADCSRB = (1<<ADHSM) | (0<<ACME) | (0<<MUX5) | (0<<ADTS3) | (1<<ADTS2) | (1<<ADTS1) | (0<<ADTS0);
            ADMUX = (1<<REFS1) | (0<<REFS0) | (1<<ADLAR) | (0<<MUX4) | (0<<MUX3) | (1<<MUX2) | (1<<MUX1) | (1<<MUX0);                      
        }
        
        void aktualisieren()
        {
            akku2 = ADCL; //muss erst ausgelesen werden, damit ADCL verfügbar ist
            akku  = ADCH;
            ADCSRA |= (1<<ADSC);  //Neue Wandlung anfordern, ist (hoffentlich) fertig bei nächstem Durchlauf    
            sensor = (akku<<2) | (akku2>>6);            
            //sensor-=608;          
        }
        
        long int holeWert()
        {
            return sensor;
        }
} adw;

Code 0-9: MODULE.h

class LOOP
{
    public:
       virtual void exec() = 0;       
};

class LoopA : public LOOP
{
    public:
        unsigned long inx;
        unsigned long index;
    
    
        LoopA()
        {
            inx=0;
            index=0;
        }
    
        void exec()
        {
            adw.aktualisieren();
            setzeMotor(   2*(-300-(adw.holeWert()-608))   );
            
            if(inx%100==0)
               index++;
                                    
            inx++;
            
            if(index>=MAXARR)
            {
                inx=0;
                index=0;
                ZUSTAND=1;
            }
        }
} loopA;

class LoopB : public LOOP
{
    public:
        unsigned long index,inx;
        
        LoopB()
        {
             index=0;
             inx=0;
        }
        
        void exec()
        {
            adw.aktualisieren();
            setzeMotor(   2*(0-(adw.holeWert()-608))   );
            if(inx%100==0) //20000Hz, %100 => 200Hz
            {
                arr[index]=(adw.holeWert()-608);            
                index++;
            }    
            inx++;
            
            if(index>=MAXARR)
            {
                index=0;
                inx=0;
                ZUSTAND=2;
                setzeMotor(0);
            }
        }
} loopB;

class LoopC : public LOOP
{
    public:    
        unsigned long index,inx;
        
        LoopC()
        {
             index=0;
             inx=0;
        }
        
        void exec()
        {
            if(inx%100==0)
            {
                Serial.print(index);
                Serial.print(" ");
                Serial.println(arr[index]);
                index++;
            }    
            
            inx++;
            
            if(index>=MAXARR)
            {
                ZUSTAND=0;
                index=0;
                inx=0;
            }             
        }
} loopC;

Code 0-10: LOOP.h

Lösungsvarianten der System-Indentifikation
regelsysteme_20160504.zip
Ordner

Bild 0-7: Ordner "identifikation_siehe_Kap21_unten" - PT3-System

Ordner

Bild 0-8: Ordner "identifikation_VERBESSERT5" - PT2-System mit richtungsabhängiger Gleitreibung, [a,b,c,d]=[362.51078 0.3013469 88.277654 23399.98].