Odotest

#include "stdlib.h"
#include "asuro.h"

void SerWert(unsigned int iWert) {
    unsigned char cWert[6] = "     ";

    itoa(iWert, cWert, 10);
    SerWrite(cWert, 5);
    SerWrite(", ", 2);
}

/** Hauptprogramm */
int main(void)
{
    unsigned int Rmin = 1024;
    unsigned int Rmax = 0;
    unsigned int Lmin = 1024;
    unsigned int Lmax = 0;
    /** Geschwindigkeit */
    int speed = 90;
    /** Odometrie links/rechts */
    unsigned int data[2];

    Init();
    GREEN_LED_ON;
    
    MotorDir(FWD,FWD);
    MotorSpeed(speed, speed);
    
    while(1) {
        OdometrieData(data); // 0. links, 1. rechts

        // max links
        if (data[0] > Lmax)
            Lmax += (data[0] - Lmax) / 2;
        // min links
        if (data[0] < Lmin)
            Lmin -= (Lmin - data[0]) / 2;
        
        // max rechts
        if (data[1] > Rmax)
            Rmax += (data[1] - Rmax) / 2;
        // min rechts
        if (data[1] < Rmin)
            Rmin -= (Rmin - data[1]) / 2;

        SerWert(Lmax);
        SerWert(Lmin);
        SerWert(Rmax);
        SerWert(Rmin);
        SerWrite("\n\r", 2);

    }
    
    return 0;
}