NXT NXC: Navigation mit Odometrie, Gyro, Kompass

Modelle zum Nachbauen oder wo gibt es etwas interessantes oder Projekte?

Moderator: Moderatoren

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5058
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

NXT NXC: Navigation mit Odometrie, Gyro, Kompass

Beitragvon HaWe » 20. Aug 2011 12:29

hi,

Hier der Navigations-Roboter mit Odometrie, Gyro und Kompass, alle Sensoren werden dabei nach Plausibilität ihrer Werte verknüpft.
Ich habe das Programm mal aus dem Gyro-Thread ausgekoppelt, weil diese Version umfangreicher ist.
Danke an ketaljen und Alphasucht für eure Ideen und Diskussionsbeiträge!
Robot640.jpg
Johnny 1


Zwischenstand: Tetrix-Tribot-Chassis, Räder mit ca. 8cm Durchmesser, Antrieb 1:2 untersetzt (Motorachse gegenüber Radachsen), Achsenlänge (Radabstand) ca. 30cm;
max. Geschwindigkeit: 360°/1s = ca. 25cm/s.
bei Dreiecksfahrt (3 virtuelle Raumpunkte) über ca. 5m: Fehler am Start/Zielpunkt mit Doppel-Antriebsachse (grob vergleichbar mit Kettenantrieb) 20cm, mit einfacher Antriebsachse (wie bei Tribot) <10cm, meist noch weniger.
Stärkste Fehlerquellen: Gyro-Drift und Kompass-Störfelder in Räumen.
Bei wiederholtem Runden-Fahren (5x, also 25 m ) schwankt die Genauigkeit (Distanzbetrag) um den Start-Zielpunkt im Bereich um +- 2%.
Hier ist der letzte Entwicklungsstand OHNE stochastischen Filter (nur Sensor-Mittelwerte etc.), evtl. kommt bald noch ein Accelerometer (Beschleunigungs-Sensor) dazu.

Plan für weitere künftige Verbesserung:
eigener Task nur für Kalman-Filter für Kompass und Gyro, nur um Gyrodrift herauszurechnen.
Die Differenz beider Ergebnisse ermitteln => =Gyro-Offset !
Rekalibrierung nur, wenn aktuelle Offset-Änderung größer als 2 Standardabweichungen vom bisherigen Mittelwert.
Oder: IMU-Sensor mit integriertem Filter und Sensor Fusion.

Code: Alles auswählen

// Navigator Robot, TriBot architecture
// for NXC, tested with BricxCC 3.3.8.10 (2011-July-03)
// version 0.43
// (c) HaWe 2011
// based on:
// Odometry
// Compass
// Gyro sensor


//--------------------------------------------------
// High level API functions:
//
// void SpinUntilHdg(float TargHdg, char tSensor)
// void MoveToPoint(float xTarg, float yTarg)
//--------------------------------------------------



//--------------------------------------------------
// mechanics, motors + sensors
//--------------------------------------------------

#define WheelDiameter   7.7    // Raddurchmesser
#define WheelGauge     28.8    // Achsbreite (Mittellinie Antriebsräder)
#define WheelGearRatio  0.5    // Untersetzungsgetriebe 1:2


#define Mleft   OUT_B
#define Mright  OUT_C
#define Mboth   OUT_BC
#define Menc(a) MotorRotationCount(a)

#define SMenc            1
#define SEncoderLeft     2
#define SEncoderRight    3
#define SCompass        10
#define SGyro           11
#define SAccel          12
#define SUSFrLeft       20
#define SUSFrRight      21
#define SUSFrMiddle     22
#define SUSLeftSide     23
#define SUSRightSide    24
#define SUSBackw        25


                               // Radumfang
float WheelCircumference   = PI*WheelDiameter;   // 2*PI*r = PI*d

                               // Abrollstrecke pro 1° MotorEncoderTick
float WheelLegPerEncDegree = (PI*WheelDiameter*WheelGearRatio)/360.0;

float MencHdg;                // Heading to Odometry, geographical sense

float fxpos=0, fypos=0;


//--------------------------------------------------

#define GYRO    S3
#define COMPASS S4

float CompHdg;
float GyroHdg, GyroValue, GyroOffset=0.0;


//--------------------------------------------------
// IO functions
//--------------------------------------------------

#define Beep(f,d) PlayTone(f,d)

#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

const char LCDline[]={56,48,40,32,24,16,8,0};

const string clrln="                 ";

inline bool btnhit(){
   return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
         || ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}



//--------------------------------------------------
// math
//--------------------------------------------------

#define min(a,b) (a<b?a:b)

#define max(a,b) (a>b?a:b)

inline long round(float f) { return ((f>=0) ? (f + 0.5) : (f - 0.5)); }


float ArrayDegreesMean (float src[], int len) {
   int i;
   for (i=0; i<len; ++i) {
     if (src[i]>180) src[i]=(src[i]-360);
   }
   return (ArrayMean(src, NA, NA));
}


inline float ArrayMedianF(float src[], int len)
{
  float ftemp[];
  ArraySort(ftemp, src, NA, NA)
  return ftemp[(len-1)/2];
}


//--------------------------------------------------
// sensor calibration
//--------------------------------------------------


safecall void SetGyroOffset()
{
  float old, delta;
  old=GyroOffset;
  do {
    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;
  } while (abs(delta)>0.005)
  Wait(20);
  CompHdg= SensorHTCompass(COMPASS);
  GyroHdg=MencHdg=CompHdg;
}

//--------------------------------------------------

safecall void TestGyroOffset() {
  float old, delta;
  old=GyroOffset;

    float tot = 0;
    for (int i=0; i<200; ++i)
    {
       tot += SensorHTGyro(GYRO, 0); // tot var is 200 gyro readings
       Wait(4);                     // 5ms between analog readings
    }
    Beep(880,100);
    GyroOffset=tot/200.0;
    delta=old-GyroOffset;
    old=GyroOffset;

  if (abs(delta)>0.01)  SetGyroOffset();
}

//--------------------------------------------------
// move commands
//--------------------------------------------------

void SpinUntilHdg(float TargHdg, char tSensor) {

  float currHdg, dHdg;
  int dirle, dirri, speedle, speedri, dle, dri, nspeed=80;

  if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
  else
  if (tSensor==SGyro)     {currHdg=GyroHdg;}
  else
  if (tSensor==SMenc)     {currHdg=MencHdg;}
 
  dHdg=TargHdg-currHdg;
  speedle=speedri=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while(abs(dHdg)>0.5) {

    if ( (abs(dHdg)>10) || ( (speedle<30) && (speedri<30)))  {  // ramp up
      if (speedle<nspeed) speedle+=5;
      if (speedri<nspeed) speedri+=5;
    }

    if (tSensor==SCompass)  {currHdg=SensorHTCompass(COMPASS);}
    else
    if (tSensor==SGyro)     {currHdg=GyroHdg;}
    else
    if (tSensor==SMenc)     {currHdg=MencHdg;}

    dHdg=TargHdg-currHdg;

    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;

    dirle=(dHdg>=0?1:-1);
    dirri=-dirle;
 
    OnFwd(Mright, dirri*(speedri+dri));
    OnFwd(Mleft,  dirle*(speedle+dle));

    if (abs(dHdg)<50) {
      if (dHdg<0) {
        speedle = abs(dHdg)*5;             // left turn:    // ramp down
        speedri = abs(dHdg)*5;
      }
      else if (dHdg>0) {
        speedle = abs(dHdg)*10;            // right turn:   // later ramp down
        speedri = abs(dHdg)*10;
      }

      if (speedle>nspeed) speedle=nspeed;
      if (speedle<30) speedle=30;

      if (speedri>nspeed) speedri=nspeed;
      if (speedri<30) speedri=30;
    }
   
    Wait(10);
  }
  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}

//--------------------------------------------------


void MoveToPoint(float xTarg, float yTarg) {

  float currHdg, dHdg, tx, ty, TargHdg, total, rest;
  int dirle, dirri,  speed, dle, dri, nspeed=80;

  tx=xTarg-fxpos;
  ty=yTarg-fypos;
  TargHdg=atan2(tx,ty)*(180/PI) ;
  total=sqrt(tx*tx+ty*ty);
 
  SpinUntilHdg(TargHdg, SGyro);
  if (abs(MencHdg-GyroHdg)>4) {
    MencHdg=GyroHdg;
  }

  speed=20;

  while(TargHdg < 0)  TargHdg+=360;
  while(TargHdg>=360) TargHdg-=360;

  while( (abs(fxpos-xTarg)>1) || (abs(fypos-yTarg)>1)) {
    rest= sqrt(tx*tx+ty*ty);

    if ( (rest>3) || (speed<30) )  {  // ramp up
      if (speed<nspeed) speed+=5;
     }
    if (abs(MencHdg-GyroHdg)>4) MencHdg=GyroHdg;
    currHdg=MencHdg;

    tx=xTarg-fxpos;
    ty=yTarg-fypos;
    TargHdg=atan2(tx,ty)*(180/PI) ;
    while (TargHdg >=360) TargHdg-=360;
    while (TargHdg < 0)   TargHdg+=360;
   
    dHdg=TargHdg-currHdg;
    if (dHdg<-180) dHdg= 360-dHdg;
    if (dHdg>=180) dHdg-=360;
   

    if  (abs(dHdg>10)) {
      Off(Mboth); Wait(100);
      tx=xTarg-fxpos;
      ty=yTarg-fypos;
      TargHdg=atan2(tx,ty)*(180/PI) ;
      total=sqrt(tx*tx+ty*ty);
      MencHdg=GyroHdg;
      SpinUntilHdg(TargHdg, SGyro);
      MencHdg=GyroHdg;
    }
    else
    if (abs(dHdg>1) || (rest<30) || (rest>total-10))  {
      if ((dHdg)>0) {
        Coast(Mright);
        OnFwd(Mleft, speed);
      }
      if ((dHdg)<0) {
        Coast(Mleft);
        OnFwd(Mright, speed);
      }
    }
    else OnFwd(Mboth, speed);
   
    if (rest<5) {                                         // ramp down
      speed = rest*200/nspeed;

      if (speed>nspeed) speed=nspeed;
      if (speed<30) speed=30;
    }
    Wait(10);

  }

  Off (Mboth);
  Beep(100,100);  Wait(100);
  Coast(Mboth);
 
}


//--------------------------------------------------
// motor control
//--------------------------------------------------

float VwMleft, VwMright;  // deg per sec

task MotorSpeed() {
 float Mle, OMle, dVMle;
 float Mri, OMri, dVMri;

 while (1) {
     Mle=Menc(Mleft);
     Mri=Menc(Mright);
     dVMle=Mle-OMle;
     dVMri=Mri-OMri;
     OMle=Mle;
     OMri=Mri;
     VwMleft=dVMle*(100);
     VwMright=dVMri*(100);
     Wait(10);
  }
}

//------------------------------------------------------------------------------


task Navigator(){
  long itemp;
  float ftemp, dfG, dfC, dfE;
  float fbuf[3];
  long  encLeft, encRight, encLeftOld, encRightOld, dEncLeft, dEncRight;
  float fxold=0, fyold=0, Urw, Ulw, Um, dMencHdgRad, MencHdgRad;
 
  float GyroHdgRad, GyroHdgOld, dGyroHdg, dGyroHdgRad;
  float CompHdgRad, CompHdgOld, dCompHdg, dCompHdgRad;

  long startTComp, startTGyro, startTMenc, dTComp, dTGyro, dTMenc;

  encLeft =Menc(Mleft);
  encRight=Menc(Mright);

  //-------------------------------------------//

  CompHdg=SensorHTCompass(COMPASS);

  //-------------------------------------------//

  GyroHdg=MencHdg=CompHdg;

  startTComp=startTGyro=startTMenc=CurrentTick();

     //-------------------------------------------//

  while(1) {

     // https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf
     
     if (ButtonPressed(BTNCENTER, false)) {  // press BTNCENTER:
        SetGyroOffset();                    // recalibrate gyro offset
     }

     if (ButtonPressed(BTNLEFT, false)) {          // press BTNLEFT:
        MencHdg=GyroHdg=SensorHTCompass(COMPASS);  // reset Hdgs to compass
        while (btnhit()); Beep(1000,10);
     }

     if (ButtonPressed(BTNRIGHT, false)) {   // press  BTNRIGHT:
        fxpos=fypos=0;                       // reset x,y to 0
        while (btnhit()); Beep(1000,10);
     }

     CompHdgOld=CompHdg;
     startTComp = CurrentTick();

     CompHdg=SensorHTCompass(COMPASS);       //----------------------------//
     dTComp = CurrentTick() - startTComp;    // Compass heading            //
     CompHdgRad=CompHdg*PI/180;              //----------------------------//

     dCompHdg=CompHdg-CompHdgOld;
     if (dCompHdg<-180) dCompHdg= 360-dCompHdg;
     if (dCompHdg >180) dCompHdg-=360;
     dCompHdgRad=dCompHdg*PI/180;
     
     startTComp = CurrentTick();

     //-------------------------------------------//

     GyroHdgOld=GyroHdg;
     GyroValue=(SensorHTGyro(GYRO,0) - GyroOffset) ;    //
     dTGyro = CurrentTick() - startTGyro;
     GyroHdg+=GyroValue*dTGyro /1000.0;

     startTGyro = CurrentTick();

     while (GyroHdg>=360) GyroHdg-=360;      //----------------------------//
     while (GyroHdg<  0)  GyroHdg+=360;      // Gyro heading               //
     GyroHdgRad=GyroHdg*PI/180;              //----------------------------//
     
     dGyroHdg=GyroHdg-GyroHdgOld;
     if (dGyroHdg<-180) dGyroHdg= 360-dGyroHdg;
     if (dGyroHdg >180) dGyroHdg-=360;
     dGyroHdgRad=dGyroHdg*PI/180;

     //-------------------------------------------//
     
     encLeftOld =encLeft;                       // save old encoder values
     encRightOld=encRight;
     encLeft =Menc(Mleft);                      // get new encoder values
     encRight=Menc(Mright);

     dEncLeft=encLeft-encLeftOld;
     Ulw= WheelLegPerEncDegree*dEncLeft;       // left wheel leg way
     dEncRight=encRight-encRightOld;
     Urw= WheelLegPerEncDegree*dEncRight;      // right wheel leg way

     Um=(Urw+Ulw)/2;                                // intermedium leg way

     dMencHdgRad=atan2((Ulw - Urw),WheelGauge);     // delta heading !! GEO !!

     //-----------------------------------------------------------------------//

     if (abs(dCompHdgRad-dGyroHdgRad)<(PI/90))
        dMencHdgRad=(dGyroHdgRad+dCompHdgRad)/2;
     else {
       fbuf[0]=dCompHdgRad;
       fbuf[1]=dGyroHdgRad;
       fbuf[2]=dMencHdgRad;
       ftemp=ArrayMean(fbuf,NA, NA);
       dfC=abs(dCompHdgRad-ftemp);
       dfG=abs(dGyroHdgRad-ftemp);
       dfE=abs(dMencHdgRad-ftemp);
       if ((dfC<dfE) && (dfG<dfE)) {MencHdgRad=ArrayMedianF(fbuf, 3); }
     }
     //-----------------------------------------------------------------------//

     fxold=fxpos; fyold=fypos;                         // save old pos (x,y)

     MencHdgRad=MencHdg*PI/180;                        // Hdg in deg => rad

     fxpos=fxold + Um*(sin((MencHdgRad+(dMencHdgRad /2))));  // new xpos !! GEO !!
     fypos=fyold + Um* cos(-(MencHdgRad+(dMencHdgRad /2)));   // new ypos !! GEO !!

     MencHdgRad = MencHdgRad + dMencHdgRad;        // new motor enc Heading
     MencHdg=MencHdgRad*180/PI;
                                              //----------------------------//
     while (MencHdg >=360) MencHdg-=360;      // odometry heading           //
     while (MencHdg < 0)  MencHdg+=360;       //----------------------------//
     
     //-----------------------------------------------------------------------//
     fbuf[0]=CompHdg;
     fbuf[1]=GyroHdg;
     fbuf[2]=MencHdg;
     MencHdg=ArrayMedianF(fbuf,3);
     MencHdgRad=MencHdg*PI/180;
     //-----------------------------------------------------------------------//

     Wait(19);
  }
}

//------------------------------------------------------------------------------

task DisplayValues() {
  float fCompass;
  while (1) {
     fCompass=SensorHTCompass(COMPASS);
     
     printf1(0, 56, "mot_B%6d", Menc(Mleft));
     printf1(0, 48, "mot_C%6d", Menc(Mright));

     printf1(0, 40, "GValu %9.5f", GyroValue);
     printf1(0, 32, "GOffs %9.5f", GyroOffset);
     printf1(0, 24, "GyroHdg %8.3f", GyroHdg);
     printf1(0, 16, "MencHdg %8.3f", MencHdg);
     printf1(0,  8, "CompHdg %8.3f", fCompass);
     
     printf1(0,  0, "x %5.1f", fxpos); printf1(50, 0, "y %5.1f", fypos);

     Wait(20);
  }
}


//------------------------------------------------------------------------------
void InitGyro() {
    int OldHdg;
   
    OldHdg=SensorHTCompass(COMPASS);
    Wait(20);

    SpinUntilHdg(OldHdg-90, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    Wait(2000);

    SpinUntilHdg(OldHdg, SCompass);
    CompHdg=SensorHTCompass(COMPASS);
    SetGyroOffset();
}
//------------------------------------------------------------------------------

task main(){
  float fold;
  SetSensorHTGyro(GYRO);
  SetSensorLowspeed(COMPASS);

  start DisplayValues;
  start MotorSpeed;

  while(!btnhit());

  if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
  }
 
  start Navigator;

rep:
  Wait(1000);
  fold=CompHdg;

  MoveToPoint(150,100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(150,-100);
  Wait(2000);  MencHdg=GyroHdg;

  MoveToPoint(0,0);
  Wait(2000);  MencHdg=GyroHdg;

  SpinUntilHdg(fold, SCompass);
  Wait(2000);  MencHdg=GyroHdg=CompHdg;


  while(1) {
    if (ButtonPressed(BTNCENTER, false))  {
    while (!ButtonPressed(BTNCENTER, false));
    InitGyro();
    goto rep;
  }
  }
}



keywords: navigation, navigator, dead reckoning, odometry, gyro, Kompass, compass, accelerometer, sensor fusion, statistisch statistischer statistical statistic filtering, stochastisch stochastischer stochastic stochastical filter
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5058
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Re: NXT NXC: Navigation mit Odometrie, Gyro, Kompass

Beitragvon HaWe » 3. Dez 2015 15:12

Merkzettel: Arduino Navigator (in Entwicklung)

benutzt einen CMPS11 IMU-Sensor (3D Gyro+3DCompass+3DAcceleration) mit Kalman-Filter (per eigenem 16-bit Signal-Prozessor on-board) !

Code: Alles auswählen

//***********************************************************************************
//
// Map and Explore Navigator Robot
// ver 0009a
// Arduino IDE 1.6.5+
//
//************************************************************************************
//
// freie Verwendung für private Zwecke
// für kommerzielle Zwecke nur nach Genehmigung durch den Autor.
// programming language: Sketch C/C++ 1.6.5
// protected under the friendly Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License
// http: //creativecommons.org/licenses/by-nc-sa/3.0/ //

#define DEBUG        // debug mode

#include <SPI.h>
#include <PID_v1.h>
#include <Wire.h>
#include <Keypad.h>
#include <DueTimer.h>
#include <Scheduler.h>
#include <SD.h>
/*#include <SdFat.h>
SdFat SD;*/
#include <ardustdio.h>

//=====================================================================================
// device properties
//=====================================================================================
#define  ADDR_CMPS11  0x60                // Defines address of CMPS11



//=====================================================================================
// environment map and global navigator values
//=====================================================================================
#define  MAPSIZE  100
const    int8_t   sMAPSIZE = MAPSIZE + 1;           
volatile static int8_t   Map[sMAPSIZE][sMAPSIZE];

volatile static int16_t  xpos,  ypos;  // map field pos
volatile static double   fxpos, fypos; // environmental pos

volatile static double   IMUHdg, IMUHdgRad;


//=====================================================================================
// motors + sensor pin arrays
//=====================================================================================
#define  MAXMOTORS          4  // maximum number of encoder motors   
#define  MAXANALOG         16  // maximum number of analog sensors   
#define  MAXDIGPINS        54  // maximum number of digital pins   




//=====================================================================================
// misc.
//=====================================================================================
#define  timeMS       millis()                      // elapsed time since Arduino start
#define  LRAND_MAX    32767                         // limit for int PRN
#define  srand(seed)  randomSeed(seed)              // random 1st seed
#define  rand()       random(LRAND_MAX)             // q+d PRNG  int 0...LRAND_MAX
#define  rando()      ((float)rand()/(LRAND_MAX+1)) // random  float 0...<1

#define  valinrange(val,vmin,vmax)  ((val>=vmin)&&(val<=vmax))

// invalid pin setting or token
#define  VOID             127  // flag


//=====================================================================================

/*

 L              CCCCCCC     DDDDDDD
 L            CCC           D      DD
 L           CC             D        DD
 L          C               D          D
 L          C               D          D 
 L          C               D          D
 L           CC             D        DD 
 L            CCC           D      DD   
 LLLLLLLLLL     CCCCCCC     DDDDDDD         

*/



//=====================================================================================
// LCD TFT
//=====================================================================================

#define   _SmallFont_        1    // 9341 6x9
#define   _MediumFont_       2    // 9341 12x16
#define   _BigFont_          3    // 9341 18x23

int16_t  LCDmaxX, LCDmaxY ;       // display size
int16_t  _curx_, _cury_,          // last x,y cursor pos on TFT screen
         _maxx_, _maxy_;          // max. x,y cursor pos on TFT screen       



// set LCD TFT type
int16_t  LCDTYPE   =   -1;

#define  _LCD1602_     1  // LCD1602  Hitachi HD44780 driver <LiquidCrystal.h>
                          // http://www.arduino.cc/en/Tutorial/LiquidCrystal   //
#define  _ILI9341_     8  // https://github.com/adafruit/Adafruit_ILI9340/1
                          // https://github.com/adafruit/Adafruit-GFX-Library //
#define  _ILI9341due_  9  // ILI9341_due NEW lib by Marek Buriak
                          // http://marekburiak.github.io/ILI9341_due/ //
                           
//--------------------------------------------------------------------------------------------------
// TFT_CS      51
// TFT_DC      52
// SD_CS       53

#define    tft_cs     51
#define    tft_dc     52
#define    tft_rst     0                       
                         

//=====================================================================================
// TFT Adafruit ILI9340/ILI9341
//=====================================================================================
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9340.h>

Adafruit_ILI9340  tft = Adafruit_ILI9340(tft_cs, tft_dc, tft_rst);


//=====================================================================================
// TFT ILI9341_due // http://marekburiak.github.io/ILI9341_due/ //
//=====================================================================================
#include <ILI9341_due_config.h>
#include <ILI9341_due.h>
#include <SystemFont5x7.h>

ILI9341_due      dtft = ILI9341_due(tft_cs, tft_dc);

// Color set
#define  BLACK          0x0000
#define RED             0xF800
#define GREEN           0x07E0
//#define BLUE          0x001F
#define BLUE            0x102E
#define CYAN            0x07FF
#define MAGENTA         0xF81F
#define YELLOW          0xFFE0
#define ORANGE          0xFD20
#define GREENYELLOW     0xAFE5
#define DARKGREEN       0x03E0
#define WHITE           0xFFFF

uint16_t  color;

//--------------------------------------------------------------------------------------------------

#define  lcdWhiteBlack()  {                                                      \
   if (LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_WHITE, ILI9340_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, BLACK) ;}         \
}

#define  lcdNormal()      {                                                     \
   if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_WHITE, ILI9340_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, BLACK) ;}        \
}

#define  lcdInvers()      {                                                     \
   if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_BLACK, ILI9340_WHITE) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(BLACK, WHITE) ;}        \
}

#define  lcdWhiteRed()    {                                                   \
   if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_WHITE, ILI9340_RED) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(WHITE, RED) ;}        \
}

#define  lcdRedBlack()    {                                                   \   
   if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_RED, ILI9340_BLACK) ;} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(RED, BLACK) ;}        \
}

#define  lcdYellowBlue()  {                                                    \     
   if(LCDTYPE==_ILI9341_)   { tft.setTextColor(ILI9340_YELLOW, ILI9340_BLUE);} \
   else if(LCDTYPE==_ILI9341due_)   { dtft.setTextColor(YELLOW, BLUE);}        \
}

int16_t  fontwi= 8;  // default
int16_t  fonthi=10;  // default


void putfonttype(uint8_t fsize) {
  if(LCDTYPE==_ILI9341_) {
     if(fsize==_SmallFont_)     { fontwi= 6; fonthi=9; }  // 5x7 + overhead
     else
     if(fsize==_MediumFont_)    { fontwi=12; fonthi=16; } // ?
     else
     if(fsize==_BigFont_)       { fontwi=18; fonthi=23; } // ?
  }
  else
  if(LCDTYPE==_ILI9341due_) {
     if(fsize==_SmallFont_)     { fontwi= 6; fonthi=9; }  // 5x7 + overhead
  }
  _maxx_ = LCDmaxX / fontwi;    // max number of letters x>>
  _maxy_ = LCDmaxY / fonthi;    // max number of letters y^^
}


void setlcdorient(int16_t orient) {
 
  if(LCDTYPE==_ILI9341_) {
      tft.setRotation(orient);
      LCDmaxX=tft.width();
      LCDmaxY=tft.height();       
   }
   else
   if(LCDTYPE==_ILI9341due_) {
      dtft.setRotation((iliRotation)orient);
      LCDmaxX=dtft.width();
      LCDmaxY=dtft.height();   
   }     
}

void lcdcls()  {                                                         
   if(LCDTYPE==_ILI9341_)    { tft.fillScreen(ILI9340_BLACK);  }
   else
   if(LCDTYPE==_ILI9341due_) { dtft.fillScreen(BLACK);  }
   _curx_ =0;  _cury_ =0;
}

void curlf()   {                                                       
   _curx_=0;
   if( _cury_ <=(LCDmaxY-10) ) _cury_+=fonthi;
   else _cury_=0;
     
   if(LCDTYPE==_ILI9341_)    { tft.setCursor(0, _cury_); } 
   else
   if(LCDTYPE==_ILI9341due_) { dtft.cursorToXY(0, _cury_); } 
}


void curxy(int16_t  x,  int16_t  y) {
   _curx_ = x;
   _cury_ = y;
   if(LCDTYPE==_ILI9341_)      {tft.setCursor(x, y); }
   else
   if(LCDTYPE==_ILI9341due_)   {dtft.cursorToXY(x, y); }
}


void lcdprintxy(int16_t x, int16_t y, char * str) {
   if(LCDTYPE==_ILI9341_)  {
      tft.setCursor(x,y);     
      tft.print(str);
      _curx_=tft.getCursorX();
      _cury_=tft.getCursorY();
   }
   else if(LCDTYPE==_ILI9341due_)  {
      dtft.cursorToXY(x,y);     
      dtft.printAt(str, x, y);
      _curx_=x+strlen(str)*fontwi;
      _cury_=y;
   }
}


void lcdprint(char * str) {
    if(LCDTYPE==_ILI9341_)  {
       tft.setCursor(_curx_, _cury_);
       tft.print(str);
       _curx_=tft.getCursorX();
       _cury_=tft.getCursorY();
    }
    else if(LCDTYPE==_ILI9341due_)  {
       dtft.cursorToXY(_curx_, _cury_);
       dtft.printAt(str, _curx_, _cury_ );
      _curx_=_curx_+strlen(str)*fontwi;
    }
}


void initLCD(uint8_t orientation) { // 0,2==Portrait  1,3==Landscape
   if(LCDTYPE==_ILI9341_) {
      tft.begin();
      setlcdorient(orientation);       
      tft.setTextSize(_SmallFont_);
      putfonttype(_SmallFont_);
   }
   else
   if(LCDTYPE==_ILI9341due_) {
      dtft.begin();
      setlcdorient(orientation); 
      dtft.fillScreen(BLACK);
      dtft.setFont(SystemFont5x7);
      dtft.setTextColor(WHITE);
      dtft.print("ILI9341due");
      putfonttype(_SmallFont_);
   } 




//=====================================================================================

/*


    SSSSSSSSS     DDDDDDD
  SS              D      DD
 S                D        DD
  SS              D          D
    SSSSSSS       D          D     
           SS     D          D
             S    D        DD
           SS     D      DD
  SSSSSSSSS       DDDDDDD


*/



//*************************************************************************************

// TFT_CS      51
// TFT_DC      52
// SD_CS       53

File     SDfile;
#define  sd_cs    53   

char     _fname_[64];
int16_t  _fnameext_;


//*************************************************************************************

File fopen_(char * filename, const char * mode) {            // see ANSI C: fopen()
   int16_t  IOresult=0;
   File     file_ ;     // can't be initialized to NULL !
 
   if(mode=="w") {                                  // remove/rewrite
      IOresult = SD.remove(filename);               // success==TRUE, failed==FALSE
      file_    = SD.open(filename, FILE_WRITE); 
   } 
   else
   if(mode=="a") {                                  // append at EOF
      file_  = SD.open(filename, FILE_WRITE); 
   }
   else
   if(mode=="r") {
      file_  = SD.open(filename, FILE_READ);       // open at beginning of file
   }
   return file_; 
}


//*************************************************************************************

int16_t   fclose_(File SDfile) {                           // see ANSI C: fclose()
   SDfile.close();
   return fileIO_NO_ERR ;
}   

//*************************************************************************************

int16_t  remove_ (char * filename) {                       // see ANSI C: remove()
   int16_t  IOresult=0;

   if (SD.exists(filename) ) {       
       IOresult=SD.remove(filename);       
       // removed: success ?
       if (IOresult) return fileIO_NO_ERR;                 // SD file remove OK
       else  return fileIO_ERR_REMOVE;                     // SD file remove failed   
    }
    else    return fileIO_ERR_NAME;                        // SD file name not found 
}


//=====================================================================================
// SD file IO error
//=====================================================================================
/*
#define fileIO_OK            +1
#define fileIO_ERR_CREATE    -1
#define fileIO_ERR_OPEN      -2
#define fileIO_ERR_REMOVE    -3
#define fileIO_ERR_WRITE     -4
#define fileIO_ERR_READ      -5
#define fileIO_ERR_IMPLAUS   -6
#define fileIO_ERR_NAME      -8
#define fileIO_ERR_SDCARD   -16
*/

//=====================================================================================
// SD init
//=====================================================================================

int16_t initSD() {
   char sbuf[128];
   uint32_t  tstamp;
   int16_t   ior=0;
 
   tstamp = clock();
   ior = SD.begin(sd_cs);  // success==1==true; else 0==false
   while( !ior) {     
      sprintf(sbuf, "#: ...SD not found... "); 
      curlf(); lcdprint("#: ...SD not found... ");
      Serial.println(sbuf);
      delay(1000);   
      ior=SD.begin(sd_cs);
      if (clock()-tstamp>20000) {Serial.println("#: ...break!"); break; }
   }
  if(!ior) return fileIO_ERR_SDCARD;     // SD ioresult==0 => error = -16
  return fileIO_OK ;                     // SD ioresult==1 => ok = 1
}






//=====================================================================================

/*

  II               000000         
  II        /    0        0           
  II       /    0          0   
  II      /    0            0     SSSs       
  II     /     0            0    S   
  II    /      0            0    SS
  II   /        0          0       SS
  II  /          0        0          S
  II               000000        sSSS

*/


uint16_t   sensorADC[MAXANALOG];
int8_t     sensorIO[MAXDIGPINS];

//=====================================================================================
// read general digital pins and buttons
//=====================================================================================
#define  SensorPTouch(pin) (!digitalRead(pin))    // btn press for _PULLUP Touch Pin (intern. pullup resistor)
#define  SensorTouch(pin)  ( digitalRead(pin))    // btn press for _PULLDOWN Touch Pin (ext. pulldown resistor)
#define  pbtn(pin)         (!digitalRead(pin))    // alias (_PULLUP Touch Pin)


//=====================================================================================
// misc. GPIO pins
//=====================================================================================
// three exposed SS pins on Due: 4, 10, 52
// extra SPI_SS  4 (reserve)


// pins   0, 1       : Serial
// pins   2, 3       : ...
// SPI_SS 4          : (reserve)
// pins   5, 6       : ...
// pins   7,8,9,10   : motor pwm
// pins   11 - 13    : ...
// pins   22 - 37    : motor dir + encoder
// pins   38 - 46    : keypad pins
// pins   47 - 50    : bumper
// TFT_CS      51
// TFT_DC      52
// SD_CS       53


// bumper
#define  PIN_FROLE     47
#define  PIN_FRORI     48
#define  PIN_BAKLE     49
#define  PIN_BAKRI     50



//************************************************************************************
// KEYPAD
//************************************************************************************

const byte ROWS = 4; //four rows
const byte COLS = 5; //five columns
char keys[ROWS][COLS] = {
  {'L','7','4','1','F' },  // L=Left, F=F1
  {'0','8','5','2','G' },  // G=F2
  {'R','9','6','3','#'},   // R=Right
  {'E','X','D','U','*'}    // E=Enter, X=ESC, D=Down, U=Up,

};
byte rowPins[ROWS] = {38,39,40,41}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {42,43,44,45,46}; //connect to the column pinouts of the keypad

Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );


//************************************************************************************
// global pin and data setup
//************************************************************************************
/* system API pin constants
#define  INPUT   0
#define  OUTPUT  1
#define  LOW     0
#define  HIGH    1
*/

//************************************************************************************
// motor pins setup
//************************************************************************************

// setup for motor/data pins
// Datei twi.c im Arduino-Programmordner suchen und die beiden unteren Zeilen auskommentieren
// wie es hier steht, falls nicht schon geschehen:
// Code:
// file: twi.c
//           //activate internal pullups for twi.
//           //digitalWrite(SDA, 1);
//           //digitalWrite(SCL, 1);


//=====================================================================================   
// Sensor analog
//=====================================================================================   
// alias
#define SensorAnalogRaw(pin) analogRead(pin)

//-------------------------------------------------------------------------------------
// read analog ADC value as volts
#define SensorAnalogVolt(pin, refVolt) { (float)analogRead(pin*(float)refVolt/1023.0; }


//-------------------------------------------------------------------------------------
// Low Pass Filter
//-------------------------------------------------------------------------------------
float FilterLowpass(float val, float old, float eta)  {
   return(eta*val + (1.0-eta)*old);
}



 
//-------------------------------------------------------------------------------------   
// Sharp GP2D IR Distance Sensors
//-------------------------------------------------------------------------------------   
float  Sensord120v33(byte port) {    // GP2D120 range 4-30 cm 3.3V
     float tmp, val;
     
     if( valinrange(port,A0,A10) ) val = (float)analogRead(port); 
     val -= 5;
     if(val<1) val=1;
     if(val>1000) val=1000;
     val=4000/val;
     if(val>31) val = 0 ;      // out of range: 0 == no signal
     else
     if(val< 4) val = 4;
     return val;
}

//-------------------------------------------------------------------------------------   
float  Sensord12v33(byte port) {     // GP2D12 range 10-80 cm 3.3V, pin A0...A10
     float tmp, val;
     
     if( valinrange(port,A0,A10) ) val = (float)analogRead(port) ;
     val +=40;
     if(val<4) val=4;
     if(val>650) val=650;
     val=10000/(val);
     if(val>81) val= 0;        // out of range: 0 == no signal     
     else
     if(val<10) val =10;
     return val;
}

//-------------------------------------------------------------------------------------   
float  Sensora21v33(byte port) {     // 2Y0A21 range 10-80 cm 3.3V
     float tmp, val;
     
     if( valinrange(port,A0,A10) ) val = (float)analogRead(port) ;
     val -=0;
     if(val<4) val=4;
     if(val>750) val=750;
     val=6800/(val);
     if(val>81) val= 0;        // out of range: 0 == no signal
     else
     if(val<10) val =10;
     return val;                     
}
//-------------------------------------------------------------------------------------   






//=====================================================================================

/*


  MM         MM       000000     TTTTTTTTTTTT     000000       RRRRRRRRR       
  M M       M M      O      O         T          O      O      R        R       
  M  M     M  M     0        0        T         0        0     R         R       
  M   M   M   M    0          0       T        0          0    R        R     
  M    M M    M    0          0       T        0          0    RRRRRRRR       
  M     M     M    0          0       T        0          0    R      RR     
  M           M     0        0        T         0        0     R       R       
  M           M      0      0         T          O      O      R        R     
  M           M       000000          T           000000       R         R     


*/


//=====================================================================================
// motor control
//===================================================================================== 

// as wheel + chassis constants:
double WheelLegPerEncDegree,
       WheelGauge;

// motor 0
#define pinenc0A   22  // enc0A yellow
#define pinenc0B   23  // enc0B blue
#define pinmot0d1  24  // dir0-1   <<
#define pinmot0d2  25  // dir0-2
#define pinmot0pwm 10  // pwm enable0  <<  %timer 2

// motor 1
#define pinenc1A   26  // enc1A yellow
#define pinenc1B   27  // enc1B blue
#define pinmot1d1  28  // dir1-1   <<
#define pinmot1d2  29  // dir1-2
#define pinmot1pwm  9  // pwm enable1  <<  %timer 2


// motor 2
#define pinenc2A   30  // enc2A yellow
#define pinenc2B   31  // enc2B blue
#define pinmot2d1  32  // dir2-1   <<
#define pinmot2d2  33  // dir2-2
#define pinmot2pwm  8  // pwm enable2  <<  %timer 4

// motor 3
#define pinenc3A   34  // enc3A yellow
#define pinenc3B   35  // enc3B blue
#define pinmot3d1  36  // dir3-1   <<
#define pinmot3d2  37  // dir3-2
#define pinmot3pwm  7  // pwm enable3  <<  %timer 4



 
// SPI header                      |--|
// SPI pins Due         -----------|  |------------
//#define MISO     74   |  RES   76_SCK   74_MISO |
//#define MOSI     75   | -GND   75_MOSI   Vc+5V  |
//#define SCK      76   --------------------------

// intern LED pin
#define LEDPIN   13


int16_t   afifo[MAXANALOG][ 3];   // fifo buffer for analog values
int16_t   dfifo[MAXDIGPINS][ 3];  // fifo buffer for boolean (I/O) values


uint8_t pinmotdir[MAXMOTORS][ 2] =  // motor direction pin array
{
  {pinmot0d1, pinmot0d2},       
  {pinmot1d1, pinmot1d2},
  {pinmot2d1, pinmot2d2},
  {pinmot3d1, pinmot3d2},
};


int16_t  pinmotpwm[MAXMOTORS] =      {pinmot0pwm, pinmot1pwm, pinmot2pwm, pinmot3pwm  };

volatile int8_t ISRab[MAXMOTORS]     = {0, 0, 0, 0};
// 1/2 Auflösung/resolution
int8_t  schrittTab[16] = {0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 1, 0, 0, -1, 0};


#define OUT_REGSTATE_NUL            0
#define OUT_REGSTATE_COAST          1
#define OUT_REGSTATE_BRAKE          2
#define OUT_REGSTATE_EMERG_STOP     3
#define OUT_REGSTATE_ON             8
#define OUT_REGSTATE_RAMPUP         9
#define OUT_REGSTATE_RAMPDOWN      10

#define OUT_REGSTATE_PIDIDLE       15

#define OUT_REGSTATE_PIDACTIVE     16
#define OUT_REGSTATE_PIDRELATIVE   17
#define OUT_REGSTATE_PIDABSOLUTE   18
#define OUT_REGSTATE_PIDHOLD       19



// buffer for motor commands
uint8_t   _motorarray_[7];     
// 0: MOTORSLOT: MotNr
// 1: mot_runstate
// 2: pwm
// 3+4: mot enc_int16
// 5+6: buffer (encoder INT_32?)






//************************************************************************************
//************************************************************************************
//************************************************************************************
//************************************************************************************
//************************************************************************************
//******************************* TEST ***********************************************
//************************************************************************************
//************************************************************************************
//************************************************************************************
//************************************************************************************
//************************************************************************************
// global motor & PID variables
//************************************************************************************

float     motspeed[MAXMOTORS] ; 

int8_t    motpwm[MAXMOTORS],
          motrmp[MAXMOTORS];

int32_t   motenc[MAXMOTORS] ,
          oldenc[MAXMOTORS] ;

#define   MLEFT    0
#define   MRIGHT   1       

double    PIDsetpoint[MAXMOTORS],
          PIDinput[MAXMOTORS],
          PIDoutput[MAXMOTORS];
uint8_t   OUTregstate[MAXMOTORS];
float     PIDencgap[MAXMOTORS];

//------------------------------------------------------------------------------------
// PID tuning parameters
//------------------------------------------------------------------------------------

double aggrKp=  0.5  ,    aggrKi= 90  ,     aggrKd=  0.1   ;
double consKp=  0.4  ,    consKi= 80  ,     consKd=  0.1   ;


PID PIDs[] =
{ PID (&PIDinput[ 0], &PIDoutput[ 0], &PIDsetpoint[ 0],  aggrKp,   aggrKi,   aggrKd,    DIRECT) ,
  PID (&PIDinput[ 1], &PIDoutput[ 1], &PIDsetpoint[ 1],  consKp,   consKi,   consKd,    DIRECT) ,
 
  PID (&PIDinput[ 2], &PIDoutput[ 2], &PIDsetpoint[ 2],   2,      5,    1,    DIRECT) ,
  PID (&PIDinput[ 3], &PIDoutput[ 3], &PIDsetpoint[ 3],  20,     50,    1,    DIRECT)
};

#define  PID_REGTIME_MS       2 //5
#define  PID_REG_PREC         2 //2

#define  PID_REGMAX        255
#define  PID_REGMIN       -255

const float  PWM_SCALE_F = 2.55;  // pwm +/- 100 ==> +/-255





//************************************************************************************
// Encoder functions courtesy of / entnommen aus: http: //www.meinDUINO.de //
//************************************************************************************


//------------------------------------------------------------------------------------
// AVR Interrupt Service Routine (ISR): encoder readings at Interrupt
//------------------------------------------------------------------------------------

ISR(TIMER1_COMPA_vect) {  // read encoder values

  ISRab [ 0] <<= 2;
  ISRab [ 0] &= B00001100;
  ISRab [ 0] |= (digitalRead(pinenc0A) << 1) | digitalRead(pinenc0B);
  motenc[ 0] += schrittTab[ISRab[0]];           //

  ISRab [ 1] <<= 2;
  ISRab [ 1] &= B00001100;
  ISRab [ 1] |= (digitalRead(pinenc1A) << 1) | digitalRead(pinenc1B);
  motenc[ 1] += schrittTab[ISRab[1]];           //

  ISRab [ 2] <<= 2;
  ISRab [ 2] &= B00001100;
  ISRab [ 2] |= (digitalRead(pinenc2A) << 1) | digitalRead(pinenc2B);
  motenc[ 2] += schrittTab[ISRab[2]];           //

  ISRab [ 3] <<= 2;
  ISRab [ 3] &= B00001100;
  ISRab [ 3] |= (digitalRead(pinenc3A) << 1) | digitalRead(pinenc3B);
  motenc[ 3] += schrittTab[ISRab[3]];           //
}




//------------------------------------------------------------------------------------
// Due Timer Handler procedure
//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
void encHandler()
//------------------------------------------------------------------------------------
{
  ISRab [ 0] <<= 2;
  ISRab [ 0] &= B00001100;
  ISRab [ 0] |= (digitalRead(pinenc0A) << 1) | digitalRead(pinenc0B);
  motenc[ 0] += schrittTab[ISRab[0]];           //

  ISRab [ 1] <<= 2;
  ISRab [ 1] &= B00001100;
  ISRab [ 1] |= (digitalRead(pinenc1A) << 1) | digitalRead(pinenc1B);
  motenc[ 1] += schrittTab[ISRab[1]];           //

  ISRab [ 2] <<= 2;
  ISRab [ 2] &= B00001100;
  ISRab [ 2] |= (digitalRead(pinenc2A) << 1) | digitalRead(pinenc2B);
  motenc[ 2] += schrittTab[ISRab[2]];           //

  ISRab [ 3] <<= 2;
  ISRab [ 3] &= B00001100;
  ISRab [ 3] |= (digitalRead(pinenc3A) << 1) | digitalRead(pinenc3B);
  motenc[ 3] += schrittTab[ISRab[3]];           //
 
}




//************************************************************************************
// bit and byte and pin operations
//************************************************************************************

//------------------------------------------------------------------------------

inline int16_t  ByteArrayToInt16(uint8_t  array[], uint8_t  slot) {
  return ((array[slot + 1] << 8) + (array[slot]));
}

inline int32_t  ByteArrayToInt24(uint8_t  array[], uint8_t  slot) {
  return ((array[slot + 2] << 16) + (array[slot + 1] << 8) + (array[slot]));
}

//------------------------------------------------------------------------------

inline void Int16ToByteArray(int16_t  integer, uint8_t  *array, uint8_t  slot) {
  array[slot]   = (integer & 0xFF); // loByte
  array[slot + 1] = (integer >> 8); // hiByte
}

inline void Int24ToByteArray(int32_t  int24, uint8_t  *array, uint8_t  slot) {
  array[slot]   = (int24 & 0xFF);         // loByte 8
  array[slot + 1] = ((int24 >> 8) & 0xFF); // hiByte 16
  array[slot + 2] = ((int24 >> 16) & 0xFF); // hiByte 24
}

//------------------------------------------------------------------------------

/*
#define bitRead(source, bit) ( ((source) >> (bit)) & 0x01 )
#define bitSet (source, bit) ( (source) |= (1UL << (bit)) )
#define bitClear(source, bit) ( (source) &= ~(1UL << (bit)) )
#define bitWrite(source, bit, bitvalue) ( bitvalue ? bitSet(source, bit) : bitClear(source, bit) )
*/


//------------------------------------------------------------------------------------

int16_t  toggleup(int16_t  nr,  int16_t  max) {
  if ( nr < (max - 1) ) ++nr;
  else nr = 0;
  return nr;
}



//************************************************************************************
// array  &  fifo operations
//************************************************************************************

inline void bubblesort(int16_t  *array,  int16_t  length)
{
  int16_t  i, j;
  int16_t  tmp;

  for (i = 0; i < length - 1; ++i)     {
    for (j = 0; j < length - i - 1; ++j) {
      if (array[j] > array[j + 1])         {
        tmp = array[j];
        array[j] = array[j + 1];
        array[j + 1] = tmp;
      }
    }
  }
}




inline int16_t  mediana3(int16_t  nr)
{
  int16_t temp[3], i;

  memset ( temp, 0, 3 );
  for ( i = 0; i < 3; ++i)  temp[i] = afifo[nr][i];
  bubblesort( temp, 3 );
  return temp[1];
}



inline int16_t  mediand3(int16_t  nr)
{
  int16_t  temp[3], i;

  memset ( temp, 0, 3 );
  for ( i = 0; i < 3; ++i) temp[i] = dfifo[nr][i];
  bubblesort( temp, 3 );
  return temp[1];
}



// next version: void * memmove ( void * destination, const void * source, size_t num );
//--------------------------------------------

inline void afifopush(int16_t  nr,  int16_t  _new)
{
  afifo[nr][2] = afifo[nr][1];
  afifo[nr][1] = afifo[nr][0];
  afifo[nr][0] = _new;
}

//--------------------------------------------

inline void dfifopush(int16_t  nr,  uint16_t  _new)
{
  dfifo[nr][2] = dfifo[nr][1];
  dfifo[nr][1] = dfifo[nr][0];
  dfifo[nr][0] = _new;
}


//--------------------------------------------









//************************************************************************************
// motor control
//************************************************************************************

// no PID

#define    motoroff   motorcoast

//------------------------------------------------------------------------------------


inline void motoron(int16_t  motnr,  int16_t  power) { 
  if (power > 0) {
     digitalWrite( pinmotdir[motnr][0], HIGH);
     digitalWrite( pinmotdir[motnr][1], LOW);
  } 
  else 
  if (power < 0) {
    digitalWrite( pinmotdir[motnr][0], LOW );
    digitalWrite( pinmotdir[motnr][1], HIGH );
  } 
  else 
  if (power == 0) {
    digitalWrite( pinmotdir[motnr][0], LOW );
    digitalWrite( pinmotdir[motnr][1], LOW );
  } 
  power = abs(power);
  if (power > PID_REGMAX) power = PID_REGMAX; 
  motpwm[motnr] = power;
  motrmp[motnr] = power;     
  analogWrite( pinmotpwm[motnr], power);
}


//------------------------------------------------------------------------------------

inline void motorbrake(int16_t  motnr, int16_t  brakepwr ) {
  digitalWrite( pinmotdir[motnr][0], LOW );
  digitalWrite( pinmotdir[motnr][1], LOW );
  brakepwr = abs(brakepwr);
  motpwm[motnr] = brakepwr;
  motrmp[motnr] = brakepwr;
  analogWrite( pinmotpwm[motnr], brakepwr );   
}

//------------------------------------------------------------------------------------

inline void motorcoast(int16_t  motnr) {
  digitalWrite( pinmotdir[motnr][0], LOW );
  digitalWrite( pinmotdir[motnr][1], LOW );
  motpwm[motnr] = 0;
  motrmp[motnr] = 0;
  analogWrite( pinmotpwm[motnr], 0 ); 
}

//------------------------------------------------------------------------------------

void EmergencyStop() {
  for (int16_t motnr = 0; motnr < MAXMOTORS; ++motnr) {
    motorcoast(motnr);
    motpwm[motnr] = 0;
    motrmp[motnr] = 0;
    OUTregstate[motnr] = OUT_REGSTATE_COAST;
  }
}


//------------------------------------------------------------------------------------

// PID control

inline void rotatePIDdegrees (int16_t  motnr,  int32_t  angle) {
      PIDs[motnr].SetMode(AUTOMATIC);
      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
           PIDsetpoint[motnr] = angle + motenc[motnr];
           OUTregstate[motnr] = OUT_REGSTATE_PIDRELATIVE;
      }
}

//------------------------------------------------------------------------------------

inline void rotatePIDtoTarget (int16_t  motnr,  int32_t  angle) {
      PIDs[motnr].SetMode(AUTOMATIC);
      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE  ) {
           PIDsetpoint[motnr] = angle;
           OUTregstate[motnr] = OUT_REGSTATE_PIDABSOLUTE;
      }
}

//------------------------------------------------------------------------------------

inline void rotatePIDholdTarget (int16_t  motnr,  int32_t  angle) {
      PIDs[motnr].SetMode(AUTOMATIC);
      if ( OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE  ) {
           PIDsetpoint[motnr] = angle;
           OUTregstate[motnr] = OUT_REGSTATE_PIDHOLD;
      }
}

//------------------------------------------------------------------------------------

inline void stopPIDcontrol (int16_t  motnr) {
      OUTregstate[motnr] = OUT_REGSTATE_PIDIDLE;
      PIDs[motnr].SetMode(MANUAL);
      motorcoast(motnr);
}



//************************************************************************************
// PID regulation
//************************************************************************************

int16_t rotatePID(int16_t  motnr,  int32_t  angle, int16_t  regmode) {
   PIDs[motnr].SetMode(AUTOMATIC);
   if (OUTregstate[motnr] < OUT_REGSTATE_PIDACTIVE ) {
       PIDsetpoint[motnr] = angle;   
       OUTregstate[motnr] = regmode;
   }   
}

//------------------------------------------------------------------------------------


void  PIDupdate( uint8_t  nr)
{
  static int16_t oldgap[MAXMOTORS], oldpwr[MAXMOTORS];
  oldgap[nr]=PIDencgap[nr];
  oldpwr[nr]=abs(PIDoutput[nr]);
 
  if(OUTregstate[nr] > OUT_REGSTATE_PIDIDLE) {
    PIDinput[nr]  = motenc[nr];
    PIDencgap[nr] = abs( PIDsetpoint[nr] - PIDinput[nr] ); //distance away from setpoint
    if(nr==0) PIDs[nr].SetTunings(aggrKp,   aggrKi,   aggrKd );
    else
    if(nr==1) PIDs[nr].SetTunings(consKp,   consKi,   consKd );
     
    PIDs[nr].Compute();
 
    if ( PIDencgap[nr] > PID_REG_PREC                     // target gap failed
        &&   OUTregstate[nr] >= OUT_REGSTATE_PIDACTIVE )   // PID_ACTIVE
    {
       motoron(nr, PIDoutput[nr]);
    }

    if( (PIDencgap[nr] <= PID_REG_PREC)
     && (OUTregstate[nr] < OUT_REGSTATE_PIDHOLD)
     && (oldpwr[nr]>=abs(PIDoutput[nr])) )
    {
        PIDs[nr].SetTunings( 0,  0,  0);
        if( oldgap[nr]==PIDencgap[nr] ) {
           PIDs[nr].SetMode(MANUAL);
           OUTregstate[nr] = OUT_REGSTATE_PIDIDLE;
           motorcoast(nr);
           PIDoutput[nr]=0;           
        }
        PIDoutput[nr]=0;
    }
  }
  else
  if( (PIDencgap[nr] <= PID_REG_PREC)
     && abs((PIDoutput[nr]) < 10) )
  {
        PIDs[nr].SetTunings( 0,  0,  0);
        PIDs[nr].SetMode(MANUAL);
        OUTregstate[nr] = OUT_REGSTATE_PIDIDLE;
        motorcoast(nr);       
        PIDoutput[nr]=0;
  }
}





//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//================================== TEST =============================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================
//=====================================================================================



/*


    SSSSSSSSS    EEEEEEEEEEE  TTTTTTTTTTTTTT  U           U  PPPPPPPPP
  SS             E                  T         U           U  P        P
 S               E                  T         U           U  P         P
  SS             E                  T         U           U  P        P
    SSSSSSS      EEEEEEEE           T         U           U  PPPPPPPPP     
           SS    E                  T         U           U  P
             S   E                  T         U           U  P
           SS    E                  T           U       U    P
  SSSSSSSSS      EEEEEEEEEEE        T            UUUUUUU     P


*/




//=====================================================================================
// SETUP ()
//=====================================================================================

void setup()
{
  int32_t i;
  char    sbuf[128];

   // TFT_CS      51
   // TFT_DC      52
   // SD_CS       53
   //------------------------------------------------------------------------------------
   // TFT LCD
      Serial.println();
      LCDTYPE = _ILI9341_;
      Serial.print("init LCD... \n");     
      initLCD(1);   
      lcdcls();
      sprintf(sbuf, "LCD=%d wi%dxhi%d Font %dx%d \n",LCDTYPE,LCDmaxX,LCDmaxY,fontwi,fonthi);
      Serial.println(sbuf);
      Serial.println();
      lcdcls(); lcdprint(sbuf);
      Serial.println("[done.] \n");

   //------------------------------------------------------------------------------------
   // Serial terminal window
      i=115200;
      Serial.begin(i); 
      sprintf(sbuf, "Serial started, baud=%ld \n");
      Serial.println(sbuf); 
      lcdprint(sbuf);     
      Serial.println("[done.] \n");

   //------------------------------------------------------------------------------------
   // init i2c devices
      sprintf(sbuf, "i2c init \n");
      lcdprintxy( 0, 0, sbuf); 
      Wire.begin();       
      // i2c CMPS11: addr=0x60
      sprintf(sbuf, "CMPS11 Example V: %d \n", IMU_soft_ver()); // software version of the CMPS11
      Serial.println(sbuf);
      lcdprint(sbuf);
      Serial.println("[done.] \n");

 
   //------------------------------------------------------------------------------------
   // SD card
      sprintf(sbuf, "SD init... ");   Serial.println(sbuf);
      i = initSD();
      if(i==fileIO_ERR_SDCARD) sprintf(sbuf, "SD failed! ERROR ! "); 
      else sprintf(sbuf, "SD OK ! ");   
      Serial.println(sbuf);   
      lcdprint(sbuf);
      Serial.println("[done.] \n");
 
 
   //------------------------------------------------------------------------------------
   // motor settings
   // setup for L293D motor driver

      for(i=0; i<MAXMOTORS; ++i) {motenc[i]=0; oldenc[i]=0; }
     
      // motor 0
      pinMode(pinenc0A, INPUT_PULLUP);  // enc0A    yellow
      pinMode(pinenc0B, INPUT_PULLUP);  // enc0B    blue
      pinMode(pinmot0d1, OUTPUT);        // dir0-1   
      pinMode(pinmot0d2, OUTPUT);        // dir0-2   
      pinMode(pinmot0pwm ,OUTPUT);       // enable0 
       
      // motor 1
      pinMode(pinenc1A, INPUT_PULLUP);  // enc1A    yellow
      pinMode(pinenc1B, INPUT_PULLUP);  // enc1B    blue
      pinMode(pinmot1d1, OUTPUT);        // dir1-1   
      pinMode(pinmot1d2, OUTPUT);        // dir1-2 
      pinMode(pinmot1pwm, OUTPUT);       // enable1
       
      // motor 2
      pinMode(pinenc2A, INPUT_PULLUP);  // enc2A    yellow
      pinMode(pinenc2B, INPUT_PULLUP);  // enc2B    blue
      pinMode(pinmot2d1, OUTPUT);        // dir2-1 
      pinMode(pinmot2d2, OUTPUT);        // dir2-2   
      pinMode(pinmot2pwm, OUTPUT);       // enable2 
       
      // motor 3
      pinMode(pinenc3A, INPUT_PULLUP);  // enc3A     yellow
      pinMode(pinenc3B, INPUT_PULLUP);  // enc3B     blue
      pinMode(pinmot3d1, OUTPUT);        // dir3-1   
      pinMode(pinmot3d2, OUTPUT);        // dir3-2 
      pinMode(pinmot3pwm, OUTPUT);       // enable3 

      sprintf(sbuf, "GPIOs motor 0: enc=%2d %2d di=r%2d %2d pwm=%2d \n", pinenc0A,pinenc0B,pinmot0d1,pinmot0d2,pinmot0pwm);
      Serial.println(sbuf);   
      lcdprint(sbuf);
      sprintf(sbuf, "GPIOs motor 0: enc=%2d %2d dir=%2d %2d pwm=%2d \n", pinenc1A,pinenc1B,pinmot1d1,pinmot1d2,pinmot1pwm);
      Serial.println(sbuf);   
      lcdprint(sbuf);
      sprintf(sbuf, "GPIOs motor 0: enc=%2d %2d dir=%2d %2d pwm=%2d \n", pinenc2A,pinenc2B,pinmot2d1,pinmot2d2,pinmot2pwm);
      Serial.println(sbuf);   
      lcdprint(sbuf);
      sprintf(sbuf, "GPIOs motor 0: enc=%2d %2d dir=%2d %2d pwm=%2d \n", pinenc3A,pinenc3B,pinmot3d1,pinmot3d2,pinmot3pwm);
      Serial.println(sbuf);   
      lcdprint(sbuf);
     
      sprintf(sbuf, "\nGPIOs OK ! \n");   
      Serial.println(sbuf);   
      lcdprint(sbuf);
      Serial.println("[done.] \n");


   //------------------------------------------------------------------------------------
   // attach Due Timer

      i=100;   // 0.1 ms
      Timer3.attachInterrupt(encHandler).start(i);
      sprintf(sbuf, "motor encoder Timer IRQ started: %f ms \n", (float)i/1000.0);
      Serial.println(sbuf);
      lcdprint(sbuf);
      delay(50);
     
      Serial.println("[done.] \n");
 
  //------------------------------------------------------------------------------------
  // attach Scheduler tasks
     Scheduler.startLoop(Task_PollSensors_Slow);
     sprintf(sbuf, "Scheduler task 1: Task_PollSensors_Slow started \n");
     Serial.println(sbuf);
     lcdprint(sbuf);

     Scheduler.startLoop(Task_IMU);
     delay(100);                  // wait for IMU sensor fusion to initialize
     sprintf(sbuf, "Scheduler task 2: Task_IMU started \n");
     Serial.println(sbuf);
     lcdprint(sbuf); 
       
     Scheduler.startLoop(Task_Navigator);
     sprintf(sbuf, "Scheduler task 3: Task_Navigator started \n");
     Serial.println(sbuf);
     lcdprint(sbuf); 
     
     
     Serial.println("[done.] \n");
 
  //------------------------------------------------------------------------------------

  // reset PID parameters
  for (int16_t i = 0; i < MAXMOTORS; ++i) {
     PIDs[ i].SetMode(AUTOMATIC);
     PIDs[ i].SetOutputLimits(PID_REGMIN, PID_REGMAX);
     PIDs[ i].SetSampleTime(PID_REGTIME_MS);
     PIDsetpoint[ i] = 0;                      // set target,
     OUTregstate[ i] = OUT_REGSTATE_NUL;       // switch the PID on to motor[i]
     motoroff(i);
  }

     sprintf(sbuf, "PID parameters reset \n");
     Serial.println(sbuf);
     lcdprint(sbuf); 
   
     Serial.println("[done.] \n");
     
  //------------------------------------------------------------------------------------ 
 
  sprintf(sbuf, "all systems running! \n");
  Serial.println(sbuf);   
  lcdprint(sbuf);
 
  sprintf(sbuf, "setup(): [done.] \n\n");
  Serial.println(sbuf);   
  lcdprint(sbuf);
   
}







//=====================================================================================

/*


 L               OOOOOO          OOOOOO      PPPPPPPPP
 L              O      O        O      O     P        P       
 L             O        O      O        O    P         P     
 L            O          O    O          O   P        P     
 L            O          O    O          O   PPPPPPPPP     
 L            O          O    O          O   P           
 L             O        O      O        O    P           
 L              O      O        O      O     P           
 LLLLLLLLLL      OOOOOO          OOOOOO      P         


*/


//=====================================================================================
// TASK LOOP 0: poll current inputs, compute outputs, user interface (train,edit,debug,SD)
//=====================================================================================

#define   MAXMAINCOUNTER   12   // main loop counter 1-12 for 1/12 task priority prescaler
int16_t   __mainloopcnt__ = 0;  // counter for priority prescaler in main loop

//=====================================================================================
void loop()
//=====================================================================================
{
  char strbuf[50];
  char key;
 
  key = keypad.getKey();

  //***** start motor control *****
  if (key != NO_KEY){   
    Serial.println(key);   
     
    if        (key=='U') {rotatePIDtoTarget( 0, 360); }
    else   if (key=='D') {rotatePIDtoTarget( 0,-360); }
    else   if (key=='*') {stopPIDcontrol(0); }

    if        (key=='R') {rotatePIDtoTarget( 1, 360); }
    else   if (key=='L') {rotatePIDtoTarget( 1,-360); }
    else   if (key=='0') {stopPIDcontrol(1); }   
  }

 
  // motor and PID handling
  for (int16_t i = 0; i < MAXMOTORS ; ++i) PIDupdate( i); // regulate motors

  //***** end motor control *****//


  //***** start debug output ***** 
  if ( !(__mainloopcnt__ %2) )  {
      sprintf(strbuf, "tgt=%5d  enc=%5d  pwr=%6d  rstate=%d",
      (int)PIDsetpoint[ 0], (int)motenc[ 0], (int)PIDoutput[ 0], OUTregstate[ 0] );
      Serial.print(strbuf); 
      Serial.print("     ");
     
      sprintf(strbuf, "tgt=%5d  enc=%5d  pwr=%6d  rstate=%d",
      (int)PIDsetpoint[ 1], (int)motenc[ 1], (int)PIDoutput[ 1], OUTregstate[ 1] );
      Serial.print(strbuf);  Serial.println();
  }

 
  toggleup( __mainloopcnt__, MAXMAINCOUNTER);       // priority = 1/12

 
  delay(1);
  yield();
}




//=====================================================================================
// Loop 1: Task_PollSensors: task for IR sensors and motor speed
//=====================================================================================
//=====================================================================================
void Task_PollSensors_Slow()
//=====================================================================================
{
   char sbuf[100];
   int16_t  i, a, idx;   
   float    fval;
   static   uint32_t  tstamp=millis();
   static   float     dt=(float)millis();   
     
   yield();
   //------------------------------------------------------------------------------------
   // motor speed
   dt = (float)(millis()-tstamp);
   if(dt!=0) {
      for(i=0; i<MAXMOTORS; ++i)  { motspeed[i] = (100.0 * (float)(motenc[i]-oldenc[i]) )/dt  ; }
   }
   else for(i=0; i<MAXMOTORS; ++i)   motspeed[i] = 0 ;
   for(i=0; i<MAXMOTORS; ++i) oldenc[i] = motenc[i]; 

   //------------------------------------------------------------------------------------
   // analog IR sensors
   for(a=0; a<4; ++a) sensorADC[a] = Sensord12v33(a+A0);    // A0-A3:   4* 10-80 GP2D12
   for(a=4; a<6; ++a) sensorADC[a] = Sensora21v33(a+A0);    // A4-A5:   2* 10-80 GPA21
   for(a=6; a<8; ++a) sensorADC[a] = Sensord120v33(a+A0);   // A6-A7:   2* 4-30  GP2D120
   for(a=8; a < 11; ++a) sensorADC[a] = analogRead(a+A0);   // A8-A11:  ADC
   for(a=12; a < MAXANALOG; ++a) sensorADC[a] = 0;          // A11-A15: dummy


   //------------------------------------------------------------------------------------
   // digital sensors
   // pins   0, 1       : Serial
   // pins   2, 3       : ...
   // SPI_SS 4          : (reserve)
   // pins   5, 6       : ...
   // pins   7,8,9,10   : motor pwm
   // pins   11 - 13    : ...
   // pins   22 - 37    : motor dir + encoder
   // pins   38 - 46    : keypad pins
   // pins   47 - 50    : bumper
   // TFT_CS      51
   // TFT_DC      52
   // SD_CS       53
   
   sensorIO[0] = 0;                                    // 0 = dummy (muxer)
   sensorIO[1] = 0;                                    // 1 = dummy (muxer)
   for(i= 2; i< 7; ++i) sensorIO[i] = digitalRead(i);  // read digital pins 2-6
   for(i= 7; i<11; ++i) sensorIO[i] = 0;               // skip pwm pins: dummy (muxer)
   for(i=11; i<14; ++i) sensorIO[i] = digitalRead(i);  // read digital pins 8-13
   for(i=14; i<22; ++i) sensorIO[i] = 0;               // skip port pins: dummy (muxer)
   for(i=22; i<38; ++i) sensorIO[i] = 0;               // skip motor ctrl pins: dummy (muxer)
   for(i=38; i<47; ++i) sensorIO[i] = 0;               // skip keypad pins: dummy (muxer)
   for(i=47; i<51; ++i) sensorIO[i] = digitalRead(i);  // read bumper pins
   for(i=51; i<54; ++i) sensorIO[i] = 0;               // skip cs-pins: dummy (muxer)


   
   

   //------------------------------------------------------------------------------------
   delay(48);   
   tstamp=millis();
}




//=====================================================================================
// Loop 2: Task_IMU:  Gyro+Compass+Accelerometer reading
//=====================================================================================

int IMU_soft_ver(){
   int data;                                      // Software version of  CMPS11 is read into data and then returned
 
   Wire.beginTransmission(ADDR_CMPS11);
   // Values of 0 being sent with write masked as a byte to be not misinterpreted as NULL
   // (bug in arduino 1.0)
   Wire.write((byte)0);                           // Sends the register we wish to start reading from
   Wire.endTransmission();

   Wire.requestFrom(ADDR_CMPS11, 1);              // Request byte from CMPS11
   while(Wire.available() < 1);
   data = Wire.read();         
 
   return(data);
}

//=====================================================================================
void display_data(float b,  int p, int r){    // pitch and roll (p, r) are recieved as ints (signed values)
  char sbuf[128];         
 
  sprintf(sbuf, "heading =  %6.1f", b);
  lcdprintxy( 0,20, sbuf);

  sprintf(sbuf, "Pitch =  %6d", p);
  lcdprintxy( 0,30, sbuf);

  sprintf(sbuf, "Roll  =  %6d", r);
  lcdprintxy( 0,40, sbuf);
}

//=====================================================================================
void Task_IMU()
//=====================================================================================
{
   uint8_t  HdgHibyte, HdgLobyte;        // HdgHibyte and HdgLobyte: high + low bytes of heading
   int8_t   pitch, roll;                 // pitch and roll values of CMPS11 (signed char value)
   double   fheading;                    // full heading (float): local!
   
   char     sbuf[128];
 
   Wire.beginTransmission(ADDR_CMPS11);  //starts communication with CMPS11
   Wire.write(2);                        //Sends the register we wish to start reading from
   Wire.endTransmission();
 
   Wire.requestFrom(ADDR_CMPS11, 4);     // Request 4 bytes from CMPS11
   while(Wire.available() < 4);          // Wait for bytes to become available
   HdgHibyte = Wire.read();         
   HdgLobyte = Wire.read();           
   pitch     = Wire.read();             
   roll      = Wire.read(); 
   fheading  = ( (double)(HdgHibyte<<8) + (double)HdgLobyte )/10.0;  // heading (float)
   
   IMUHdg    = fheading;                 // assign global IMU values
   IMUHdgRad = IMUHdg * PI/180.0;

   yield();
   display_data(fheading, pitch, roll);  // Display data to the LCD  <<< to do: extra display task

   
   delay(10);   
   //...
   yield();

}




//=====================================================================================
// Loop 3: Task_Navigator: Odometry + Gyro + Compass sensor fusioning
//=====================================================================================
//=====================================================================================
void Task_Navigator()
//=====================================================================================
{
     volatile static int32_t encLeft, encRight,
                             encLeftOld, encRightOld,
                             dEncLeft, dEncRight;
     volatile static double  wroll, wlroll, wrroll,
                             OdomHdg, OdomHdgRad, OdomHdgRadOld, dOdomHdgRad;
     volatile static double  fxold, fyold;

  // odometry
     encLeftOld  = encLeft;                      // save old encoder values
     encRightOld = encRight;
     encLeft     = motenc[MLEFT];                // get new encoder values
     encRight    = motenc[MRIGHT];

     dEncLeft    = encLeft-encLeftOld;
     wlroll      = WheelLegPerEncDegree * (float)dEncLeft;   // left wheel leg way
     dEncRight   = encRight-encRightOld;
     wrroll      = WheelLegPerEncDegree * (float)dEncRight;  // right wheel leg way

     wroll=(wrroll+wlroll)/2;                                // intermedium leg way

     // don't use motor enc odometry any more for heading!
     // dOdomHdgRad=atan2( (wlroll-wrroll), WheelGauge );       // delta heading by odometry !! GEO !!


  // new heading by IMU (3DGyro + 3DCompass + 3DAcc)
     OdomHdgRadOld = OdomHdgRad;                          // save old odometry heading radians
     
  // refresh odometry headings
     OdomHdg       = IMUHdg;                              // get new odometry heading degrees from IMU degrees
     OdomHdgRad    = IMUHdgRad;                           // get new odometry heading radians from IMU radians       
     dOdomHdgRad   = OdomHdgRad - OdomHdgRadOld;          // change of heading since last measurement   
     while (dOdomHdgRad > 180.0) dOdomHdgRad -= 180.0; // 359° <-> 0° transition
     while (dOdomHdgRad <-180.0) dOdomHdgRad += 180.0; //
     


  // new coordinates position
     fxold=fxpos; fyold=fypos;                         // save old pos (x,y)

     fxpos=fxold + wroll*(sin( (OdomHdgRad+(dOdomHdgRad /2))));   // new x-pos !! GEO !!
     fypos=fyold + wroll* cos(-(OdomHdgRad+(dOdomHdgRad /2)));    // new y-pos !! GEO !!
 
     

   
  delay(4);
  //...
  yield();
 
}



//=====================================================================================
//=====================================================================================
// Move Commands + semaphores
//=====================================================================================
//=====================================================================================

int16_t MotorCommand;


#define CommNone  -1       // undefined

#define mvstop      0       // all motors stopp==float
#define mvbrake  -100       // all motors brake     

#define mvfwd     1         // both motors forward
#define mvrev     2         // both motors reverse

#define mvspinle  3         // motors contrarotating left == counter-clockwise
#define mvspinri  4         // motors contrarotating right == clockwise

#define mvturnle  5         // motors left slow/stop, right forw
#define mvturnri  6         // motors right slow/stop, left forw

#define mvturnrevle   7     // motors left turns rev, right slow/stop
#define mvturnrevri   8     // motors right turns rev, left slow/stop

#define mvremotectrl 20     // manual remote control


//=====================================================================================
// subsumption semaphores
//=====================================================================================

int8_t  flag_runCruisetask=1;
int8_t  flag_runAvoidtask=1;
int8_t  flag_runAvoidbacktask=1;
int8_t  flag_runRemctrltask=1;
int8_t  flag_runalltasks=1;        // only stoppable by task Emergencystopp
int8_t  flag_runEmergstopptask=1;  // never stopp!

int16_t semCruise;
int16_t semAvoid;
int16_t semAvoidback;
int16_t semRemctrl;
int16_t semEmergstopp; 
 


//=====================================================================================
void  Task_Arbitrate ()           // supersede low priority by higher tasks
//=====================================================================================
{         
    // behaviour priorities: set semaphores   
    while(true) {
       if (semCruise     != CommNone)  MotorCommand = semCruise;      // lowest Pr.
       if (semAvoid      != CommNone)  MotorCommand = semAvoid;
       if (semRemctrl    != CommNone)  MotorCommand = semRemctrl;
       if (semEmergstopp != CommNone)  MotorCommand = semEmergstopp;  // highest Pr.

       MotorControl ();     //  semaphores activate loops in related tasks
                            //  and deactivates other task loops
   }                       
}


//=====================================================================================
void  InitCommNone ()             // set all behaviour semaphores = undefined
//=====================================================================================
{
   semCruise=CommNone;
   semAvoid=CommNone;
   semAvoidback=CommNone;
   semRemctrl=CommNone;
   semEmergstopp=CommNone;
}


//=====================================================================================
void MotorControl ()              //   assign motor + std move patterns
//=====================================================================================
{
   if      (MotorCommand == mvfwd)        {  }
   else if (MotorCommand == mvrev)        {  }
   else if (MotorCommand == mvspinle)     {  }
   else if (MotorCommand == mvspinri)     {  }
   else if (MotorCommand == mvturnle)     {  }
   else if (MotorCommand == mvturnri)     {  }
   else if (MotorCommand == mvstop)       {  }
   else if (MotorCommand == mvturnrevri)  {  }
   else if (MotorCommand == mvturnrevri)  {  }
   else if (MotorCommand == mvremotectrl) {  }
}






////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////




Dateianhänge
robonavigatorchassis.jpg
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


Zurück zu „Projekte, Showcase, Bauanleitungen“

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 13 Gäste

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss