Two wheeled self balancing robot (redesign)

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/

#ifndef  Vehicle_h
#define  Vehicle_h
#include "Motor.h"
#include "Arduino.h"
#include "Config.h"
/**********************************************************************/
class Vehicle
/**********************************************************************/
{
  public:
    Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,
            PidControl * PidControler, PidControl * PidControlerPos, int iPinSleepSwitch);  // Constructor

    Motor *pMotorA;
    Motor *pMotorB;
    PidControl *pPidControler;
    PidControl *pPidControlerPos;

    void Run(MpuYawPitchRoll YawPitchRoll ,  int &PositionA, int &PositionB, JStickData JStick);
    void init();

    void Stop( );

    float DeltaPos       = 0.0;
    float DeltaForward   = 0.0;
    float DeltaTurning   = 0.0;
    float PositionAB     = 0.0;
    float HoldPosition   = 0.0;
    float StepsA         = 0.0;
    float StepsB         = 0.0;
    float CalJstickX     = 0;
    float CalJstickY     = 0;
    bool  spinning       = false;
    bool  spinningOld    = false ;
    bool  moving         = false;
    int   skipPos; // wait befor starting position controll
    int freqA;
    int freqB;


  protected:
    DuePWMmod *ptrpwm;
    int PinSleepSwitch;
    bool firstRun = true;

};
#endif

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
  The robot is controlled by PID controllers.
  The PID controllers ensures that the robot remains upright.
  The PID controllers are implemented in the "PIDControl" class.
  The standard PID algorithm was slightly modified after longer test series. To the P-component the parameter Kx multiplied
  by the I-component was added.
*/
#include "Vehicle.h"

/**********************************************************************/
Vehicle::Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,  // Constructor
                 PidControl * PidControler, PidControl * PidControlerPos, int iPinSleepSwitch)
/**********************************************************************/
{
  PinSleepSwitch = iPinSleepSwitch;
  pinMode(PinSleepSwitch, INPUT_PULLUP);
  pMotorA            = MotorA;
  pMotorB            = MotorB;
  pPidControler      = PidControler;
  pPidControlerPos   = PidControlerPos;
  ptrpwm             = ipwm;
  firstRun           = true;
}

/**********************************************************************/
void Vehicle::Run(MpuYawPitchRoll YawPitchRoll , int &iPositionA, int &iPositionB, JStickData JStick)
/**********************************************************************/
{ /*
    Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev => 1.8 degree per step
    Wheel Diameter 88mm = > Distance per Pulse Dpp = d * pi / 200 =  1,3816 mm
    Distance per Revolution =  276,32 mm
    Max 1000 Steps per Second = 5 Revolutions => 13816 mm Distance

    "iPositionA" in  microsteps
    8 microsteps = 1 full Step
    1 microstepp = 0,125 full steps
    after division one change in "PositionA" = 0.01 microstepp and 0,0125 full steps = 0,01727 mm
  */
  const int     tDelay = 10;

  PositionAB = ((float(iPositionA ) + float(iPositionB )) / 100);
/*
  Serial.print(iPositionA );  //grau
  Serial.print(",");
  Serial.print(iPositionB );  //grau
  Serial.print(",");
  Serial.print(HoldPosition );  //grau
  Serial.println(" ");
*/
  /********************* driverless **************************************/
  if (firstRun) {
    firstRun = false;
    skipPos = - (2 * tDelay); // first time wait a little bit longer
    CalJstickX = JStick.Xvalue;  // calibrate Joy Stick
    CalJstickY = JStick.Yvalue;
    spinningOld = false;
    pMotorA->RunMode();
    pMotorB->RunMode();
    HoldPosition = PositionAB;
  }
  JStick.Xvalue = JStick.Xvalue - CalJstickX ;
  JStick.Yvalue = JStick.Yvalue - CalJstickY;

  DeltaForward = float(JStick.Yvalue) / 100.0 ;
  DeltaTurning = float(JStick.Xvalue ) / 4.0;

  if ((abs(JStick.Xvalue) > 5 ) || (abs(JStick.Yvalue) > 5 ) )  {
    spinning = true;
    if (!spinningOld) {  // changing to spinning
    }
    spinningOld = true;
    HoldPosition = PositionAB;
  } else {
    spinning = false;
    if (spinningOld) {  // changing to not spinning
      skipPos = - (2 * tDelay); // wait a little bit longer
    }
    spinningOld = false;
  }

  if (!spinning) {
    if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
      skipPos = 0;
      // PID calculation Delta Position
      // DeltaPos is necessary for the robot to keep its position clean after moving forward or backward
      DeltaPos =  pPidControlerPos->calculate(HoldPosition, PositionAB);
      // slowly adjust the hold position to the current position
      // so, DeltaPos is slowly reduced and the pPidControler
      // works again without offset
      HoldPosition = (0.9 * HoldPosition) + ( 0.1 * PositionAB);
    }
  }

  // PID calculation of new steps
  StepsA = pPidControler->calculate(YawPitchRoll.pitch, -DeltaForward + DeltaPos );
  StepsB = StepsA;

  StepsA =  StepsA  + DeltaTurning;  // Moving right or left
  StepsB =  StepsB  - DeltaTurning;

  StepsA = pMotorA->Run(StepsA);  // run the motors
  StepsB = pMotorB->Run(StepsB);

  uint32_t freqA_abs = abs(StepsA); // only positive Steps
  uint32_t freqB_abs = abs(StepsB);  // direction via PinDir
  ptrpwm->setFreq2( freqA_abs, freqB_abs  );

}

/**********************************************************************/
void Vehicle::init()
/**********************************************************************/
{
  Serial.println("Vehicle Init Motor Pointer....");
  int ptr1 = (int) pMotorA;
  int ptr2 = (int) pMotorB;
  Serial.println(ptr1 , HEX);
  Serial.println(ptr2 , HEX);

  Serial.println("Vehicle Init PID Pointer....");
  int ptr3 = (int) pPidControler;
  int ptr4 = (int) pPidControlerPos;
  Serial.println(ptr3 , HEX);
  Serial.println(ptr4 , HEX);

  pMotorA->init( );
  pMotorB->init();

  pMotorA->MsMicrostep();  // set microstepping
  pMotorB->MsMicrostep();

  pMotorA->SleepMode();
  pMotorB->SleepMode();

}
/**********************************************************************/
void Vehicle::Stop() {
  pMotorA->SleepMode( );
  pMotorB->SleepMode( );
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
PID auto Tuning with Twiddle

    Twiddle is an algorithm that tries to find a good choice of parameters for an algorithm. 
    Also known as Hill climbing, it is an analogy to a mountaineer who looks for the summit in dense 
    fog and steers his steps as steeply uphill as possible. If it only goes down in all directions, 
    he has arrived at a summit.The Twiddle algorithm is used for auto tuning of PID parameter. 
    First of all, parameters can be tested with a manual tuning with a potentiometer.
*/
#ifndef Twiddle_h
#define  Twiddle_h
#include "Arduino.h"
//********************************************************************** /
class Twiddle
///**********************************************************************/
{
  public:
    Twiddle( int anzParams, float p0 , float p1, float p2, float p3, float p4 , float p5, float p6, float p7,
             float dp0, float dp1, float dp2, float dp3 , float dp4, float dp5, float dp6 , float dp7)  ; // Constructor
    Twiddle(int anzParams, float iparams[], float idparams[]  )  ; // Constructor

    float next(float error, float &p0, float &p1, float &p2, float &p3, float &p4, float &p5, float &p6, float &p7);
    void calcCost(float average);
    void logging();
    float params[8];
    float dparams[8];
    float besterr;
    float lastcost;
    float average;
    int   index ;
    int   AnzParams = 8;
    float sum_average;
    unsigned int cnt_average;
    int   nextStep;
    int AnzahlElements = 8;


};
#endif

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#include "Twiddle.h"
/**********************************************************************/
Twiddle::Twiddle( int anzParams, float p0 , float p1, float p2, float p3, float p4 ,
                  float p5, float p6, float p7,
                  float dp0, float dp1, float dp2, float dp3 , float dp4,
                  float dp5, float dp6 , float dp7)
/**********************************************************************/
{
  params[0]  = p0;
  params[1]  = p1;
  params[2]  = p2;
  params[3]  = p3;
  params[4]  = p4;
  params[5]  = p5;
  params[6]  = p6;
  params[7]  = p7;
  dparams[0] = dp0;
  dparams[1] = dp1;
  dparams[2] = dp2;
  dparams[3] = dp3;
  dparams[4] = dp4;
  dparams[5] = dp5;
  dparams[6] = dp6;
  dparams[7] = dp7;
  index = 0;
  besterr = 9999.99;
  nextStep = 1;
  AnzahlElements = anzParams;
}
/**********************************************************************/
Twiddle::Twiddle( int anzParams, float iparams[], float idparams[]  )   // Constructor
/**********************************************************************/
{ index = 0;
  besterr = 9999.99;
  nextStep = 1;
  AnzahlElements = anzParams;
}
/**********************************************************************/
float Twiddle::next(float error,  float &p0, float &p1, float &p2, float &p3,
                    float &p4, float &p5, float &p6, float &p7)
/**********************************************************************/
{
  static int skip = 0;

  sum_average += abs(error);
  cnt_average ++;

  if (skip > 5 ||  // for collection some data
      skip == 0 ) { //first Time
    skip = 1;
    if ( cnt_average > 0 ) {
      average = sum_average / cnt_average;
      sum_average = 0;
      cnt_average = 0;
    }
  }
  else {
    skip ++;
    return average;
  }

  if (( dparams[0] +  dparams[1] +  dparams[2] +  dparams[3] +  ( dparams[4] +  dparams[5] +  dparams[6] +  dparams[7])) < 0.03 ) {
    //   Serial.println(" erledigt ");
    p0 = params[0];
    p1 = params[1];
    p2 = params[2];
    p3 = params[3];
    p4 = params[4];
    p5 = params[5];
    p6 = params[6];
    p7 = params[7];

    // try again
    dparams[0] *= 4.0;
    dparams[1] *= 4.0;
    dparams[2] *= 4.0;
    dparams[3] *= 4.0;
    dparams[4] *= 4.0;
    dparams[5] *= 4.0;
    dparams[6] *= 4.0;
    dparams[7] *= 4.0;
    besterr = 9999.99;

    return average;  // it is done
  }


  if (  nextStep == 3 ) {
    nextStep = 1;
    if (index == AnzahlElements - 1) {
      index = 0;
    } else {
      index ++;
    }
    params[index] +=  dparams[index];
  }

  logging(); // last step

  calcCost(average);

  p0 = params[0];
  p1 = params[1];
  p2 = params[2];
  p3 = params[3];
  p4 = params[4];
  p5 = params[5];
  p6 = params[6];
  p7 = params[7];
  return average;
}
//----------------------------------------------------------------
void Twiddle::calcCost(float average)
//----------------------------------------------------------------
// Dieser Algorithmus sucht nun den Parameterraum intelligent ab und
// variiert die Schrittweite der Suche, je nachdem ob man in der Nhe
// eines Maxima bzw. Minima ist.
{

  switch (nextStep) {
    case 1:
      if (average < besterr) {
        // aktuelle Kosten < besterr # There was some improvement
        besterr = average;
        //mit grerem Schritt probieren
        dparams[index] *= 1.1;
        nextStep = 3;
      } else // # There was no improvement
      {
        // # Go into the other direction
        params[index] =  params[index] - (2 * dparams[index]);
        nextStep = 2;
      }
      break;

    case 2:
      if (average < besterr) {
        // aktuelle Kosten < besterr # There was some improvement
        besterr = average;
        //mit grerem Schritt probieren
        dparams[index] *= 1.05;
        nextStep = 1;
      } else {
        // As there was no improvement, the step size in either
        // direction, the step size might simply be too big.
        params[index] += dparams[index];
        dparams[index] *=  0.95;//an sonsten kleineren Schritt
        nextStep = 3;
      }
      break;
  }
}
/*------------------------------------------------------------
  # Choose an initialization parameter vector
  p = [0, 0, 0]
  # Define potential changes
  dp = [1, 1, 1]
  # Calculate the error
  best_err = A(p)
  threshold = 0.001
  while sum(dp) > threshold:
    for i in range(len(p)):
        p[i] += dp[i]
        err = A(p)
        if err < best_err:  # There was some improvement
            best_err = err
            dp[i] *= 1.1
        else:  # There was no improvement
            p[i] -= 2*dp[i]  # Go into the other direction
            err = A(p)
            if err < best_err:  # There was an improvement
                best_err = err
                dp[i] *= 1.05
            else  # There was no improvement
                p[i] += dp[i]
                # As there was no improvement, the step size in either
                # direction, the step size might simply be too big.
                dp[i] *= 0.95

  https://www.gomatlab.de/twiddle-algorithmus-zum-optimieren-von-parametern-t24517.html
   % Maximierung der Kostenfunktion!
  while sum(dparams) > 0.01
    for i=1:length(params) % alle Parameter durch gehen
        params(i)=params(i)+dparams(i);
        %Kostenfunktion ausrechnen
        cfzz(it) = calcCost(params(1),params(2));
        if cfzz(it) > besterr
            besterr = cfzz(it);
            dparams(i)= dparams(i)*1.05;
        else
            % in andere Richtung suchen
            params(i)=params(i)- 2*dparams(i);
            cfzz(it) = calcCost(params(1),params(2));
            if cfzz(it) > besterr %wenn aktuelle Kosten hher (=gut)
                besterr = cfzz(it);
                dparams(i) = dparams(i)*1.05; %mit grerem Schritt probieren
            else
                params(i)=params(i)+dparams(i);
                dparams(i)=dparams(i)*0.95; % an sonsten kleineren Schritt
            end
        end
    it = it+1;
   end
*/

//----------------------------------------------------------------
void Twiddle::logging()
//----------------------------------------------------------------
{

  Serial.print(" Step= ");
  Serial.print(nextStep );
  Serial.print(" Ind= ");
  Serial.print(index );
  Serial.print(" av= ");
  Serial.print(average , 4 );
  Serial.print(" besterr ");
  Serial.print(besterr , 4 );
  Serial.print(" P0 ");
  Serial.print(params[0]  , 4 );
  Serial.print(" P1 ");
  Serial.print(params[1]  , 4);
  Serial.print(" P2 ");
  Serial.print(params[2]  , 4 );
  Serial.print(" P3 ");
  Serial.print(params[3]  , 4 );
  Serial.print(" P4 ");
  Serial.print(params[4]  , 2 );
  Serial.print(" P5 ");
  Serial.print(params[5]  , 2);
  Serial.print(" P6 ");
  Serial.print(params[6]  , 2 );
  Serial.print(" P7 ");
  Serial.print(params[7]  , 4 );
  Serial.println(" ");
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
// --------------------------------------------------------------
void SeriellerPlotter( JStickData JStick)
// --------------------------------------------------------------
{
  // blau, rot, grn, gelb, lila, grau

  Serial.print(YawPitchRoll.pitch * 5);  //grau
  Serial.print(",");
  Serial.print(PidControler.pTerm );  //grau
  Serial.print(",");
  Serial.print(PidControler.dTerm / 10 );  //grau
  Serial.print(",");
  Serial.print(PidControler.iTerm * 10 );  //grau
  Serial.print(",");
  Serial.print(Robot.StepsA / 50 ); //grau
  Serial.println(" ");
  /*
    Serial.print(YawPitchRoll.pitch * 5);  //grau
    Serial.print(",");
    Serial.print(Robot.StepsA / 100 ); //grau
    Serial.print(",");
    Serial.print(Robot.PositionAB );  //grau
    Serial.print(",");
    Serial.print(Robot.HoldPosition );  //grau
    Serial.print(",");
    Serial.print(Robot.DeltaPos * 10 ); //grau
    Serial.print(" ");
    Serial.print(Robot.DeltaForward * 10 ); //grau
    Serial.println(" ");
  */
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#ifndef PidParameter_h
#define  PidParameter_h
#include "PidParameter.h"

// PidParameter Motor
struct PidParameter {
  float          K = 5.0;
  float          Kp =  9.4282 ;
  float          Ki =  5.5223;
  float          Kd = 0.145;
  float          Ka = 0.1220;
  float          Kx = 0.0; //
};
// PidParameter Position
struct PidParameterPos {
  float          K = 1.0;
  float          Kp = 0.0;
  float          Ki = 0.0;
  float          Kd = 0.08 ;
  float          Ka = 0.0 ;
  float          Kx = 0.0;
};
#endif

//Step= 1 Ind= 0 av= 1.5500 besterr 0.0300 P0 9.4282 P1 5.5223 P2 0.1445 P3 0.1220 P4 0.03 P5 0.00 P6 0.02 P7 0.0000
//Step= 1 Ind= 0 av= 5.1000 besterr 0.0267 P0 0.0315 P1 0.0015 P2 0.0000 P3 0.0000 P4 9.43 P5 5.52 P6 0.14 P7 0.1220 
//Step= 1 Ind= 1 av= 4.6900 besterr 0.0133 P0 0.0336 P1 0.0025 P2 0.0010 P3 0.0000 P4 9.43 P5 5.52 P6 0.14 P7 0.1220 

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#ifndef PidControl_h
#define  PidControl_h
#include "Arduino.h"

#include "PidParameter.h"

/**********************************************************************/
class PidControl
/**********************************************************************/
{
  public:

    PidControl(float K, float Kp, float Ki, float Kd, float iKa , float iKx) ;
    PidControl(PidParameter      Params) ;
    PidControl(PidParameterPos    Params) ;
    PidControl* getInstance();

    /* C++ Overloading // changePIDparams = different PidParameter !!!!
      An overloaded declaration is a declaration that is declared with the same
      name as a previously declared declaration in the same scope, except that
      both declarations have different arguments and obviously different
      definition (implementation).
    */
    void     changePIDparams(PidParameter Params);
    void     changePIDparams(PidParameterPos Params);

    float    calculate (float iAngle, float isetPoint );
    float    getSteps ();
    void     reset ();
    void     test      ( );
    float    DeltaKp(float iError);
    float    eSpeed;
    float    pTerm;
    float    iTerm;
    float    dTerm;
    float    error;
    float    integrated_error;
   // volatile bool  DirForward;
    float    Last_error;

  protected:
    struct   PidParameter params;
    float    LastK;
    float    K;
    float    Ki;
    float    Kd;
    float    Kp;
    float    Ka;
    float    Kx;
    //   float    Last_angle;
    float    timeChange ;
    unsigned long Last_time;
    unsigned long Now;
    int      ptr;
    PidControl* pPID;
    bool first ;
};

#endif

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    PID Controller
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#include "PidControl.h"
#include <Arduino.h>

/**********************************************************************/
PidControl::PidControl (float iK, float iKp, float iKi, float iKd, float iKa, float iKx)
/**********************************************************************/
{ // Constructor 1
  K  = iK;
  Kp = iKp;
  Kd = iKd;
  Ki = iKi;
  Ka = iKa;
  Kx = iKx;
  Last_error = 0;
  integrated_error = 0;
  first = true;
}
/**********************************************************************/
PidControl::PidControl (PidParameter Params)
/**********************************************************************/
{ // Constructor 2 Motor, different PidParameter
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
  Last_error = 0;
  integrated_error = 0;
  first = true;
}
/**********************************************************************/
PidControl::PidControl (PidParameterPos Params)
/**********************************************************************/
{ // Constructor 3 Distance, different PidParameter
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
  Last_error = 0;
  integrated_error = 0;
  first = true;
}
/**********************************************************************/
PidControl* PidControl::getInstance()
/**********************************************************************/
{
  pPID = this;
  return pPID;
}
/**********************************************************************/
void PidControl::test ()
/**********************************************************************/
{
  Serial.print("PID Test ");
  ptr = (int) this;
  Serial.print("PIDptr ");
  Serial.println(ptr , HEX);
}
/**********************************************************************/
float  PidControl::calculate (float iAngle, float isetPoint )
/**********************************************************************/
// new calculation of Steps per Second // PID algorithm
{
  Now = micros();
  if (first) {
    first = false;
    Last_time = Now;
    integrated_error = 0;
  }
  timeChange = (Now - Last_time)  ;
  timeChange = timeChange / 1000.0;  // in millisekunden
  error = isetPoint -  iAngle;

  if ( timeChange != 0) {
    dTerm =  1000.0 * Kd * (error - Last_error) /  timeChange  ;
  }

  integrated_error = integrated_error  + ( error * timeChange );
  iTerm =   Ki * integrated_error / 1000.0;

  pTerm =   Kp  * error + ( Ka * integrated_error ); // modifying Kp

  // Compute PID Output in Steps per second
  eSpeed = K * (pTerm + iTerm + dTerm) ;

  /*Remember something*/
  Last_time  = Now;
  Last_error = error;

  //  digitalWrite(TestPIDtime, !digitalRead(TestPIDtime)); // Toggle  Pin for reading the Frequenzy
  // eSpeed = constrain (eSpeed , -500.0 , 500.0 ); // 10 Steps per Second because Microstep
 
 return eSpeed;  // Steps per Second
}

/**********************************************************************/
void PidControl::reset ()
/**********************************************************************/
{
  integrated_error = 0.0;
  Last_error = 0.0;
}


/**********************************************************************/
void PidControl::changePIDparams (PidParameter Params)
// changePIDparams = different PidParameter !!!!
/**********************************************************************/
{
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
}
/**********************************************************************/
void PidControl::changePIDparams (PidParameterPos Params)
// changePIDparams = different PidParameter !!!!
/**********************************************************************/
{ // different PidParameter !!!!
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
}

/**********************************************************************/
float PidControl::getSteps () {
  return eSpeed;
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
    Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
    by Jeff Rowberg <jeff@rowberg.net>

    MPU-6050 Accelerometer + Gyro

    The MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single chip. 
    It is very accurate, as it contains 16-bits analog to digital conversion hardware for each channel. 
    Therefor it captures the x, y, and z channel at the same time. The sensor uses the I2C -bus to interface with the Arduino.

    I used Digital Motion Processing with the MPU-6050 sensor, to do fast calculations directly on the chip. 
    This reduces the load for the Arduino.  

    https://playground.arduino.cc/Main/MPU-6050

    Because of the orientation of my board, I used yaw/pitch/roll angles (in degrees) calculated from the quaternions coming from the FIFO. 
    By reading Euler angle I got problems with Gimbal lock.    
*/

/*    0x02,   0x16,   0x02,   0x00, 0x07                // D_0_22 inv_set_fifo_rate

    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

    // It is important to make sure the host processor can keep up with reading and processing
    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
*/
// ------------------------------------------------------------------------
// orientation/motion vars
// ------------------------------------------------------------------------
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
// ------------------------------------------------------------------------
// MPU control/status vars
// ------------------------------------------------------------------------
bool     dmpReady = false;  // set true if DMP init was successful
uint8_t  mpuIntStatus;      // holds actual interrupt status byte from MPU
uint8_t  devStatus;         // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;        // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;         // count of all bytes currently in FIFO
uint8_t  fifoBuffer[64];    // FIFO storage buffer



// --------------------- Sensor aquisition  -------------------------
MpuYawPitchRoll ReadMpuRunRobot()
// --------------------- Sensor aquisition  -------------------------
// wait for MPU interrupt or extra packet(s) available
// --------------------------------------------------------------------
{
  if (mpuInterrupt or fifoCount >= packetSize) {
    if  (mpuInterrupt) mpuInterrupt = false;  // reset interrupt flag
    digitalWrite(LED_PIN, HIGH); // blink LED to indicate activity
    //    Angle = ReadMpuRunRobot6050() - CalibrationAngle;
    YawPitchRoll = ReadMpuRunRobot6050();
    YawPitchRoll.pitch  +=  Calibration;
    // blinkState = !blinkState;
    digitalWrite(LED_PIN, LOW);
  }
  return YawPitchRoll ;
}
// -------------------------------------------------------------------- -
MpuYawPitchRoll ReadMpuRunRobot6050()
// -------------------------------------------------------------------- -
{
  static float pitch_old;
  static float yaw_old;
  static float yaw_tmp;
  static float yaw_delta;
  //  static float pitch;

  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();
  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    //    mpu.setDMPEnabled(false);
    mpu.resetFIFO();
    FifoOverflowCnt ++;
    fifoCount = 0;
    YawPitchRoll.pitch = pitch_old;
    return YawPitchRoll;

  }
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  // Register 58  lnterrupt Status INT_STATUS
  // MPU-6500 Register Map and Descriptions Revision  2.1
  // Bit [1] DMP_INT This bit automatically sets to 1 when the DMP interrupt has been generated.
  // Bit [0] RAW_DATA_RDY_INT1 Sensor Register Raw Data sensors are updated and Ready to be read.
  if ((mpuIntStatus) & 0x02 || (mpuIntStatus & 0x01)) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize)  fifoCount = mpu.getFIFOCount();

    while (fifoCount > 0) {
      // read a packet from FIFO
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount = mpu.getFIFOCount();
      //      fifoCount -= packetSize;
    }
    // the yaw/pitch/roll angles (in degrees) calculated from the quaternions coming
    // from the FIFO. Note this also requires gravity vector calculations.
    // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
    // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    // mpu.dmpGetEuler(euler, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    YawPitchRoll.pitch = -(ypr[1] * 180 / M_PI); //pitch

    yaw_tmp  = (abs(ypr[0] * 180 / M_PI));
    yaw_delta = yaw_tmp - yaw_old;
    yaw_old = yaw_tmp;
    YawPitchRoll.yaw  += yaw_delta;

    // actual quaternion components in a [w, x, y, z]
    // YawPitchRoll.pitch = (q.y) * 180;
    // YawPitchRoll.yaw = (q.z );
    // YawPitchRoll.yaw =  mapfloat(YawPitchRoll.yaw , -1.0, 1.0, 0.0, 360.0);
  }
  pitch_old = YawPitchRoll.pitch ;
  return  YawPitchRoll ;
}
// --------------------------------------------------------------
void MpuInit()
// --------------------------------------------------------------
// MPU6050_6Axis_MotionApps20.h
// 0x02,   0x16,   0x02,   0x00, 0x09  => 50msec 20 Hz
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
// It is important to make sure the host processor can keep up with reading and processing
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.

{
  // after Reset of Arduino there is no Reset of MPU
  pinMode(MpuInterruptPin, INPUT);

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-84);
  mpu.setZAccelOffset(1788); //

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(MpuInterruptPin), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
    //    mpu.resetFIFO();  // after Reset importand

  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print( "Error MPU6050 "  );
    do  {} while ( 1 == 1);
  }
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#ifndef Motor_h
#define  Motor_h
#include "Arduino.h"
#include "PidControl.h"
#include "DuePWMmod.h"

// ------------------------------------------------------------------------
class Motor
// ------------------------------------------------------------------------

{ public:
    Motor(DuePWMmod* ipwm,  int iPinDir, int iPinStep,
          int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide);

    volatile bool  DirForward;

   // struct PidParameter params;
    void init() ;
    Motor* getInstance();
    void SleepMode ( );
    void RunMode ( );
    void toggleMode ( );
    float Run(float Steps);

    // Four different step resolutions: full-step, half-step, 1/4-step, and 1/8-step
    void MsFull ( );
    void MsHalf ( );
    void MsQuater ( );
    void MsMicrostep ( );

    int _Ms1, _Ms2, _PinDir, _PinStep, _PinSleep;
    int _Divisor;
    Motor* pMotor;

    unsigned long  lastTime;
    char          _MotorSide;
    DuePWMmod     *ptrpwm;


  private:
    bool MotorMode;
    int ptr;
};
#endif

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de

  Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev, 4248mm, 4V, 1.2 A/Phase
  https://www.pololu.com/product/1200/faqs
  This NEMA 17-size hybrid stepping motor can be used as a unipolar or bipolar stepper motor
  and has a 1.8 step angle (200 steps/revolution). Each phase draws 1.2 A at 4 V,
  allowing for a holding torque of 3.2 kg-cm (44 oz-in).

  Wheel Durchmesser 88mm = > Distance per Pulse Dpp = d * pi / 200 =  1,3816 mm
  Distance per Revolution =  276,32 mm
  Max 1000 Steps per Second = 5 Revolutions => 13816 mm Distance

  Motion Control Modules  Stepper Motor Drivers  MP6500 Stepper Motor Driver Carriers
  https://www.pololu.com/product/2966https://www.pololu.com/product/2966
  This breakout board for the MPS MP6500 microstepping bipolar stepper motor driver has a
  pinout and interface that are very similar to that of our popular A4988 carriers,
  so it can be used as a drop-in replacement for those boards in many applications.
  The MP6500 offers up to 1/8-step microstepping, operates from 4.5 V to 35 V,
  and can deliver up to approximately 1.5 A per phase continuously without a heat sink
  or forced air flow (up to 2.5 A peak).

  MS1   MS2   Microstep Resolution
  Low   Low   Full step
  High  Low   Half (1/2) step
  Low   High  Quarter (1/4) step
  High  High  Eighth (1/8) step
*/

#include "Motor.h"
#include "Config.h"
/**********************************************************************/
Motor::Motor(DuePWMmod* ipwm, int iPinDir, int iPinStep,
             int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide )
/**********************************************************************/
{
  _Ms1       =  iPinMs1;
  _Ms2       =  iPinMs2;
  _PinStep   = iPinStep;
  _PinDir    = iPinDir;
  _PinSleep  = iPinSleep;
  _MotorSide = iMotorSide;

  // The default SLEEP state prevents the driver from operating;
  // this pin must be high to enable the driver
  pinMode(_PinSleep, OUTPUT);
  pinMode(_PinDir, OUTPUT);
  pinMode(_Ms1, OUTPUT);
  pinMode(_Ms2, OUTPUT);

  ptr = (int) this;
  ptrpwm = ipwm;
}
/**********************************************************************/
Motor* Motor::getInstance()
/**********************************************************************/
{
  pMotor = this;
  return pMotor;
}
/**********************************************************************/
void Motor::init ( )
/**********************************************************************/
{

  Serial.print("Motor ");
  Serial.print(ptr , HEX);
  Serial.print(" Side ");
  Serial.print(_MotorSide);
  Serial.print(" iPinStep ");
  Serial.print(_PinStep);
  Serial.print(" iPinSleep ");
  Serial.print(_PinSleep);
  Serial.println("  init...");
  lastTime = millis();

}
/**********************************************************************/
void Motor::SleepMode( )
/**********************************************************************/
{
  digitalWrite(_PinSleep, LOW);
  MotorMode = false;
}
/**********************************************************************/
void Motor::RunMode( )
/**********************************************************************/
{
  digitalWrite(_PinSleep, HIGH);
  MotorMode = true;
}
/**********************************************************************/
void Motor::toggleMode( )
/**********************************************************************/
{
  if ( MotorMode == false ) RunMode( );
  else  SleepMode();
}
/**********************************************************************/
float Motor::Run(float Steps) {
  /**********************************************************************/

  //  Motor 20070C9C Side A iPinStep 6 iPinSleep 22  init...
  // Motor 20070CF4 Side B iPinStep 7 iPinSleep 24  init...

  if (!digitalRead(PinSleepSwitch)) {
    RunMode( );

    if (_MotorSide == rechterMotor) {
      if (Steps >= 0 ) {
        digitalWrite(_PinDir, LOW);
        DirForward = true ;
      }
      else {
        digitalWrite(_PinDir, HIGH);
        DirForward = false ;
      }
    } else
    {
      if (Steps >= 0 ) {
        digitalWrite(_PinDir, HIGH);
        DirForward = true ;
      }
      else {
        digitalWrite(_PinDir, LOW);
        DirForward = false ;
      }
    }

    if (_Divisor > 0) {
      Steps = Steps * _Divisor; // convert into Microsteps
    }

    if ((abs(Steps) < 2.0))  Steps = 2.0;
    return Steps;
  }
  else SleepMode( );
}
/**********************************************************************/
void  Motor::MsFull ( ) {
  digitalWrite(_Ms1, LOW);
  digitalWrite(_Ms2, LOW);
  _Divisor = 1;
}
void  Motor::MsHalf ( ) {
  digitalWrite(_Ms1, LOW);
  digitalWrite(_Ms2, HIGH);
  _Divisor = 2;
}
void  Motor::MsQuater ( ) {
  digitalWrite(_Ms1, HIGH);
  digitalWrite(_Ms2, LOW);
  _Divisor = 4;
}
void  Motor::MsMicrostep ( ) {
  digitalWrite(_Ms1, HIGH);
  digitalWrite(_Ms2, HIGH);
  _Divisor = 8;
}

/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing  
 *  written by : Rolf Kurth in 2019
 *  rolf.kurth@cron-consulting.de
 */
 
 // --------------------------------------------------------------
void LCD_show()
// --------------------------------------------------------------
{
  static int Line = 0;
  Line = !Line;
  switch (Line) {
    case 0:
      LCD_showLine0();
      break;
    default:
      LCD_showLine1();
      break;
  }
}
// --------------------------------------------------------------
void LCD_showLine1()
// --------------------------------------------------------------
{

  static int delay;
  if ( Running == true ) {
    lcd.setCursor(0, 1);
    lcd.print("P        ");
    lcd.setCursor(2, 1);
    lcd.print(YawPitchRoll.pitch, 2);
    lcd.setCursor(8, 1);
    lcd.print("Y      ");
    lcd.setCursor(10, 1);
    lcd.print(YawPitchRoll.yaw, 2);
    lcd.setCursor(15, 1);

  } else { // it is Init Mode

    delay = (( millis() -  ProgStartTime ) ) ;
    lcd.setCursor(0, 1);
    lcd.print("P ");
    lcd.setCursor(2, 1);
    lcd.print(YawPitchRoll.pitch, 2);
    lcd.setCursor(8, 1);
    lcd.print("Y ");
    lcd.setCursor(10, 1);
    lcd.print(YawPitchRoll.yaw, 2);
  }
}
// --------------------------------------------------------------
void LCD_showLine0()
// --------------------------------------------------------------
{
  static int delay;
  if ( Running == true ) {
    lcd.setCursor(0, 0);
    lcd.print("Run  ");
    lcd.setCursor(4, 0);
    lcd.print("K ");
    lcd.setCursor(5, 0);
    lcd.print(TuningValue, 3);
   // lcd.print(FifoOverflowCnt);
    lcd.setCursor(11, 0);
    lcd.print(Voltage, 1);
    lcd.print("V");
  } else { // it is Init Mode
    delay = ((StartDelay - ( millis() -  ProgStartTime)) / 1000 ) ;
    lcd.setCursor(0, 0);
    lcd.print("Init ");
    lcd.setCursor(7, 0);
    lcd.print("  ");
    lcd.setCursor(7, 0);
    lcd.print(delay);
    lcd.setCursor(11, 0);
    lcd.print(Voltage, 1);
    lcd.print("V");
  }
}