/*  BSD-License:

Copyright (c) 2010 by Matthias Bunte, Germany

All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  * Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.
  * Neither the names of the authors the name nicai-systems nor
    the names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

/*! @file    motorpid.c
 *  @brief   Regulating the motor speed with PID
 *  @author  Matthias Bunte (m_bunte@arcor.de)
 *  @date    2010-04-02
 */

#include <stdlib.h>
#include <avr/interrupt.h>
#include "iodefs.h"
#include "scheduler.h"
#include "odometers.h"
#include "motorctrl.h"
#include "motorpid.h"

//#undef MOTORPID_RAM_OPTIMIERT
#define MOTORPID_RAM_OPTIMIERT

// 
#define MOTORPID_SPEED_MAX          (100)
//#define MOTORPID_SPEED_SHIFT        (10)
#define MOTORPID_SPEED_SHIFT        (12)

#define MOTORPID_MIN_START_VAL      (MOTORCTRL_FULLSPEED_VAL >> 2)
//#define MOTORPID_MIN_START_VAL      ((MOTORCTRL_FULLSPEED_VAL >> 3) + (MOTORCTRL_FULLSPEED_VAL >> 4))
#define MOTORPID_MAX_START_VAL      (MOTORCTRL_FULLSPEED_VAL - (MOTORCTRL_FULLSPEED_VAL >> 3))

// Umrechnung von cm auf Radsensorticks: Auf 10 cm machen die Radsensoren etwa 35 Ticks.
#define MOTORPID_CM_TO_TICKS(WAYCM)     ( ((int16_t)WAYCM * 35) / 10 )

// Immer in 1/16-Schritten regeln
#define MOTORPID_DIVISOR      (4)

// Minimale Anzahl Ticks, die gemessen wird:
#define MOTORPID_MIN_TICKS    (2)

// Minimale Zeit, die fuer einen Tick vergeht,
// Kehrwert der maximalen Geschwindigkeit:
// v_max = 8 Ticks in 50 ms = 0,160 Ticks / ms
// => 1 / v_max = (1 / 0,160) ms / Tick = 6,25 ms / Tick
// Zur Berechnung der Zeit in Prozenten als Grundlage den
// Wert mit 100 multiplizieren, also 6,25 * 100 = 625

// Nachteilig ist, dass nun zwar bei 100 % die maximale Geschwindigkeit
// erreicht wird, unter 11 % die Biene allerdings stehen bleibt.
// Die minimale Geschwindigkeit liegt also bei 11 % der maximalen
// Geschwindigkeit, also um 10 %-Punkte nach oben verschoben. Um die
// Skala von 1 % - 100 % nutzbar zu machen, wird der Prozentwert intern
// um 10 %-Punkte erhoeht, im Gegenzug muss der Basiswert fuer die Zeit-
// berechnung korrigiert werden: 6,25 * 110 = 687,5 (688)
// An die Funktionen wird weiterhin 1..100% uebergeben, intern wird der
// Offset MOTORPID_SPEED_SHIFT aufgeschlagen.

//#define MOTORPID_MIN_TIME_PER_TICK  (688)
#define MOTORPID_MIN_TIME_PER_TICK  (840)

#define MOTORPID_MIN_TIME_PER_MIN_TICKS (MOTORPID_MIN_TIME_PER_TICK * MOTORPID_MIN_TICKS)
#define MOTORPID_MIN_MEASURE_TIME     (40)
#define MOTORPID_MOTOR_STEHT_TIMEOUT  (750)

#define MOTORPID_MAX_STEERING_VAL       (100)
#define MOTORPID_STEERING_DIV_VAL       (2)
#define MOTORPID_MAX_DIV_STEERING_VAL   (MOTORPID_MAX_STEERING_VAL / MOTORPID_STEERING_DIV_VAL)



//
#define MOTORPID_CHANGE_DIR(ISBACKWARD) do { \
  if (ISBACKWARD) \
  { \
    ISBACKWARD = 0; \
  } \
  else  \
  { \
    ISBACKWARD = 1; \
  } \
} while (0)


//
#define MOTORPID_CHANGE_DIR_FLAG(ISBACKWARD_FLAG) do { \
  if (GET_FLAG(MotorPID_Flags, ISBACKWARD_FLAG)) \
  { \
    CLR_FLAG(MotorPID_Flags, ISBACKWARD_FLAG); \
  } \
  else  \
  { \
    SET_FLAG(MotorPID_Flags, ISBACKWARD_FLAG); \
  } \
} while (0)


#define MOTORPID_INITSTATE        (_BV(0))
#define MOTORPID_BLOCKED          (_BV(1))
#define MOTORPID_ODOCOUNT_ISLEFT  (_BV(2))
#define MOTORPID_L_ISBACKWARD     (_BV(3))
#define MOTORPID_R_ISBACKWARD     (_BV(4))


#ifdef __cplusplus
extern "C" {
#endif

#ifdef MOTORPID_RAM_OPTIMIERT
 // Flags
 static uint8_t  MotorPID_Flags = 0;
#else
 static uint8_t MotorPID_InitState = 0;
 static uint8_t MotorPID_Blocked;
 static int8_t  MotorPID_L_IsBackward;
 static int8_t  MotorPID_R_IsBackward;
 static uint8_t  MotorPID_OdoCountIsLeft;
#endif

// Sollgeschwindigkeit links und rechts, Richtung
static int8_t MotorPID_L_Speed;
static int8_t MotorPID_R_Speed;
// Beim Starten der Regelung muessen die Radsensor-Werte aktualisiert
// werden. Damit die Start-Funktion zugreifen kann, modulglobal anlegen
static uint8_t  MotorPID_Last_L_OdoVal;
static uint8_t  MotorPID_Last_R_OdoVal;
// 
static uint8_t  MotorPID_Old_L_OdoVal;
static uint8_t  MotorPID_Old_R_OdoVal;
// Wenn nur ein bestimmter Weg zurueckgelegt werden soll:
// So viele Radsensor-Ticks sind zurueckzulegen
// Achtung! int16, nicht uint16!!!
// Wenn das rechte Rad als Referenz gilt (Linkskurven) ist IsLeft geloescht
static int16_t  MotorPID_OdoCount;
// Zeiger auf Event-Funktion, die bei Motorstillstand oder erreichen
// der zurueckgelegten Wegstrecke aufgerufen wird
static pMotorPID_EventFkt_t pMotorPID_EventFkt;
// Speicher fuer internen Eventhandler
static uint16_t MotorPID_EventVal;
//
static uint16_t MotorPID_L_NewSpeed;
static uint16_t MotorPID_R_NewSpeed;

// Interner Eventhandler, falls kein externer zum Einsatz kommen sollte
void MotorPID_InternalEventHandler(uint16_t Event)
{
  // Event intern speichern
  MotorPID_EventVal = Event;
}

/*!
 * Liefert das zuletzt aufgetretene Event, damit auch ohne Eventhandler
 * Events ausgewertet werden koennen.
 */
uint16_t MotorPID_GetLastEvent(void)
{
  uint16_t LastEventVal = MotorPID_EventVal;
  
  MotorPID_EventVal = MotorPID_EventInvalid;
  return LastEventVal;
}

// Initialisierung der Motorregelung
void MotorPID_Init(pMotorPID_EventFkt_t pEventFkt)
{
  // Benutze SystemTimer, Odometers und MotorCtrl -> ggfs Initialisieren
  
  Odometers_CheckAndInit();
  MotorCtrl_CheckAndInit();
  // Geschwindigkeit und Drehung zuruecksetzen
  MotorPID_L_Speed = 0;
  MotorPID_R_Speed = 0;
  // Eventfunktion zuweisen
  if (pEventFkt)
  {
    pMotorPID_EventFkt = pEventFkt;
  }
  else
  {
    pMotorPID_EventFkt = MotorPID_InternalEventHandler;
  }
  // Internen Eventspeicher initialisieren
  MotorPID_EventVal = MotorPID_EventInvalid;
  // Wegstreckenzaehler zuruecksetzen
  MotorPID_OdoCount = 0;
#ifdef MOTORPID_RAM_OPTIMIERT
  MotorPID_Flags = 0;
  // Initialisierung ist nun abgeschlossen
  SET_FLAG(MotorPID_Flags, (MOTORPID_ODOCOUNT_ISLEFT | MOTORPID_INITSTATE));
#else
  MotorPID_L_IsBackward = 0;
  MotorPID_R_IsBackward = 0;
  MotorPID_OdoCountIsLeft = 1;
  // Motor-Block-Erkennung aus
  MotorPID_Blocked = 0;
  // Initialisierung ist nun abgeschlossen
  MotorPID_InitState = 1;
#endif
}

// Initialisierungszustand auslesen
uint8_t MotorPID_Initialized(void)
{
#ifdef MOTORPID_RAM_OPTIMIERT
  return GET_FLAG(MotorPID_Flags, MOTORPID_INITSTATE);
#else
  return MotorPID_InitState;
#endif
}

// Faengt Stillstand eines oder beider Motoren ab
static inline void MotorPID_BlockedFkt(uint16_t dummy)
{
  uint8_t  Actual_L_OdoVal;
  uint8_t  Actual_R_OdoVal;
  
  Actual_L_OdoVal = Odometers_Get_L_Val();
  Actual_R_OdoVal = Odometers_Get_R_Val();
  if (Actual_L_OdoVal == MotorPID_Old_L_OdoVal)
  {
    if (Actual_R_OdoVal == MotorPID_Old_R_OdoVal)
    {
      Add_TT_Entry( 1, pMotorPID_EventFkt, MotorPID_Event_LR_Blocked);
    }
    else
    {
      Add_TT_Entry(1, pMotorPID_EventFkt, MotorPID_Event_L_Blocked);
    }
  }
  else if (Actual_R_OdoVal == MotorPID_Old_R_OdoVal)
  {
    Add_TT_Entry(1, pMotorPID_EventFkt, MotorPID_Event_R_Blocked);
  }
#ifdef MOTORPID_RAM_OPTIMIERT
  CLR_FLAG(MotorPID_Flags, MOTORPID_BLOCKED);
#else
  // 
  MotorPID_Blocked = 0;
#endif
}

// Steuerung eines Motors
static inline void MotorPID_Running(uint8_t IsLeft)
{
                             // Zwischenspeicher
   uint8_t  OdoVal;
   uint8_t  LastOdoVal;
   uint8_t  OdoDiff;
   int8_t   Speed;
   uint16_t NewSpeed;
   int8_t   IsBackward;

                             // 
   if (IsLeft)
   {
      OdoVal      = Odometers_Get_L_Val();
      LastOdoVal  = MotorPID_Last_L_OdoVal;
      Speed       = MotorPID_L_Speed;
      NewSpeed    = MotorPID_L_NewSpeed;
#ifdef MOTORPID_RAM_OPTIMIERT
      IsLeft      = MOTORPID_ODOCOUNT_ISLEFT;
      IsBackward  = GET_FLAG(MotorPID_Flags, MOTORPID_L_ISBACKWARD);
#else
      IsBackward  = MotorPID_L_IsBackward;
#endif
   }
   else
   {
      OdoVal      = Odometers_Get_R_Val();
      LastOdoVal  = MotorPID_Last_R_OdoVal;
      Speed       = MotorPID_R_Speed;
      NewSpeed    = MotorPID_R_NewSpeed;
#ifdef MOTORPID_RAM_OPTIMIERT
      IsBackward  = GET_FLAG(MotorPID_Flags, MOTORPID_R_ISBACKWARD);
#else
      IsBackward  = MotorPID_R_IsBackward;
#endif
   }
                                 // Ermittle Differenz zum letzten Aufruf
   OdoDiff =  OdoVal - LastOdoVal;

   // Pruefe, ob diese Regelunsgseite fuer eine Wegstreckenbegrenzung zu verwenden ist
   // Wenn ja, pruefe, ob die Wegstrecke in vorigen laeufen noch nicht abgelaufen war
   // Dann halte die Regelung (und Motoren) an, rufe Eventfunktion auf und breche diese
   // Funktion ab
#ifdef MOTORPID_RAM_OPTIMIERT
   if (GET_FLAG(MotorPID_Flags, MOTORPID_ODOCOUNT_ISLEFT) == IsLeft)
#else
   if (MotorPID_OdoCountIsLeft == IsLeft)
#endif
   {
      if (MotorPID_OdoCount > 0)
      {
         MotorPID_OdoCount -= OdoDiff;
//      if (MotorPID_OdoCount <= 0)
         if (MotorPID_OdoCount <= (Speed >> 1))
         {
            // Verhindert, dass die nachfolgende Regelung ausgefuehrt wird
            // indem das Soll gleich dem Ist gesetzt wird
            OdoDiff = Speed;
            // Anhalten und Eventfunktion in 1 ms aufrufen
            MotorPID_Stopp();
            Add_TT_Entry( 1, pMotorPID_EventFkt, MotorPID_EventFinished);
         }
      }
   }

   // Berechne Soll-Ist-Differenz
   Speed -= OdoDiff;
   // Motor mit falscher Geschwindigkeit
   if (Speed != 0)
   {
      int16_t MotorCtrl_Speed;

      MotorCtrl_Speed = (int16_t)MotorCtrl_GetSpeed(IsLeft);

/*
    // Differenz nach unten begrenzen
    if (Speed <= -(1 << MOTORPID_DIVISOR))
    {
      Speed = -(1 << MOTORPID_DIVISOR) + 1;
      led_set(LED_L_RD, On);
      led_set(LED_R_RD, On);
    }
*/

      if ( (Speed > 0) && (MotorCtrl_Speed == MOTORCTRL_FULLSPEED_VAL) )
      {
#ifdef MOTORPID_RAM_OPTIMIERT
         if (!GET_FLAG(MotorPID_Flags, MOTORPID_BLOCKED))
         {
            SET_FLAG(MotorPID_Flags, MOTORPID_BLOCKED);
#else
         if ( !MotorPID_Blocked )
         {
             MotorPID_Blocked = 1;
#endif
            MotorPID_Old_L_OdoVal = Odometers_Get_L_Val();
            MotorPID_Old_R_OdoVal = Odometers_Get_R_Val();
            Add_TT_Entry( MOTORPID_MOTOR_STEHT_TIMEOUT, MotorPID_BlockedFkt, 0);
         }
      }
      else
      {
         MotorCtrl_Speed += Speed * (MotorCtrl_Speed >> MOTORPID_DIVISOR);
      }
      if (MotorCtrl_Speed <= 0)
      {
         MotorCtrl_Speed = MOTORPID_MIN_START_VAL;
         if (MotorCtrl_Speed < NewSpeed)
         {
            MotorCtrl_Speed = NewSpeed;
         }
      }
      MotorCtrl_SetSpeedDir(IsLeft, (uint16_t)MotorCtrl_Speed, IsBackward);
   }

   // Aktuelle Odo-Ticks merken
   if ( IsLeft )
   {
      MotorPID_Last_L_OdoVal = OdoVal;
   }
   else
   {
      MotorPID_Last_R_OdoVal = OdoVal;
   }
}

 // Linken Motor in TimerVal-Abstaenden regeln
static inline void MotorPID_RunLeft(uint16_t TimerVal)
{
  // Funktion in TimerVal wieder aufrufen
  Add_TT_Entry( TimerVal, MotorPID_RunLeft, TimerVal);
  // Regelungsfunktion ausfuehren
  MotorPID_Running(MOTORCTRL_LEFT);
}

// Rechten Motor in TimerVal-Abstaenden regeln
static inline void MotorPID_RunRight(uint16_t TimerVal)
{
  // Funktion in TimerVal wieder aufrufen
  Add_TT_Entry(TimerVal, MotorPID_RunRight, TimerVal);
  // Regelungsfunktion ausfuehren
  MotorPID_Running(MOTORCTRL_RIGHT);
}

// Stoppe Motor
void MotorPID_Stopp(void)
{
  // Loesche Steuerungsfunktion in Timer
  Del_TT_Funktion( MotorPID_BlockedFkt );
  Del_TT_Funktion( MotorPID_RunLeft );
  Del_TT_Funktion( MotorPID_RunRight );
  // Blockierungserkennung zuruecksetzen
#ifdef MOTORPID_RAM_OPTIMIERT
  CLR_FLAG(MotorPID_Flags, MOTORPID_BLOCKED);
#else
  // 
  MotorPID_Blocked = 0;
#endif
  // Halte Motoren an
  MotorCtrl_Stopp();
}

// Starte Motor
//void MotorPID_Start(int8_t Speed, int8_t Steering, uint8_t WayCm, pMotorPID_EventFkt_t ReadyEventFkt)
void MotorPID_Start(int8_t Speed, int8_t Steering, uint8_t WayCm)
{
  uint16_t Time_L;
  uint16_t Time_R;

  // Motorsteuerung anhalten
  MotorPID_Stopp();

  // Wenn Speed einen Wert ungleich Null hat, soll die Biene fahren
  if (Speed != 0)
  {
    // Eventfunktion mit Startevent aufrufen
    Add_TT_Entry(1, pMotorPID_EventFkt, MotorPID_EventStarted);
//    pMotorPID_EventFkt(MotorPID_EventStarted);  // Start without timing system
    // Wegstreckenzaehler einstellen, die Strecke wird links gezaehlt
    MotorPID_OdoCount       = MOTORPID_CM_TO_TICKS(WayCm);
#ifdef MOTORPID_RAM_OPTIMIERT
    SET_FLAG(MotorPID_Flags, MOTORPID_ODOCOUNT_ISLEFT);
#else
    MotorPID_OdoCountIsLeft = 1;
#endif
    // Wenn Speed negativ ist, soll die Biene rueckwaerts fahren,
    // sonst soll sie vorwaerts fahren
    if (Speed < 0)
    {
      // Fahrtrichtung BACKWARD einstellen und Speed absolut berechnen
      Speed = -Speed;
#ifdef MOTORPID_RAM_OPTIMIERT
      SET_FLAG(MotorPID_Flags, (MOTORPID_L_ISBACKWARD | MOTORPID_R_ISBACKWARD));
#else
      MotorPID_L_IsBackward = 1;
#endif
    }
    else
    {
      // Fahrtrichtung FORWARD einstellen
#ifdef MOTORPID_RAM_OPTIMIERT
      CLR_FLAG(MotorPID_Flags, (MOTORPID_L_ISBACKWARD | MOTORPID_R_ISBACKWARD));
#else
      MotorPID_L_IsBackward = 0;
#endif
    }
    // Annahme: Geradeausfahrt, dann ist die Drehrichtung des rechten Rades
    // die gleiche wie die des linken Rades
#ifdef MOTORPID_RAM_OPTIMIERT
#else
    MotorPID_R_IsBackward = MotorPID_L_IsBackward;
#endif
    // Speed auf +/-100 begrenzen
    if (Speed > MOTORPID_SPEED_MAX)
    {
      Speed = MOTORPID_SPEED_MAX;
    }

    // Initialwerte fuer Regelung berechnen
    MotorPID_L_NewSpeed = ( MOTORPID_MAX_START_VAL * Speed) / MOTORPID_SPEED_MAX;
    MotorPID_R_NewSpeed = MotorPID_L_NewSpeed;
    // Geschwindigkeit in Zeit zwischen zwei Radsensor-Ticks umrechnen
    Speed += MOTORPID_SPEED_SHIFT;
    Time_L = MOTORPID_MIN_TIME_PER_MIN_TICKS / Speed;
    Time_R = Time_L;
    
    // Ab hier Geschwindigkeitsanpassung fuer Kurvenfahrten berechnen
    // Es soll eine Kurve gefahren werden, mit dieser Interpretation von Steering:
    // -100 bis -1: Es soll eine Linkskurve gefahren werden
    // 0:           Es soll geradeaus gefahren werden
    // 1 bis 100:   Es soll eine Rechtskurve gefahren werden
    // Noch genauer:
    // -100 bis -51: Das linke Rad soll sich andersherum drehen als das rechte Rad
    // -50:          Das linke Rad soll stehen bleiben
    // -49 bis -1:   Das linke Rad soll sich langsamer drehen als das rechte Rad
    // 1 bis 49:   Das rechte Rad soll sich langsamer drehen als das linke Rad
    // 50:         Das rechte Rad soll stehen bleiben
    // 51 bis 100: Das rechte Rad soll sich andersherum drehen als das linke Rad
    // Steering erstmal durch zwei teilen, da sonst ein Ueberlauf bei der
    // Geschwindigkeitsberechnung stattfinden wuerde
    Steering /= MOTORPID_STEERING_DIV_VAL;
    if (Steering)
    {
      // Es soll linksherum gefahren werden
      if (Steering < 0)
      {
        // Verschieben des Parameters in den gueltigen Wertebereich
        if (Steering < -MOTORPID_MAX_DIV_STEERING_VAL)
        {
          Steering = -MOTORPID_MAX_DIV_STEERING_VAL;
        }
        // Den Steering-Wert "zentrieren"
        Steering += (MOTORPID_MAX_DIV_STEERING_VAL / 2);
        // Wenn Steering jetzt gleich Null ist, linkes Rad anhalten
        if (!Steering)
        {
          Time_L = 0;
        }
        else
        {
          // 
          if (Steering < 0)
          {
            Steering = -Steering;
#ifdef MOTORPID_RAM_OPTIMIERT
            MOTORPID_CHANGE_DIR_FLAG(MOTORPID_L_ISBACKWARD);
#else
            MOTORPID_CHANGE_DIR(MotorPID_L_IsBackward);
#endif
          }
          // 
          Time_L = (Time_L * (MOTORPID_MAX_DIV_STEERING_VAL / 2)) / Steering;
          //
          MotorPID_L_NewSpeed = ( (MotorPID_L_NewSpeed * Steering)
                              /   (MOTORPID_MAX_DIV_STEERING_VAL / 2) );
          // Das linke Rad faehrt langsamer als das rechte, also fuer die
          // Streckenberechnung das rechte Rad zu Grunde legen
#ifdef MOTORPID_RAM_OPTIMIERT
          CLR_FLAG(MotorPID_Flags, MOTORPID_ODOCOUNT_ISLEFT);
#else
          MotorPID_OdoCountIsLeft = 0;
#endif
        }
      }
      else
      {
        // Verschieben des Parameters in den gueltigen Wertebereich
        if (Steering > MOTORPID_MAX_DIV_STEERING_VAL)
        {
          Steering = MOTORPID_MAX_DIV_STEERING_VAL;
        }
        // Den Steering-Wert "zentrieren"
        Steering -= (MOTORPID_MAX_DIV_STEERING_VAL / 2);
        // Wenn Steering jetzt gleich Null ist, rechtes Rad anhalten
        if (!Steering)
        {
          Time_R = 0;
        }
        else
        {
          // 
          if (Steering < 0)
          {
            Steering = -Steering;
          }
          else
          {
#ifdef MOTORPID_RAM_OPTIMIERT
            MOTORPID_CHANGE_DIR_FLAG(MOTORPID_R_ISBACKWARD);
#else
            MOTORPID_CHANGE_DIR(MotorPID_R_IsBackward);
#endif
          }
          // 
          Time_R = (Time_R * (MOTORPID_MAX_DIV_STEERING_VAL / 2)) / Steering;
          // 
          MotorPID_R_NewSpeed = ( (MotorPID_R_NewSpeed * Steering)
                              /   (MOTORPID_MAX_DIV_STEERING_VAL / 2) );
        }
      }
    }

    // Regelstrecke berechnen
    // 1. Initialisieren mit der minimalen Anzahl Ticks
    // 2. Solange weiter mit 2 multiplizieren,
    //    bis die Regelzeit mindestens MOTORPID_MIN_MEASURE_TIME ms betraegt
    MotorPID_L_Speed = MOTORPID_MIN_TICKS;
    MotorPID_R_Speed = MotorPID_L_Speed;
    
    MotorPID_Last_L_OdoVal = Odometers_Get_L_Val();
    MotorPID_Last_R_OdoVal = Odometers_Get_R_Val();

#if 1
    if (Time_R)
    {
      while (Time_R < MOTORPID_MIN_MEASURE_TIME)
      {
        Time_R *= 2;
        MotorPID_R_Speed *= 2;
      }
//      SystemTimer_SchedulerAdd(1, MotorPID_RunRight, Time_R);
      MotorPID_RunRight(Time_R);
    }
    if (Time_L)
    {
      while (Time_L < MOTORPID_MIN_MEASURE_TIME)
      {
        Time_L *= 2;
        MotorPID_L_Speed *= 2;
      }
//      SystemTimer_SchedulerAdd(1, MotorPID_RunLeft, Time_L);
      MotorPID_RunLeft(Time_L);
    }
#else
    if (Time_L)
    {
      MotorPID_RunLeft(Time_L);
    }
    if (Time_R)
    {
      MotorPID_RunRight(Time_R);
    }
#endif

  }
}


#ifdef __cplusplus
} // extern "C"
#endif

