Project in progress
Self Balancing Robot via Stepper Motor

Self Balancing Robot via Stepper Motor © GPL3+

With stepper motor via microstepping, digital motion processing, auto tuning via Twiddle Algorithmus, cascaded PID Controller

  • 1,993 views
  • 3 comments
  • 18 respects

Components and supplies

Ard due
Arduino Due
×1
11028 01
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
The InvenSense MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single chip, with Digital Motion Processing Unit
×1
MP6500 Stepper Motor Driver Carrier
The MP6500 offers up to 1/8-step microstepping and can deliver up to approximately 1.5 A per phase.
×2
398 09
Adafruit RGB Backlight LCD - 16x2
×1
61pby065esl  sx679  tnr8syww5d
HC-05 Bluetooth Module
×1
Nema%2017 2
OpenBuilds NEMA 17 Stepper Motor
×2
7.4V 2S 3300mAh 35C Li-Polymer Lipo
http://www.floureon.com/product-g_335.html
×1

About this project

Control of the robot via an Android Bluethoot App.

I bought my first Arduino three years ago. I was fascinated by by the idea of a self-balancing-robot and this was my first project.

I underestimated the difficulties, so the development took a long time. Many changes were necessary:

The most important changes were:

  • Change from Arduino Mega to Arduino DUE
  • Change from direct-current-motor with encoder to stepper motor
  • Use of digital motion processing
  • MP 6500 stepper motor driver carrier

Features of the Robot

  • Control of the robot via an Android Bluethoot App.
  • Stepper Motor, Unipolar/Bipolar, 200 Steps/Rev, 42×48mm, 4V, 1.2 A/Phase
  • Stepper Motor Driver Carrier can deliver up to 1.5 A per phase continuously, four different step resolutions: full-step, half-step, 1/4-step, and 1/8-step.
  • cascaded PID Controller for Motor and for Position
  • Task Dispatcher via Interrupt
  • PWM Controller
  • MPU-6050 sensor with accelerometer and gyro, using Digital Motion Processing with MPU-6050
  • Auto Tuning via Twiddle Algorithmus
  • Battery Control
  • Software Design Object oriented

Restrictions

Running only at Arduino Due

Stepper Motors

I decided to use Stepper engines because they offer the following advantages:

  • Exact positioning, no accumulated errors
  • Holding torque in rest position
  • No deceleration/lag due to the moment of inertia of the motor
  • simple position sensing by counting PWM signal

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.

MP6500 Stepper Motor Driver Carrier

The used stepper motor driver lets you control one bipolar stepper motor at up to approximately 1.5 A per phase continuously without a heat sink or forced air flow ( Datasheet).

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.

Cascaded PID Controler

The robot is controlled by cascaded PID controllers. Two controllers are responsible for driving the motors (right and left side). Two additional controllers for controlling the position of the robot.

The motor controller ensures that the robot remains upright. The Position Controler ensures that the robot maintains its correct setpoint prescribed position.

Cascade control is a cascading of several controllers; the associated control loops are nested in each other. The controller output variable of one controller (master controller, Position) serves as the reference variable for another (slave controller, Motor).

The PID controllers are implemented in the "PIDControl" class. 4 instances of the class are instantiated for the 4 controllers.

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.

Before this change the robot always wanted to run away. A simple increase of the I-portion made the robot unstable. This solution certainly depends strongly on the structure of the robot (weight, centre of gravity, etc.).

PWM Controler

To generate the PWM signals I modified the library of "randomvibe . (https://github.com/cloud-rocket/DuePWM). The PWM controller is implemented in the DuePWMmod class. Unique frequencies set via PWM Clock-A ("CLKA") and Clock-B ("CLKB").

Task Dispatching

The Dispatching of the tasks:

  • Robot
  • LCD
  • plotter

is done with the help of the Timer Library by Ivan Seidel.( https://github.com/ivanseidel/DueTimer thanks)Three interrupts with the corresponding times are generated to call the tasks.

Software architecture

The robot consists of the main program SBRobotDue02 and the following classes:

  • PidControl
  • Battery
  • DuePWMmod
  • DueTimer
  • Twiddle
  • Motor
  • Vehicle
  • MyMPU

and the following includes:

  • Config
  • LCD
  • PidParameter
  • Plotter

The architecture is shown in the following UML Diagrams:

The folowing sequence diagram shows the interactions between instanced objects in the sequential order that those interactions occur.

The vehicle in detail

The run method of the Vehicle class is crucial for control the Robotin all directions.

/**********************************************************************/
void Vehicle::Run(float angle,  int iPositionA, int iPositionB )
/**********************************************************************/
{ /*
   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
 */
 const int     tDelay = 10;
 const int     tDelaySpeed = tDelay * 2;
 const float   MaxSpeed = 1.0  ; //1.5;    11.03.19
 static bool   decelerate = false;
 static bool   moving = false;
 /* "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
 */
 PositionA = (float(iPositionA ) / 100 );
 PositionB = (float(iPositionB ) / 100 );
 if (firstRun) {
   firstRun = false;
   skipPos = tDelay - 1;
   LastPositionA = (PositionA); 
   LastPositionB = (PositionB);
 }
 if ( BluetoothRead( DeltaPosA,  DeltaPosB, StopDriving, spinning )) {
   // new Movement value DeltaPosA via Bluetooth
   skipPos = tDelay - 1;
   skipSpeed = tDelay - 1;
 }
 if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
   skipPos = 0;
   if (StopDriving) {  // Stop driving, Last Current position as target position
     if (abs(DeltaPosA) < 0.001 && abs(DeltaPosA) < 0.001)
     { DeltaPosA = DeltaPosA = 0.0;
       decelerate = false;
     } else
     { // decelerate slowly
       decelerate = true;
       DeltaPosA = DeltaPosA * 0.35;
       DeltaPosB = DeltaPosB * 0.35;
     }
   }
   // a basic distinction must be made between driving and standing on the spot
   // If the robot is not to move, the last position is important as the setpoint value.
   if (abs((DeltaPosA) == 0 && abs(DeltaPosB) == 0 ) || decelerate)
   { // standing on the spot the robot has to use the last position as target value
     SetpointPositionA = LastPositionA;
     SetpointPositionB = LastPositionB;
     SetpointA = constrain( (pPidDistA->calculate (PositionA , SetpointPositionA)), -MaxSpeed, MaxSpeed);
     SetpointB = constrain( (pPidDistB->calculate (PositionB , SetpointPositionB)), -MaxSpeed, MaxSpeed);
     moving = false;
   } else { // driving : the robot has to use the actual position with the setpoint as target value
     SetpointPositionA = PositionA + DeltaPosA;
     SetpointPositionB = PositionA + DeltaPosB;
     SetpointA = constrain( (pPidDistA->calculate (PositionA , SetpointPositionA)), -MaxSpeed, MaxSpeed);
     SetpointB = constrain( (pPidDistB->calculate (PositionB , SetpointPositionB)), -MaxSpeed, MaxSpeed);
     moving = true;
   }
   if (!decelerate) {
     // LastPositionA = (PositionA); // for the change from driving to standstill remember the last position
     // LastPositionB = (PositionB);
   }
   if (moving) {
     LastPositionA = (PositionA); // for the change from driving to standstill remember the last position
     LastPositionB = (PositionB);
   }
 }
 // PID calculation of new steps
 StepsA = pMotorA->Calculate(angle, -SetpointA);
 StepsB = pMotorB->Calculate(angle, -SetpointB);
 //After the robot has moved to the right or left, slight deviations occur during the next straight ride.
 //workaound: when driving straight forwad or backwad(!spinning), the equality of the steps is forced.
 if (!spinning)  {
   ABdiff = StepsA - StepsB;
 }
 // run the Motors, here Steps = full steps
 pMotorA->Run(StepsA );
 pMotorB->Run(StepsB + ABdiff);
 Speed = ( StepsA + StepsB ) / 2;
}

The BluetoothRead method receives the values for the movement of the robot.

 if ( BluetoothRead( DeltaPosA,  DeltaPosB, StopDriving, spinning )) {
   // new Movement value DeltaPosA via Bluetooth
   skipPos = tDelay - 1;
   skipSpeed = tDelay - 1;
 }

The following query ensures that position control is called 10 times slower than motor control.

if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
   skipPos = 0;

A basic distinction must be made between driving and standing on the spot. The different treatment can be seen as follows. Here the PID calculation for the position control is done (outer PID controler).

   if (abs((DeltaPosA) == 0 && abs(DeltaPosB) == 0 ) || decelerate)
   { /* standing on the spot the robot has to use
       the last position as target value*/
   }
   SetpointPositionA = LastPositionA;
   SetpointPositionB = LastPositionB;
   SetpointA = constrain( (pPidDistA->calculate (PositionA , 
                          SetpointPositionA)),
                          -MaxSpeed, MaxSpeed);
   SetpointB = constrain( (pPidDistB->calculate (PositionB , 
                           SetpointPositionB)),
                          -MaxSpeed, MaxSpeed);
   moving = false;
 } else {
   /* driving : the robot has to use the actual position
     with the setpoint as target value*/
   SetpointPositionA = PositionA + DeltaPosA;
   SetpointPositionB = PositionA + DeltaPosB;
   SetpointA = constrain( (pPidDistA->calculate (PositionA , 
                          SetpointPositionA)), -MaxSpeed, MaxSpeed);
   SetpointB = constrain( (pPidDistB->calculate (PositionB , 
                           SetpointPositionB)), -MaxSpeed, MaxSpeed);
   moving = true;
 }

If the Robot is moving and after the new command should stop, the last position is needed to continue at these position.

 if (moving) {
   LastPositionA = (PositionA); /* for the change from 
      driving to standstill remember the last position */
   LastPositionB = (PositionB);
 }

Last baut not least,the PID control for the Motor (Steps /sec) is needed for the robot (inner PID controler).

 // PID calculation of new steps
 StepsA = pMotorA->Calculate(angle, -SetpointA);
 StepsB = pMotorB->Calculate(angle, -SetpointB);

After the robot has moved to the right or left, slight deviations occur during the next straight ride.

Workaound: when driving straight forwad or backwad(!spinning), the equality of the steps is forced.

 if (!spinning)  {
   ABdiff = StepsA - StepsB;
 }
 // run the Motors, here Steps = full steps
 pMotorA->Run(StepsA );
 pMotorB->Run(StepsB + ABdiff);
}

Thats all to drive the robot.

PID controler in detail

In the following diadram you can see the reaction of the cascaded PID controller after a disturbance.

The standard PID algorithm was slightly modified after longer test series.

There are 4 instances of the class PidControl. Two instances for motor control and two instances for position control. The calculation is done in the method PidControl::calculate.

/**********************************************************************/
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;
 if (eSpeed > 0 ) {
   DirForward = true ;
 } else {
   DirForward = false;
 }
 return eSpeed;  // Steps per Second
}

The instances of the classes are created in the sketch SBRobotDue02.

// ----------------------------------------------------------------------
// create PID Controller for Motor A and B
// -----------------------------------------------------------------------
#include "PidControl.h"
PidParameter PidParams;
PidControl PidMotorA(PidParams);
PidControl PidMotorB(PidParams);
// -----------------------------------------------------------------------
// create PID Controller Position A and B
// -----------------------------------------------------------------------
PidParameterDi PidParamsDi;
PidControl PidDistA(PidParamsDi);
PidControl PidDistB(PidParamsDi);

The different PID parameters are defined in the following structures and are passed during instanciation of the class. (C++ Overloading of the constructor )

// PidParameter Motor
struct PidParameter {
 float          K = 5.0;
 float          Kp = 9.3 ;
 float          Ki = 6.01;
 float          Kd = 0.12 ;
 float          Ka = 0.1246;
 float          Kx = 0.0; //
};
// PidParameter Position
struct PidParameterDi {
 float          K = 1.0;
 float          Kp = 0.18;
 float          Ki = 0.0;
 float          Kd = 0.4 ;
 float          Ka = 0.0 ;
 float          Kx = 0.0;
};

Motor in detail

To drive the motors, two instances of the motor class are created.

// -----------------------------------------------------------------------
// create objects for Motor 1 and Motor 2
// -----------------------------------------------------------------------
#include "Motor.h"
Motor MotorA(&pwm, &PidMotorA,  PinDirMotorA, PinStepMotorA, PinMs1MotorA,
            PinMs2MotorA, PinSleepA, rechterMotor); // create MotorA
Motor MotorB(&pwm, &PidMotorB,  PinDirMotorB, PinStepMotorB, PinMs1MotorB,
            PinMs2MotorB, PinSleepB, linkerMotor); // create MotorB 

The run method is responsible for controlling the stepper motor.

/**********************************************************************/
void Motor::Run(float Steps) {
/**********************************************************************/
 if (Steps >= 0 ) {
   if (_MotorSide == rechterMotor) {
     digitalWrite(_PinDir, LOW);
   }
   else {
     digitalWrite(_PinDir, HIGH);
   }
 } else
 {
   if (_MotorSide == rechterMotor) {
     digitalWrite(_PinDir, HIGH);
   }
   else {
     digitalWrite(_PinDir, LOW);
   }
 }
 if (_Divisor > 0) {
   Steps = Steps * _Divisor; // Microsteps
 }
 Steps_tmp = Steps;
 Steps = abs(Steps_tmp);
 if ( Steps < 2.0) {
   Steps = 2; // Microsteps
 }
 if (_MotorSide == rechterMotor) {
   ptrpwm->setFreq( Steps, rechterMotor  );
 }
 else {
   ptrpwm->setFreq( Steps, linkerMotor  );
 }
}

The method setFreq of the class DuePWMmod is called to generate the pwm signals.

Code

SBRobotDue02.inoC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    Auto Tuning via Twiddle Algorithmus
    cascaded PID Controller for Motor and for Position
    Task Dispatcher (function handler) via Interrupt
    PWM Controller

    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/

// ------------------------------------------------------------------------
// default settings
// ------------------------------------------------------------------------
//const bool AutoTuning = true;
const bool AutoTuning = false;
//const bool xyPlot = true;
const bool xyPlot = false;

// ------------------------------------------------------------------------
//  https://www.arduino.cc/en/Reference/HomePage
// ------------------------------------------------------------------------
/* based on Arduino Due
     The Arduino Due is the first Arduino board based on a 32-bit ARM core microcontroller.
     With 54 digital input/output pins, 12 analog inputs, it is the perfect board for
     powerful larger scale Arduino projects.
*/
/* Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev, 42x48mm, 4V, 1200mA
    https://www.pololu.com/product/1200/
*/
/*  MegunoLink is a customizable interface tool for Arduino sketches.
     https://www.megunolink.com/documentation/plotting/xy-plot-library-reference/?utm_source=mlp&utm_medium=app&utm_campaign=product&utm_content=plot-doc&utm_term=R
     The XYPlot class provides a convenient set of methods for setting properties and sending data to the MegunoLink Pro X-Y Plot visualizer.
     The plot we are sending data to.*/
//#include "MegunoLink.h" // Helpful functions for communicating with MegunoLink Pro.
//XYPlot MyPlot;

/* MP6500 Stepper Motor Driver Carrier
    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). This version of the board uses an
    on-board trimmer potentiometer for setting the current limit.
    https://www.pololu.com/product/2966
*/
/* Wireless Bluetooth Transceiver Module mini HC-05 (Host-Slave Integration)
*/

// ------------------------------------------------------------------------
// Libraries
/*Timer Library fully implemented for Arduino DUE
   to call a function handler every 1000 microseconds:
   Timer3.attachInterrupt(handler).start(1000);
   There are 9 Timer objects already instantiated for you: Timer0, Timer1, Timer2, Timer3, Timer4, Timer5, Timer6, Timer7 and Timer8.
   from https://github.com/ivanseidel/DueTimer
*/
#include "DueTimer.h"
/*MPU-6050 Accelerometer + Gyro
  The InvenSense 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.
  The sensor has a "Digital Motion Processor" (DMP), also called a "Digital Motion Processing Unit".
  This DMP can be programmed with firmware and is able to do complex calculations with the sensor values.
  The DMP ("Digital Motion Processor") can do fast calculations directly on the chip.
  This reduces the load for the microcontroller (like the Arduino).
  I2Cdev and MPU6050 must be installed as libraries
*/
/* MPU-6050 Accelerometer + Gyro
  The InvenSense 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.
  The sensor has a "Digital Motion Processor" (DMP), also called a
  "Digital Motion Processing Unit".
  The DMP ("Digital Motion Processor") can do fast calculations directly on the chip.
  This reduces the load for the Arduino.
  DMP is used in this Program
  https://playground.arduino.cc/Main/MPU-6050
  MPU-6050 I2Cdev library collection
  MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation#
  Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
  by Jeff Rowberg <jeff@rowberg.net> */
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu; // create object mpu
// ------------------------------------------------------------------------
/* Create PWM Controller
   Purpose: for Stepper PWM Control
   Setup one or two unique PWM frequenices directly in sketch program,
   set PWM duty cycle, and stop PWM function.
   Applies to Arduino-Due board, PWM pins 6, 7, 8 & 9, all others ignored
   Written By:  randomvibe
   modified by : Rolf Kurth
   https://github.com/cloud-rocket/DuePWM
*/
// ------------------------------------------------------------------------
#include "DuePWMmod.h"
DuePWMmod pwm; // create object pwm
// ------------------------------------------------------------------------
#include <LiquidCrystal.h> // LiquidCrystal(rs, enable, d4, d5, d10, d11)
LiquidCrystal lcd(9, 8, 4, 5, 10, 11); //create LiquidCrystal object
// ------------------------------------------------------------------------
// own classes in Tabs
// ------------------------------------------------------------------------
#include  "Config.h"
#include "Battery.h"  // Object Measurement
Battery myBattery(VoltPin); // create Object Measurement
// ------------------------------------------------------------------------
// create PID Controller for Motor A and B
// ------------------------------------------------------------------------
#include "PidControl.h"
PidParameter PidParams;
PidControl PidMotorA(PidParams);
PidControl PidMotorB(PidParams);
// ------------------------------------------------------------------------
// create PID Controller Position A and B
// ------------------------------------------------------------------------
PidParameterDi PidParamsDi;
PidControl PidDistA(PidParamsDi);
PidControl PidDistB(PidParamsDi);
// ------------------------------------------------------------------------
// create PID Controller Speed A and B
// ------------------------------------------------------------------------
PidParamsSpeed PidParamsSpeed;
PidControl PidSpeed(PidParamsSpeed);
// ------------------------------------------------------------------------
// create objects for Motor 1 and Motor 2
// ------------------------------------------------------------------------
#include "Motor.h"
Motor MotorA(&pwm, &PidMotorA,  PinDirMotorA, PinStepMotorA, PinMs1MotorA,
             PinMs2MotorA, PinSleepA, rechterMotor); // create MotorA
Motor MotorB(&pwm, &PidMotorB,  PinDirMotorB, PinStepMotorB, PinMs1MotorB,
             PinMs2MotorB, PinSleepB, linkerMotor); // create MotorB

// ------------------------------------------------------------------------
// create Robot
// ------------------------------------------------------------------------
#include "Vehicle.h"
Vehicle Robot = Vehicle(&pwm, &MotorA, &MotorB, &PidMotorA, &PidMotorB,
                        &PidDistA, &PidDistB,  &PidSpeed , PinSleepSwitch);

// ------------------------------------------------------------------------
#include "Twiddle.h"
/*  Twiddle Tuning ( params,  dparams);
    https://martin-thoma.com/twiddle/
    Twiddle is an algorithm that tries to find a good choice of parameters p for an algorithm A that returns an error.*/
Twiddle Tuning ( 7, PidParams.Kp , PidParams.Ki , PidParams.Kd , PidParams.Ka , PidParamsDi.Kp , PidParamsDi.Ki , PidParamsDi.Kd , PidParamsDi.Ka,
                 0.1, 0.1, 0.01, 0.01, 0.1, 0.1, 0.01, 0.01);

// ------------------------------------------------------------------------
// Declaration
// ------------------------------------------------------------------------
int             LoopTimeMsec = 12;
float           LoopTimeMicrosec = LoopTimeMsec * 1000;
unsigned long   ProgStartTime;  //general Start Time
const int       StartDelay = 20000; // msec
unsigned long   CheckTimeStart;
int             CheckTimeEnd;

boolean          Running        = false;     // Speed Control is running
float            TuningValue;
float            SetpointA = 0;
float            SetpointB = 0;
float            setPoint = 0;
float            Angle    = 0; // Sensor Aquisition
float            Calibration = -3.7;
float            Voltage;
float            error ;
float            average;

volatile bool    mpuInterrupt = false;  // indicates whether MPU interrupt pin has gone high
volatile boolean PlotterFlag;           // Interrupt serieller Plotte
volatile boolean RunFlag;               // Interrupt Vicle run
volatile boolean LcdFlag;               // Interrupt LCD Display
volatile int     PositionA;
volatile int     PositionB;
boolean          First = true;

uint32_t         duty;
uint32_t         period;
uint32_t         Speed ;


/**********************************************************************/
void setup()
/**********************************************************************/
{
  ProgStartTime = millis();

  // Serial.begin(9600);  // initialize serial communication
  Serial.begin  (115200);
  while (!Serial); //

  Serial1.begin  (9600);  // Bluetooth

  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

  Serial.println(F("Setup..."));

  // LCD initialisieren
  lcd.begin(16, 2);   lcd.clear( );

  pinMode(PinSleepSwitch, INPUT_PULLUP);

  // initialize device
  Serial.println(F("Initializing I2C devices..."));

  Robot.changePIDparams(PidParams); // PID Parameter setzen

  digitalWrite(PinSleepA, LOW);
  digitalWrite(PinSleepB, LOW);

  // Setup PWM Once (Up to two unique frequencies allowed
  //-----------------------------------------------------
  pwm.pinFreq( PinStepMotorA, rechterMotor);  // Pin 6 freq set to "pwm_freq1" on clock A
  pwm.pinFreq( PinStepMotorB, linkerMotor);  // Pin 7 freq set to "pwm_freq2" on clock B

  // Write PWM Duty Cycle Anytime After PWM Setup
  //-----------------------------------------------------
  uint32_t pwm_duty = 127; // 50% duty cycle
  pwm.pinDuty( PinStepMotorA, pwm_duty );  // 50% duty cycle on Pin 6
  pwm.pinDuty( PinStepMotorB, pwm_duty );  // 50% duty cycle on Pin 7

  /**********************************************************************/
  // for Checking the position from Stepper Motor
  attachInterrupt(digitalPinToInterrupt(PinStepMotorA), ISR_PWMA, RISING );
  attachInterrupt(digitalPinToInterrupt(PinStepMotorB), ISR_PWMB, RISING );
  /**********************************************************************/
  /* Timer Library fully implemented for Arduino DUE
    https://github.com/ivanseidel/DueTimer
    To call a function handler every 1000 microseconds:
    Timer3.attachInterrupt(handler).start(1000);
    or:
    Timer3.attachInterrupt(handler).setPeriod(1000).start();
    or, to select whichever available timer:
    Timer.getAvailable().attachInterrupt(handler).start(1000);
    To call a function handler 10 times a second:
    Timer3.attachInterrupt(handler).setFrequency(10).start();
  */
  /* Dispatching taks for Plotter, LCD Display and Robot
  */
  Timer4.attachInterrupt(PlotterISR).setPeriod(8000).start(); // Plotter
  Timer3.attachInterrupt(LcdTimer).setPeriod(500000).start(); // LCD Display 500 msec
  Timer6.attachInterrupt(RobotFlag).setPeriod(LoopTimeMicrosec).start(); // RobotFlag  10 msec
  /**********************************************************************/
  Angle = 0;
  Speed = 0;
  duty = period / 2;
  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
  pinMode(MpuIntPin, OUTPUT);

  Robot.init(  );  // Init Robot

  MpuInit();       // Init MPU

  //  MyPlot.SetTitle("My Analog Measurement");
  //  MyPlot.SetXlabel("Channel 0");
  //  MyPlot.SetYlabel("Channel 1");
  //  MyPlot.SetSeriesProperties("ADCValue", Plot::Magenta, Plot::Solid, 2, Plot::Square);

}

//---------------------------------------------------------------------/
// LCD Display
//  ---------------------------------------------------------------------*/
void LcdTimer() {
  LcdFlag = true;
}
/**********************************************************************/
// Plotter ISR Routine
/**********************************************************************/
void PlotterISR() {
  PlotterFlag = true;
}
/**********************************************************************/
// Timer6 Robot ISR Routine
/**********************************************************************/
void RobotFlag()  {
  RunFlag = true;
}
// ------------------------------------------------------------------------
/* MPU 6050    ISR Routine
  The FIFO buffer is used together with the interrupt signal.
  If the MPU-6050 places data in the FIFO buffer, it signals the Arduino
  with the interrupt signal so the Arduino knows that there is data in
  the FIFO buffer waiting to be read.*/
void dmpDataReady() {
  mpuInterrupt = true;  // indicates whether MPU interrupt pin has gone high
  digitalWrite(MpuIntPin, !digitalRead(MpuIntPin)); // Toggle  Pin for reading the Frequenzy
}

//---------------------------------------------------------------------/
void ISR_PWMA() {

  if (PidMotorA.DirForward )   {
    // if ( PinDirMotorA == LOW )   {
    PositionA ++;
  } else {
    PositionA --;
  }
}
//---------------------------------------------------------------------/
void ISR_PWMB() {
  if ( PidMotorB.DirForward )   {
    // if ( PinDirMotorB == LOW )   {
    PositionB ++;
  } else {
    PositionB --;
  }
}/**********************************************************************/
void loop()
/**********************************************************************/

{
  if (First) {
    setPoint = 0;
    First = false;
    Angle = 0;
    MotorA.SleepMode();
    MotorB.SleepMode();
    PositionA = 0;
    PositionB = 0;
    if (!( myBattery.VoltageCheck()))  ErrorVoltage(); // Check Voltage of Batterie
  }
  // If PinSleepSwitch is on, release Motors
  if (!digitalRead(PinSleepSwitch)) {
    MotorA.RunMode();
    MotorB.RunMode();
  } else {
    MotorA.SleepMode();
    MotorB.SleepMode();
  }

  // --------------------- Sensor aquisition  -------------------------
  Angle = ReadMpuRunRobot() ; // wait for MPU interrupt or extra packet(s) available
  // --------------------------------------------------------------
  if (!Running) {
    if ( ( abs(Angle) < 15.0 )  && ( millis() > ( ProgStartTime +  StartDelay)))  {
      Running = true; // after Delay time set Running true
      MotorA.RunMode();
      MotorB.RunMode();
    }
  }
  if ( ( abs(Angle) > 15.0 ) && ( Running == true )) {
    ErrorHandler1();
  }
  // --------------------------------------------------------------
  // Run Robot
  // --------------------------------------------------------------
  if ( RunFlag ) {
    RunFlag = false;

    int PositionAtemp; //becaus conflicting declaration 'volatile int PositionA'
    int PositionBtemp;

    manuelPidTuning(); // PID Parameter tuning

    if (Running) {
      PositionAtemp = PositionA;
      PositionBtemp = PositionB;
      Robot.Run( Angle, PositionAtemp , PositionAtemp ); //   Robot.Run


      if (AutoTuning) {
        static int skipT = 0;
        skipT++;
        if (skipT > 10) {
          skipT = 11;
          average = Tuning.next( PidMotorA.dTerm, PidParams.Kp , PidParams.Ki , PidParams.Kd , PidParams.Ka, PidParamsDi.Kp , PidParamsDi.Ki , PidParamsDi.Kd , PidParamsDi.Ka);
          Robot.changePIDparams(PidParams); // PID Parameter setzen
          Robot.changePIDparams(PidParamsDi); // PID Parameter setzen
        }
      }
    }
  }
  // --------------------------------------------------------------
  //  SeriellerPlotter
  // --------------------------------------------------------------

  if ( PlotterFlag ) {
    PlotterFlag = false;
    if (!(AutoTuning))   SeriellerPlotter();
  }

  // --------------------------------------------------------------
  // Show Data via LCD and Check Battery
  // --------------------------------------------------------------
  if (LcdFlag) {
    LcdFlag = false;

    Voltage =  myBattery.Voltage;
    if (!( myBattery.VoltageCheck()))  ErrorVoltage();
    CheckTimeStart = micros();  // Test Timing
    LCD_show();   // Testausgaben LCD
    CheckTimeEnd = ( (micros() - CheckTimeStart)) ;
  }
}
// --------------------------------------------------------------
void  manuelPidTuning()
// --------------------------------------------------------------
// remove comment to get Tuning Value via Poti 10Kohm
{
  TuningValue = (float(analogRead(TuningPin)));
  // manuel Tuning Motor
  // PidParams.Kp = TuningValue  = TuningValue / 50;
  // PidParams.Kd = TuningValue  = TuningValue / 5000;
  // PidParams.Ki   = TuningValue  = TuningValue / 100;
  // PidParams.Ka = TuningValue  = TuningValue / 500;
  // Robot.changePIDparams(PidParams); // PID Parameter setzen


  // manuel Tuning Position
  // PidParamsDi.Ki = TuningValue  = TuningValue / 1000;
  // PidParamsDi.Kp = TuningValue  = TuningValue / 1000;
 //  PidParamsDi.Ka = TuningValue  = TuningValue / 5000;
  // PidParamsDi.Kd = TuningValue  = TuningValue / 1000;
   Robot.changePIDparams(PidParamsDi); // PID Parameter setzen
}
/**********************************************************************/
void ErrorVoltage()
/**********************************************************************/
{
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print( "Akku Low!! "  );
  lcd.setCursor(0, 1);
  lcd.print( myBattery.Voltage );
  lcd.print( " Volt"  );
  do  {} while ( 1 == 1); // never ending
}
// --------------------------------------------------------------
void ErrorHandler1()
// --------------------------------------------------------------
{
  Serial.println(F("Robot tilt!"));
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Robot tilt!");
  lcd.setCursor(0, 1);
  lcd.print("please Restart!");
  MotorA.SleepMode( );
  MotorB.SleepMode( );
  do {} while (1 == 1); // never ending
}
Vehicle.hC/C++
/* 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 "PidControl.h"
#include "PidParameter.h"
#include "Config.h"

/**********************************************************************/
class Vehicle
/**********************************************************************/
{
#define forward HIGH
#define backward LOW
  public:
    Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,
            PidControl * PidMotorA, PidControl * PidMotorB, PidControl * PidDistA,
            PidControl * PidDistB, PidControl * PidSpeed, int iPinSleepSwitch);  // Constructor

    Motor *pMotorA;
    Motor *pMotorB;
    PidControl *pPidMotorA;
    PidControl *pPidMotorB;
    PidControl *pPidDistA;
    PidControl *pPidDistB;
    PidControl *pPidSpeed;

    void Run(float angle,  int PositionA, int PositionB );
    bool BluetoothRead(float  &SetpointA, float &SetpointB, bool &reset, bool &spinning );
    void init();

    void changePIDparams(PidParameter Params);
    void changePIDparams(PidParameterDi Params);

    void resetPIDs( );
    void Stop( );
    //   Microstep Resolution

    float SetpointA = 0.0;
    float SetpointB = 0.0;
    float DeltaPosA = 0.0;
    float DeltaPosB = 0.0;
    float PositionA = 0.0;
    float PositionB = 0.0;
    float SetpointPositionA = 0.0;
    float SetpointPositionB = 0.0;
    float LastPositionA;
    float LastPositionB;

    float StepsA = 0.0;
    float StepsB = 0.0;
    float Speed;
    float SetpointSpeed = 0.0;
    bool  spinning    = false;
    bool  spinningOld = false ;
    bool  StopDriving = false;
    float ABdiff = 0;
    int skipPos; // wait befor starting position controll
    int skipSpeed = 0;


  protected:
    DuePWMmod *ptrpwm;
    int PinSleepSwitch;
    bool firstRun = true;
    // ------------------------------------------------------------------------
    // Reading serial Data
    // ------------------------------------------------------------------------
    String  InString = "";         // a string to hold incoming data
    boolean BTReady = false;  // whether the string is complete

};
#endif
Vehicle.cppC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#include "Vehicle.h"
//#include "Config.h"
/**********************************************************************/
Vehicle::Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,  // Constructor
                 PidControl * PidMotorA, PidControl * PidMotorB, PidControl * PidDistA,
                 PidControl * PidDistB, PidControl * PidSpeed, int iPinSleepSwitch)
/**********************************************************************/
{
  PinSleepSwitch = iPinSleepSwitch;
  pinMode(PinSleepSwitch, INPUT_PULLUP);
  pMotorA    = MotorA;
  pMotorB    = MotorB;
  pPidMotorA = PidMotorA;
  PidMotorB  = PidMotorB;
  pPidDistA  = PidDistA;
  pPidDistB  = PidDistB;
  pPidSpeed  = PidSpeed;
  ptrpwm     = ipwm;
  firstRun   = true;
}
/**********************************************************************/
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) pPidMotorA;
  int ptr4 = (int) pPidMotorB;
  Serial.println(ptr3 , HEX);
  Serial.println(ptr4 , HEX);

  Serial.println("Vehicle Init PID Pointer....");
  int ptr5 = (int) pPidDistA;
  int ptr6 = (int) pPidDistB;
  Serial.println(ptr5 , HEX);
  Serial.println(ptr6 , HEX);

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

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

}
/**********************************************************************/
void Vehicle::Run(float angle,  int iPositionA, int iPositionB )
/**********************************************************************/
{ /*
    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
  */
  const int     tDelay = 10;
  const int     tDelaySpeed = tDelay * 2;
  const float   MaxSpeed = 1.0  ; //1.5;    11.03.19
  static bool   decelerate = false;
  static bool   moving = false;

  /* "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
  */
  PositionA = (float(iPositionA ) / 100 );
  PositionB = (float(iPositionB ) / 100 );

  if (firstRun) {
    firstRun = false;
    skipPos = tDelay - 1;
    LastPositionA = (PositionA); // for the change from driving to standstill remember the last position
    LastPositionB = (PositionB);
  }
  if ( BluetoothRead( DeltaPosA,  DeltaPosB, StopDriving, spinning )) {
    // new Movement value DeltaPosA via Bluetooth
    skipPos = tDelay - 1;
    skipSpeed = tDelay - 1;
  }
  if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be 10 times lower than Motor Control
    skipPos = 0;

    if (StopDriving) {  // Stop driving, Last Current position as target position
      if (abs(DeltaPosA) < 0.001 && abs(DeltaPosA) < 0.001)
      { DeltaPosA = DeltaPosA = 0.0;
        decelerate = false;
      } else
      { // decelerate slowly
        decelerate = true;
        DeltaPosA = DeltaPosA * 0.35;
        DeltaPosB = DeltaPosB * 0.35;
      }
    }
    // a basic distinction must be made between driving and standing on the spot
    // If the robot is not to move, the last position is important as the setpoint value.
    if (abs((DeltaPosA) == 0 && abs(DeltaPosB) == 0 ) || decelerate)
    { // standing on the spot the robot has to use the last position as target value
      SetpointPositionA = LastPositionA;
      SetpointPositionB = LastPositionB;
      SetpointA = constrain( (pPidDistA->calculate (PositionA , SetpointPositionA)), -MaxSpeed, MaxSpeed);
      SetpointB = constrain( (pPidDistB->calculate (PositionB , SetpointPositionB)), -MaxSpeed, MaxSpeed);
      moving = false;

    } else { // driving : the robot has to use the actual position with the setpoint as target value
      SetpointPositionA = PositionA + DeltaPosA;
      SetpointPositionB = PositionA + DeltaPosB;
      SetpointA = constrain( (pPidDistA->calculate (PositionA , SetpointPositionA)), -MaxSpeed, MaxSpeed);
      SetpointB = constrain( (pPidDistB->calculate (PositionB , SetpointPositionB)), -MaxSpeed, MaxSpeed);
      moving = true;
    }
    if (!decelerate) {
      // LastPositionA = (PositionA); // for the change from driving to standstill remember the last position
      // LastPositionB = (PositionB);
    }
    if (moving) {
      LastPositionA = (PositionA); // for the change from driving to standstill remember the last position
      LastPositionB = (PositionB);
    }
  }

  // PID calculation of new steps
  StepsA = pMotorA->Calculate(angle, -SetpointA);
  StepsB = pMotorB->Calculate(angle, -SetpointB);

  //After the robot has moved to the right or left, slight deviations occur during the next straight ride.
  //workaound: when driving straight forwad or backwad(!spinning), the equality of the steps is forced.

  if (!spinning)  {
    ABdiff = StepsA - StepsB;
  }
  // run the Motors, here Steps = full steps
  pMotorA->Run(StepsA );
  pMotorB->Run(StepsB + ABdiff);

  Speed = ( StepsA + StepsB ) / 2;
}

/**********************************************************************/
void Vehicle::Stop() {
  pMotorA->SleepMode( );
  pMotorB->SleepMode( );
}
/**********************************************************************/
void Vehicle::changePIDparams(PidParameter Params) {
  pPidMotorA->changePIDparams(Params);
  pPidMotorB->changePIDparams(Params);
  //  Serial.println("Vehicle PID Params changed! ");
}
void Vehicle::changePIDparams(PidParameterDi Params) {
  pPidDistA->changePIDparams(Params);
  pPidDistB->changePIDparams(Params);
  //  Serial.println("Vehicle PID Params changed! ");
}

/**********************************************************************/
bool  Vehicle::BluetoothRead(float  & iSetpointA, float & iSetpointB , bool & StopDriving, bool & spinning) {
  char inChar;
  const float Diff   = 0.8;
  const float Diffrl = 0.05;
  // const bool StopDriving;

  // reply only when you receive data:
  if (Serial1.available() > 0) {
    // read the incoming byte:
    //  incomingByte = Serial1.read();
    inChar = ((char)Serial1.read());
    if (isAlphaNumeric(!(inChar))) {
      Serial.println("ilegal Command");
      return false;
    }
    // say what you got:
    Serial1.print("2 received: ");
    Serial1.println(inChar);
    if (inChar == '1') {
      iSetpointA += Diff;
      iSetpointB += Diff;
      StopDriving = false;
      spinning = false;
    }
    else if (inChar == '2') {
      iSetpointA -= Diff;
      iSetpointB -= Diff;
      StopDriving = false;
      spinning = false;
    }
    else if (inChar == '3') {
      iSetpointA += Diffrl;
      iSetpointB -= Diffrl;
      StopDriving = false;
      spinning = true;
    }
    else if (inChar == '4') {
      iSetpointA -= Diffrl;
      iSetpointB += Diffrl;
      StopDriving = false;
      spinning = true;
    } else if (inChar == 'S') {
      // iSetpointA = 0.0;
      // iSetpointB = 0.0;
      StopDriving = true;
      spinning = false;
    } else {
      inChar = ' ';
      return false;
      Serial.println("ilegal Command");
    }
    /* Serial.print("Pos A : ");
       Serial.print(iSetpointA);
       Serial.print("Pos B : ");
       Serial.println(iSetpointB); */
    return true;
  }
  return false;
}
Twiddle.hC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing
    written by : Rolf Kurth in 2019
    rolf.kurth@cron-consulting.de
*/
#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
Twiddle.cppC/C++
/* 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]  , 2 );
  Serial.print(" P1 ");
  Serial.print(params[1]  , 2);
  Serial.print(" P2 ");
  Serial.print(params[2]  , 2 );
  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(" ");
}
PidParameter.hC/C++
/* 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.3 ;
  float          Ki = 6.01;
  float          Kd = 0.12 ;
  float          Ka = 0.1246;
  float          Kx = 0.0; //
};
// PidParameter Position
struct PidParameterDi {
  float          K = 1.0;
  float          Kp = 0.18;
  float          Ki = 0.0;
  float          Kd = 0.4 ;
  float          Ka = 0.0 ;
  float          Kx = 0.0;
};
// PidParameter Position
struct PidParamsSpeed { //not yet used
  float          K = 1.0;
  float          Kp = 1.0;
  float          Ki = 1.0;
  float          Kd = 0.0 ;
  float          Ka = 0.0 ;
  float          Kx = 0.0;
};
#endif
PidControl.hC/C++
/* 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(PidParameterDi    Params) ;
    PidControl(PidParamsSpeed    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(PidParameterDi Params);
    void     changePIDparams(PidParamsSpeed Params);

    float    calculate (float iAngle, float isetPoint );
    void     test      ( );
    float    getdTerm ();
    float    getiTerm ();
    float    getpTerm ();
    float    getSteps ();
   float    setdTerm (float dTerm);
    float    setiTerm (float dTerm);
    float    setpTerm (float dTerm);
    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
PidControl.cppC/C++
/* 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 (PidParameterDi 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 (PidParamsSpeed 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 , -120.0 , 120.0 ); // 10 Steps per Second because Microstep
  if (eSpeed > 0 ) {
    DirForward = true ;
  } else {
    DirForward = false;
  }
  return eSpeed;  // Steps per Second
}

/**********************************************************************/
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 (PidParameterDi Params)
// changePIDparams = different PidParameter !!!!
/**********************************************************************/
{ // different PidParameter !!!!
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
}
/**********************************************************************/
void PidControl::changePIDparams (PidParamsSpeed 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::getdTerm () {
  return dTerm;
}
float PidControl::getiTerm () {
  return iTerm;
}
float PidControl::getpTerm () {
  return pTerm;
}
float PidControl::getSteps () {
  return eSpeed;
}

float PidControl::setdTerm ( float idterm) {
  dTerm = idterm ;
}
float PidControl::setiTerm ( float iiterm) {
  iTerm = iiterm ;
}
float PidControl::setpTerm ( float ipterm) {
  pTerm = ipterm ;
}
Motor.hC/C++
/* 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, PidControl* Pid,  int iPinDir, int iPinStep,
          int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide);

    struct PidParameter params;
    PidControl *ptrPid;
    void init() ;
    Motor* getInstance();
    void SleepMode ( );
    void RunMode ( );
    void toggleMode ( );
    float Calculate(float angle, float Setpoint);
    void 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;
    float      Steps;
    float      SetpointTmp;
    float     Steps_tmp;
    char _MotorSide;
    DuePWMmod *ptrpwm;


  private:
    bool MotorMode;
    int ptr;
};
#endif
Motor.cppC/C++
/* 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, PidControl * Pid, 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;
  // WrapperMode = true;
  ptrPid = Pid;
  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();

  ptrPid->changePIDparams( params);
}
/**********************************************************************/
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::Calculate(float angle, float Setpoint)
/**********************************************************************/
{
  Steps = ptrPid->calculate (angle, Setpoint );

}
/**********************************************************************/
void Motor::Run(float Steps) {
  if (Steps >= 0 ) {
    if (_MotorSide == rechterMotor) {
      digitalWrite(_PinDir, LOW);
    }
    else {
      digitalWrite(_PinDir, HIGH);
    }
  } else
  {
    if (_MotorSide == rechterMotor) {
      digitalWrite(_PinDir, HIGH);
    }
    else {
      digitalWrite(_PinDir, LOW);
    }
  }

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

  Steps_tmp = Steps;
  Steps = abs(Steps_tmp);

  if ( Steps < 2.0) {
    Steps = 2; // Microsteps
  }

  if (_MotorSide == rechterMotor) {
    ptrpwm->setFreq( Steps, rechterMotor  );
  }
  else {
    ptrpwm->setFreq( Steps, linkerMotor  );
  }
}
/**********************************************************************/
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;
  //  Serial.println("MsMicrostep");
}
DueTimer.hC/C++
/*
  DueTimer.h - DueTimer header file, definition of methods and attributes...
  For instructions, go to https://github.com/ivanseidel/DueTimer

  Created by Ivan Seidel Gomes, March, 2013.
  Modified by Philipp Klaus, June 2013.
  Released into the public domain.
*/

#ifdef __arm__

#ifndef DueTimer_h
#define DueTimer_h

#include "Arduino.h"

#include <inttypes.h>

/*
  This fixes compatibility for Arduono Servo Library.
  Uncomment to make it compatible.

  Note that:
    + Timers: 0,2,3,4,5 WILL NOT WORK, and will
          neither be accessible by Timer0,...
*/
// #define USING_SERVO_LIB  true

#ifdef USING_SERVO_LIB
  #warning "HEY! You have set flag USING_SERVO_LIB. Timer0, 2,3,4 and 5 are not available"
#endif


#define NUM_TIMERS  9

class DueTimer
{
protected:

  // Represents the timer id (index for the array of Timer structs)
  const unsigned short timer;

  // Stores the object timer frequency
  // (allows to access current timer period and frequency):
  static float  _frequency[NUM_TIMERS];

  // Picks the best clock to lower the error
  static uint8_t bestClock(float  frequency, uint32_t& retRC);

  // Make Interrupt handlers friends, so they can use callbacks
  friend void TC0_Handler(void);
  friend void TC1_Handler(void);
  friend void TC2_Handler(void);
  friend void TC3_Handler(void);
  friend void TC4_Handler(void);
  friend void TC5_Handler(void);
  friend void TC6_Handler(void);
  friend void TC7_Handler(void);
  friend void TC8_Handler(void);

  static void (*callbacks[NUM_TIMERS])();

  struct Timer
  {
    Tc *tc;
    uint32_t channel;
    IRQn_Type irq;
  };

  // Store timer configuration (static, as it's fixed for every object)
  static const Timer Timers[NUM_TIMERS];

public:

  static DueTimer getAvailable(void);

  DueTimer(unsigned short _timer);
  DueTimer& attachInterrupt(void (*isr)());
  DueTimer& detachInterrupt(void);
  DueTimer& start(float  microseconds = -1);
  DueTimer& stop(void);
  DueTimer& setFrequency(float  frequency);
  DueTimer& setPeriod(float  microseconds);

  float  getFrequency(void) const;
  float  getPeriod(void) const;
};

// Just to call Timer.getAvailable instead of Timer::getAvailable() :
extern DueTimer Timer;

extern DueTimer Timer1;
// Fix for compatibility with Servo library
#ifndef USING_SERVO_LIB
  extern DueTimer Timer0;
  extern DueTimer Timer2;
  extern DueTimer Timer3;
  extern DueTimer Timer4;
  extern DueTimer Timer5;
#endif
extern DueTimer Timer6;
extern DueTimer Timer7;
extern DueTimer Timer8;

#endif

#else
  #error Oops! Trying to include DueTimer on another device!?
#endif
DueTimer.cppC/C++
/*
  DueTimer.cpp - Implementation of Timers defined on DueTimer.h
  For instructions, go to https://github.com/ivanseidel/DueTimer

  Created by Ivan Seidel Gomes, March, 2013.
  Modified by Philipp Klaus, June 2013.
  Thanks to stimmer (from Arduino forum), for coding the "timer soul" (Register stuff)
  Released into the public domain.
*/

#include <Arduino.h>
#if defined(_SAM3XA_)
#include "DueTimer.h"

const DueTimer::Timer DueTimer::Timers[NUM_TIMERS] = {
  {TC0, 0, TC0_IRQn},
  {TC0, 1, TC1_IRQn},
  {TC0, 2, TC2_IRQn},
  {TC1, 0, TC3_IRQn},
  {TC1, 1, TC4_IRQn},
  {TC1, 2, TC5_IRQn},
  {TC2, 0, TC6_IRQn},
  {TC2, 1, TC7_IRQn},
  {TC2, 2, TC8_IRQn},
};

// Fix for compatibility with Servo library
#ifdef USING_SERVO_LIB
// Set callbacks as used, allowing DueTimer::getAvailable() to work
void (*DueTimer::callbacks[NUM_TIMERS])() = {
  (void (*)()) 1, // Timer 0 - Occupied
  (void (*)()) 0, // Timer 1
  (void (*)()) 1, // Timer 2 - Occupied
  (void (*)()) 1, // Timer 3 - Occupied
  (void (*)()) 1, // Timer 4 - Occupied
  (void (*)()) 1, // Timer 5 - Occupied
  (void (*)()) 0, // Timer 6
  (void (*)()) 0, // Timer 7
  (void (*)()) 0  // Timer 8
};
#else
void (*DueTimer::callbacks[NUM_TIMERS])() = {};
#endif
float  DueTimer::_frequency[NUM_TIMERS] = { -1, -1, -1, -1, -1, -1, -1, -1, -1};

/*
  Initializing all timers, so you can use them like this: Timer0.start();
*/
DueTimer Timer(0);

DueTimer Timer1(1);
// Fix for compatibility with Servo library
#ifndef USING_SERVO_LIB
DueTimer Timer0(0);
DueTimer Timer2(2);
DueTimer Timer3(3);
DueTimer Timer4(4);
DueTimer Timer5(5);
#endif
DueTimer Timer6(6);
DueTimer Timer7(7);
DueTimer Timer8(8);

DueTimer::DueTimer(unsigned short _timer) : timer(_timer) {
  /*
    The constructor of the class DueTimer
  */
}

DueTimer DueTimer::getAvailable(void) {
  /*
    Return the first timer with no callback set
  */

  for (int i = 0; i < NUM_TIMERS; i++) {
    if (!callbacks[i])
      return DueTimer(i);
  }
  // Default, return Timer0;
  return DueTimer(0);
}

DueTimer& DueTimer::attachInterrupt(void (*isr)()) {
  /*
    Links the function passed as argument to the timer of the object
  */

  callbacks[timer] = isr;

  return *this;
}

DueTimer& DueTimer::detachInterrupt(void) {
  /*
    Links the function passed as argument to the timer of the object
  */

  stop(); // Stop the currently running timer

  callbacks[timer] = NULL;

  return *this;
}

DueTimer& DueTimer::start(float  microseconds) {
  /*
    Start the timer
    If a period is set, then sets the period and start the timer
  */

  if (microseconds > 0)
    setPeriod(microseconds);

  if (_frequency[timer] <= 0)
    setFrequency(1);

  NVIC_ClearPendingIRQ(Timers[timer].irq);
  NVIC_EnableIRQ(Timers[timer].irq);

  TC_Start(Timers[timer].tc, Timers[timer].channel);

  return *this;
}

DueTimer& DueTimer::stop(void) {
  /*
    Stop the timer
  */

  NVIC_DisableIRQ(Timers[timer].irq);

  TC_Stop(Timers[timer].tc, Timers[timer].channel);

  return *this;
}

uint8_t DueTimer::bestClock(float  frequency, uint32_t& retRC) {
  /*
    Pick the best Clock, thanks to Ogle Basil Hall!

    Timer   Definition
    TIMER_CLOCK1  MCK /  2
    TIMER_CLOCK2  MCK /  8
    TIMER_CLOCK3  MCK / 32
    TIMER_CLOCK4  MCK /128
  */
  const struct {
    uint8_t flag;
    uint8_t divisor;
  } clockConfig[] = {
    { TC_CMR_TCCLKS_TIMER_CLOCK1,   2 },
    { TC_CMR_TCCLKS_TIMER_CLOCK2,   8 },
    { TC_CMR_TCCLKS_TIMER_CLOCK3,  32 },
    { TC_CMR_TCCLKS_TIMER_CLOCK4, 128 }
  };
  float ticks;
  float error;
  int clkId = 3;
  int bestClock = 3;
  float bestError = 9.999e99;
  do
  {
    ticks = (float) VARIANT_MCK / frequency / (float) clockConfig[clkId].divisor;
    // error = abs(ticks - round(ticks));
    error = clockConfig[clkId].divisor * abs(ticks - round(ticks)); // Error comparison needs scaling
    if (error < bestError)
    {
      bestClock = clkId;
      bestError = error;
    }
  } while (clkId-- > 0);
  ticks = (float) VARIANT_MCK / frequency / (float) clockConfig[bestClock].divisor;
  retRC = (uint32_t) round(ticks);
   return clockConfig[bestClock].flag;  //Rolf
  
}


DueTimer& DueTimer::setFrequency(float  frequency) {
  /*
    Set the timer frequency (in Hz)
  */

  // Prevent negative frequencies
  if (frequency <= 0) {
    frequency = 1;
  }

  // Remember the frequency  see below how the exact frequency is reported instead
  //_frequency[timer] = frequency;

  // Get current timer configuration
  Timer t = Timers[timer];

  uint32_t rc = 0;
  uint8_t clock;

  // Tell the Power Management Controller to disable
  // the write protection of the (Timer/Counter) registers:
  pmc_set_writeprotect(false);

  // Enable clock for the timer
  pmc_enable_periph_clk((uint32_t)t.irq);

  // Find the best clock for the wanted frequency
  clock = bestClock(frequency, rc);

  switch (clock) {
    case TC_CMR_TCCLKS_TIMER_CLOCK1:
      _frequency[timer] = (float )VARIANT_MCK / 2.0 / (float )rc;
      break;
    case TC_CMR_TCCLKS_TIMER_CLOCK2:
      _frequency[timer] = (float )VARIANT_MCK / 8.0 / (float )rc;
      break;
    case TC_CMR_TCCLKS_TIMER_CLOCK3:
      _frequency[timer] = (float )VARIANT_MCK / 32.0 / (float )rc;
      break;
    default: // TC_CMR_TCCLKS_TIMER_CLOCK4
      _frequency[timer] = (float )VARIANT_MCK / 128.0 / (float )rc;
      break;
  }

  // Set up the Timer in waveform mode which creates a PWM
  // in UP mode with automatic trigger on RC Compare
  // and sets it up with the determined internal clock as clock input.
  TC_Configure(t.tc, t.channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | clock);
  // Reset counter and fire interrupt when RC value is matched:
  TC_SetRC(t.tc, t.channel, rc);
  // Enable the RC Compare Interrupt...
  t.tc->TC_CHANNEL[t.channel].TC_IER = TC_IER_CPCS;
  // ... and disable all others.
  t.tc->TC_CHANNEL[t.channel].TC_IDR = ~TC_IER_CPCS;

  return *this;
}

DueTimer& DueTimer::setPeriod(float  microseconds) {
  /*
    Set the period of the timer (in microseconds)
  */

  // Convert period in microseconds to frequency in Hz
  float  frequency = 1000000.0 / microseconds;
  setFrequency(frequency);
  return *this;
}

float  DueTimer::getFrequency(void) const {
  /*
    Get current time frequency
  */

  return _frequency[timer];
}

float  DueTimer::getPeriod(void) const {
  /*
    Get current time period
  */

  return 1.0 / getFrequency() * 1000000;
}


/*
  Implementation of the timer callbacks defined in
  arduino-1.5.2/hardware/arduino/sam/system/CMSIS/Device/ATMEL/sam3xa/include/sam3x8e.h
*/
// Fix for compatibility with Servo library
#ifndef USING_SERVO_LIB
void TC0_Handler(void) {
  TC_GetStatus(TC0, 0);
  DueTimer::callbacks[0]();
}
#endif
void TC1_Handler(void) {
  TC_GetStatus(TC0, 1);
  DueTimer::callbacks[1]();
}
// Fix for compatibility with Servo library
#ifndef USING_SERVO_LIB
void TC2_Handler(void) {
  TC_GetStatus(TC0, 2);
  DueTimer::callbacks[2]();
}
void TC3_Handler(void) {
  TC_GetStatus(TC1, 0);
  DueTimer::callbacks[3]();
}
void TC4_Handler(void) {
  TC_GetStatus(TC1, 1);
  DueTimer::callbacks[4]();
}
void TC5_Handler(void) {
  TC_GetStatus(TC1, 2);
  DueTimer::callbacks[5]();
}
#endif
void TC6_Handler(void) {
  TC_GetStatus(TC2, 0);
  DueTimer::callbacks[6]();
}
void TC7_Handler(void) {
  TC_GetStatus(TC2, 1);
  DueTimer::callbacks[7]();
}
void TC8_Handler(void) {
  TC_GetStatus(TC2, 2);
  DueTimer::callbacks[8]();
}
#endif
DuePWMmod.hC/C++
/* DuePWMmod

  Library:     pwm01.h (version 01)
  Date:        2/11/2013
  Written By:  randomvibe
  modified by : Rolf Kurth

  Purpose:
   Setup one or two unique PWM frequenices directly in sketch program,
   set PWM duty cycle, and stop PWM function.

  Notes:
   - Applies to Arduino-Due board, PWM pins 6, 7, 8 & 9, all others ignored
   - Libary Does not operate on the TIO pins.
   - Unique frequencies set via PWM Clock-A ("CLKA") and Clock-B ("CLKB")
     Therefore, up to two unique frequencies allowed.
   - Set max duty cycle counts (pwm_max_duty_Ncount) equal to 255
     per Arduino approach.  This value is best SUITED for low frequency
     applications (2hz to 40,000hz) such as PWM motor drivers,
     38khz infrared transmitters, etc.
   - Future library versions will address high frequency applications.
   - Arduino's "wiring_analog.c" function was very helpful in this effort.

*/

#ifndef DuePWMmod_h
#define DuePWMmod_h
#define rechterMotor 'A'
#define linkerMotor 'B'

#include <Arduino.h>

class DuePWMmod
{
  public:
    DuePWMmod();

    // MAIN PWM INITIALIZATION
    //--------------------------------
    void  pinFreq( uint32_t  pin, char ClockAB );

    // WRITE DUTY CYCLE
    //--------------------------------
    void  pinDuty( uint32_t  pin,  uint32_t  duty );

    //void pwm_set_resolution(int res);
    void  setFreq(uint32_t  clock_freq, char ClockAB);

    void  DisableChannel( uint32_t  pin );
    void  EnableChannel( uint32_t  pin );

  protected:
    uint32_t  pwm_clockA_freq;
    uint32_t  pwm_clockB_freq;
  private:
    static  const uint32_t  pwm_max_duty_Ncount = 255;
};
#endif
DuePWMmod.cppC/C++
/* DuePWMmod
  Purpose:
   Setup one or two unique PWM frequenices directly in sketch program,
   set PWM duty cycle, and stop PWM function.
   modified by : Rolf Kurth
*/

#include "DuePWMmod.h"
#include "Arduino.h"


DuePWMmod::DuePWMmod()
{
  /*
    uint32_t pmc_enable_periph_clk  ( uint32_t  ul_id )
    Enable the specified peripheral clock.
    Note
    The ID must NOT be shifted (i.e., 1 << ID_xxx).
    Parameters
    ul_id  Peripheral ID (ID_xxx).
    Return values
    0 Success.
    1 Invalid parameter.
    http://asf.atmel.com/docs/latest/sam3x/html/group__sam__drivers__pmc__group.html#gad09de55bb493f4ebdd92305f24f27d62
  */
  pmc_enable_periph_clk( PWM_INTERFACE_ID );
}

// MAIN PWM INITIALIZATION
//--------------------------------
void  DuePWMmod::pinFreq( uint32_t  pin, char ClockAB )
{

  /*
    uint32_t pio_configure  ( Pio *   p_pio, const pio_type_t  ul_type, const uint32_t  ul_mask, const uint32_t  ul_attribute)    )
    Perform complete pin(s) configuration; general attributes and PIO init if necessary.
    Parameters
    p_pio Pointer to a PIO instance.
    ul_type PIO type.
    ul_mask Bitmask of one or more pin(s) to configure.
    ul_attribute  Pins attributes.
    Returns
    Whether the pin(s) have been configured properly.
    http://asf.atmel.com/docs/latest/sam3x/html/group__sam__drivers__pio__group.html#gad5f0174fb8a14671f06f44042025936e
  */
  uint32_t  chan = g_APinDescription[pin].ulPWMChannel;

  PIO_Configure( g_APinDescription[pin].pPort,  g_APinDescription[pin].ulPinType,
                 g_APinDescription[pin].ulPin,  g_APinDescription[pin].ulPinConfiguration);
  if ( ClockAB == rechterMotor) {
    PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
  }
  if ( ClockAB == linkerMotor) {
    PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKB, 0, 0);
  }
  PWMC_SetPeriod(PWM_INTERFACE, chan, pwm_max_duty_Ncount);
  PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);  // The 0 is the initial duty cycle
  PWMC_EnableChannel(PWM_INTERFACE, chan);
}

void DuePWMmod::setFreq(uint32_t  clock_freq, char ClockAB)
{
  if ( ClockAB == rechterMotor) {
    pwm_clockA_freq = pwm_max_duty_Ncount * clock_freq;
  }
  if ( ClockAB == linkerMotor) {
    pwm_clockB_freq =  pwm_max_duty_Ncount * clock_freq;
  }

  /// void PWMC_ConfigureClocks(unsigned int clka, unsigned int clkb, unsigned int mck)
  /// Configures PWM clocks A & B to run at the given frequencies. This function
  /// finds the best MCK divisor and prescaler values automatically.
  /// \param clka  Desired clock A frequency (0 if not used).
  /// \param clkb  Desired clock B frequency (0 if not used).
  /// \param mck  Master clock frequency.
  // #include "pwmc.h"

  PWMC_ConfigureClocks( pwm_clockA_freq, pwm_clockB_freq, VARIANT_MCK );
}

// WRITE DUTY CYCLE
//--------------------------------
void  DuePWMmod::pinDuty( uint32_t  pin,  uint32_t  duty )
{
  PWMC_SetDutyCycle(PWM_INTERFACE, g_APinDescription[pin].ulPWMChannel, duty);
}


//--------------------------------
void  DuePWMmod::EnableChannel( uint32_t  pin )
//Enable the specified PWM channel.
// PWM_INTERFACE = Module hardware register base address pointer
// g_APinDescription[pin].ulPWMChannel = PWM channel number
{
  PWMC_EnableChannel (PWM_INTERFACE, g_APinDescription[pin].ulPWMChannel);
}
void  DuePWMmod::DisableChannel( uint32_t  pin )
//Disable the specified PWM channel.
// PWM_INTERFACE = Module hardware register base address pointer
// g_APinDescription[pin].ulPWMChannel = PWM channel number
{
  PWMC_DisableChannel (PWM_INTERFACE, g_APinDescription[pin].ulPWMChannel);
}
Config.hC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing  
 *  written by : Rolf Kurth in 2019
 *  rolf.kurth@cron-consulting.de
 */
#ifndef Config_h
#define  Config_h
#include "Arduino.h"

/* PIN Belegung
   D2  Interrupt 0 for MPU6050
   D4  LiquidCrystal DB4
   D5  LiquidCrystal DB5
   D9  LiquidCrystal rs grau
   D8  LiquidCrystal enable lila
   D10 LiquidCrystal DB6 gelb >
   D11 LiquidCrystal DB7 orange >
   D18 BlueThooth ????
   D19 BlueThooth ????+
   D36 InterruptStartButton
   D42 Test Pin
   D44 Test Pin
 
   D36 Motor A Direction
   D34 Motor A Step
   D40 Motor B Direction
   D42 Motor B Step

   D28 PinMs1MotorA
   D30 PinMs2MotorA
   D46 PinMs1MotorB
   D48 PinMs2MotorB

   D22 Sleep MotorA
   D24 Sleep MotorA
   D53 Sleep Switch Input 

   A8  Spannung VoltPin
   A7  Tuning Poti 10Kohm

  LiquidCrystal(rs, enable, d4, d5, d6, d7)
  LiquidCrystal lcd(9, 8, 4, 5, 10, 11);
    10K resistor:
    ends to +5V and ground
    wiper (3) to LCD VO pin
*/

#define MpuInterruptPin 2       // use pin on Arduino for MPU Interrupt
#define LED_PIN 13
#define MpuIntPin 27            //the interrupt is used to determine when new data is available.



const int  PinMs1MotorA  = 28;
const int  PinMs2MotorA  = 30;
const int  PinMs1MotorB  = 46;
const int  PinMs2MotorB  = 48;
const int  PinDirMotorA  = 36;
const int  PinDirMotorB  = 40;
const int  PinStepMotorA  = 6;
const int  PinStepMotorB  = 7;
const int  PinSleepSwitch = 53;
const int  PinSleepA      = 22;
const int  PinSleepB      = 24;
const int  VoltPin        = A6; // Voltage VIN
const int  TuningPin      = A7; //Potentiometer



#endif
Battery.hC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing  
 *  written by : Rolf Kurth in 2019
 *  rolf.kurth@cron-consulting.de
 */
 
#ifndef Battery_h
#define  Battery_h
#include "Arduino.h"
//********************************************************************** /
class Battery
///**********************************************************************/
{
  public:
    Battery(int PIN)  ;  // Constructor
    //   Battery(int _Pin)  ;  // Constructor
    bool    VoltageCheck();
    float   VoltageRead();
    float   Voltage;
    int     _VoltPin = 6;
};
#endif
Battery.cppC/C++
/* Self balancing Robot via Stepper Motor with microstepping and Digital Motion Processing  
 *  written by : Rolf Kurth in 2019
 *  rolf.kurth@cron-consulting.de
 */
 
 #include "Battery.h"
#include <Arduino.h>

/**********************************************************************/
Battery::Battery(int PIN)
/**********************************************************************/
{
  _VoltPin = PIN;
  pinMode(_VoltPin, INPUT);
}

// -----------------------------------------------------------------------------
float Battery::VoltageRead()
// --------------------------------------------------------------
{
  Voltage = ( analogRead(_VoltPin)) * 0.018 ;
  /*Serial.print("Voltage ");
    Serial.print(Voltage);
    Serial.println(VoltPin );*/
  return Voltage;
}
// -----------------------------------------------------------------------------
bool Battery::VoltageCheck()
// --------------------------------------------------------------
{
 // Lipo Nennspannung von 3,7 Volt 
 // die Spannung der einzelnen Zellen nicht unter 3 Volt sinkt.
  // voltage divider:
  // 21 Kohm 4,7 Kohm maximal 12 Volt
  // Uinp = U * R1 / ( R1 + R2 ) = 0,183 * U
  // 1024 (resolution)
  // analogRead(VoltPin)) * 5.0VoltRef / 1024.0  / 0.183 = 0.027
  // in case of Resistor toleranz  0.0225

  //  Voltage = ( analogRead(VoltPin)) * 0.0225 ;
  Voltage = VoltageRead();

  if (Voltage > 4.8 &&  Voltage < 6.4 ) {
    return false;
  }
  return true;
}
LCD.inoC/C++
/* 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("A        ");
    lcd.setCursor(2, 1);
    lcd.print(Angle, 2);
    lcd.setCursor(8, 1);
    lcd.print("Sp     ");
    lcd.setCursor(11, 1);
    lcd.print(PidMotorA.getSteps () / 8);
    lcd.setCursor(15, 1);
    //lcd.print(MyMPU6050.FifoOverflowCnt);

  } else { // it is Init Mode

    delay = (( millis() -  ProgStartTime ) ) ;
    lcd.setCursor(0, 1);
    lcd.print("A      ");
    lcd.setCursor(2, 1);
    lcd.print(Angle, 2);
    lcd.setCursor(8, 1);
    lcd.print("Av      ");
  }
}
// --------------------------------------------------------------
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, 2);
    //    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");
  }
}
MyMPU.inoC/C++
/* 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>
*/
// ------------------------------------------------------------------------
// 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
int      FifoOverflowCnt;

// --------------------- Sensor aquisition  -------------------------
float 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;
    Angle = ReadMpuRunRobot6050() + Calibration;
    // blinkState = !blinkState;
    digitalWrite(LED_PIN, LOW);
    //  if (Running)         Robot.Run( Angle, SetpointA, SetpointB ); //   Robot.Run( Angle, Speed, Speed );
  }
  return Angle ;
}
// -------------------------------------------------------------------- -
float ReadMpuRunRobot6050()
// -------------------------------------------------------------------- -
{
  static float Winkel_old;
  static float Winkel;

  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;
    return Winkel_old;

  }
  // 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);

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

    //   Winkel = (0.9 * Winkel) + (0.1 * Winkel_old);
  }
  /*
    if (!Running) {
      CalibrationAngle = ( 0.95 * CalibrationAngle ) + ( 0.05 * Winkel );  // low-pass filter
    }
  */
  Winkel_old = Winkel ;
  return  Winkel ;
}
// --------------------------------------------------------------
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);
  }
}
Plotter.inoC/C++
/* 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()
// --------------------------------------------------------------
{


  /*   Serial.print(Angle , 2 );
     Serial.print(",  ");
     Serial.print(PidMotorA.pTerm , 2 );
     Serial.print(",  ");
     Serial.print(PidMotorA.iTerm , 2 );
     Serial.print(",  ");
     Serial.print(PidMotorA.dTerm , 2 );
     Serial.print(",  ");
     Serial.print(MotorA.Steps_tmp / 150 ); //Microsteps /10

     Serial.println(" ");
  */

  /*   Serial.print(Angle , 2 );
     Serial.print(",  ");
     Serial.print(Robot.SetpointA );
     Serial.print(",  ");
     Serial.print(Robot.SetpointAPos - Robot.PositionA  );
     Serial.print(",  ");
     Serial.print(PidDistA.pTerm );
     Serial.print(",  ");
     Serial.print(PidDistA.iTerm );
     Serial.print(",  ");
     Serial.print(Robot.StepsA / 150 ); //Microsteps /10
  */

  // 1 blau 2 rot 3 grn 4 gelb 5 lila 6 grau
  Serial.print(Angle * 10 , 2 );
  Serial.print(",  ");
  Serial.print(Robot.DeltaPosA, 2 );
  Serial.print(",  ");
  Serial.print(Robot.SetpointA * 10, 2 );
  Serial.print(",  ");
  Serial.print(Robot.SetpointPositionA * 10, 2 );
  Serial.print(",  ");
  Serial.print(Robot.PositionA, 2 );
  Serial.print(",  ");
  Serial.print(Robot.StepsA / 10); //Microsteps /10);
  Serial.print(", ");

  Serial.println(" ");



}

Schematics

Fritzing
Fritzing 1s7fu5ju2i

Comments

Similar projects you might like

CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Project in progress by Pigeon-Kicker

  • 1,603 views
  • 0 comments
  • 8 respects

Arduino Balancing Robot

Project showcase by TEAM DIY

  • 3,236 views
  • 0 comments
  • 7 respects

PHPoC - Arduino Self Balancing Robot with BT+Web Control

Project in progress by Suyog Gunjal

  • 3,999 views
  • 2 comments
  • 24 respects

Self balancing robot

Project showcase by Stephan Schultz

  • 10,136 views
  • 4 comments
  • 41 respects
Add projectSign up / Login