Project in progress
Two wheeled self balancing robot (redesign)

Two wheeled self balancing robot (redesign) © GPL3+

Robot wirth stepper motor via microstepping, digital motion processing, auto tuning, cascaded PID controller with Joy Stick Control.

  • 2,989 views
  • 0 comments
  • 8 respects

Components and supplies

About this project

I redesigned the robot. The hardware has not changed, but the software has been redesigned.

In the old version there was a separate PID controller for each motor.

To steer the robot to the right or left, the setpoint of the PID controller was assigned by the appropriate value.

If the robot should drive straight ahead again afterwards, the resulting difference in steps had to be artificially neutralized and this was sometime the reason for some trouble.

Now the robot has only one PID controller for both motors. This is sufficient to keep the robot standing upright. To move forwards or backwards the setpoint only needs to be changed with the appropriate value.

A second cascaded PID controller ensures that the robot maintains the position it has reached.

To allow the robot to move right or left, the number of steps is increased for both motors with reversed sign without input to the PID Controller.

Now I am satisfiedwith the behavior of the robot.

further project gestures controlled robot

https://create.arduino.cc/projecthub/RolfK/gesture-control-of-a-self-balancing-robot-using-tensorflow-52037e

principle

The two wheeled self balancing robot represents a robotic platform with two independently actuated wheels and center of gravity above the axis of the wheels rotation.The behavior of the robot is similar to the classical mechanical system of an inverted pendulum.

Featuresof the Robot

  • Control of the robot via Joy Stick using Bluetooth
  • 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

StepperMotors

I decided to useStepper 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-6050Accelerometer + 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. There for 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 DigitalMotion Processing with the MPU-6050 sensor, to do fast calculations directly onthe 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.

MP6500Stepper Motor Driver Carrier

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

CascadedPID Controler

The robot is controlled by cascaded PID controllers. 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 acascading of several controllers; the associated control loops are nested ineach other. The controller output variable of one controller (mastercontroller, Position) serves as the reference variable for another (slavecontroller, Motor).

The standard PIDalgorithm 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 structureof the robot (weight, centre of gravity, etc.).

PWMControler

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 viaPWM Clock-A ("CLKA") and Clock-B ("CLKB").

TaskDispatching

The Dispatching ofthe 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.

Softwarearchitecture

The robot consistsof the main program SBRobotDue02 and the following classes:

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

and the followingincludes:

  • Config
  • LCD
  • PidParameter
  • Plotter

The architecture isshown in the following UML Diagrams:

The Joy Stick

The Funduino Joystick Shield V1.A is used to control the robot in all directions. The shield is using the Arduino Mega. The data from the shield are received via a serial event over a Bluetooth connection. The sketch JoyStickSlave01 sends data to the robot as soon as something has changed.

PIDauto Tuning with Twiddle

Twiddle is an algorithm that tries to find agood 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 aspossible. 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.

Code

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

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

    The robot consists of the main sketch and the following classes/includes:
    -PidControl
    -Battery
    -DuePWMmod
    -DueTimer
    -Twiddle
    -Motor
    -Vehicle
    -MyMPU
    and the following includes:
    -Config
    -LCD
    -PidParameter
    -Plotter

    Features of the Robot
    -Control of the robot via Joy Stick using Bluetooth
    -Stepper Motor, Unipolar/Bipolar, 200 Steps/Rev, 4248mm, 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.
    -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

    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


    Components
    -Arduino Due
    -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
    -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.
    -Adafruit RGB Backlight LCD - 16x2
    -HC-05 Bluetooth Module
    - 7.4V 2S 2200mAh 35C Li-Polymer Lipo
    -Joystick Shield
    -Arduino Mega 2560 & Genuino Mega 2560 used for the joy stick shield
    -OpenBuilds NEMA 17 Stepper Motor Unipolar/Bipolar, 200 Steps/Rev, 42x48mm, 4V, 1200mA  https://www.pololu.com/product/1200/

    Restrictions: Running only at Arduino Due

   This program is free software; you can redistribute it and/or modify it under the terms of
   the GNU General Public License as published by the Free Software Foundation;
   either version 3 of the License, or (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
   or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
   see <http://www.gnu.org/licenses/>.
*/
#ifdef __arm__

// ------------------------------------------------------------------------
//  https://www.arduino.cc/en/Reference/HomePage
// ------------------------------------------------------------------------
/* 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, d6, d7)
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
// ------------------------------------------------------------------------
#include "PidControl.h"
PidParameter PidParams;
PidParameterPos PidParamsPos;

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

// ------------------------------------------------------------------------
// create Robot
// ------------------------------------------------------------------------
#include "Vehicle.h"
Vehicle Robot = Vehicle(&pwm, &MotorA, &MotorB, &PidControler, &PidControlerPos, PinSleepSwitch);

// ------------------------------------------------------------------------
/*  Twiddle Auto 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.
*/
#include "Twiddle.h"
//const bool AutoTuning = true;  // sets Twiddle on
const bool AutoTuning = false;
// Auto Tuning both PID Controler
//Twiddle Tuning ( 7, PidParams.Kp , PidParams.Ki , PidParams.Kd ,  PidParams.Ka , PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka,
//                 0.1,             0.1,           0.01,            0.01,            0.01,              0.01,              0.01,            0.01);
// Auto Tuning only Position Controler
Twiddle Tuning ( 3,  PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka, PidParams.Kp , PidParams.Ki , PidParams.Kd ,  PidParams.Ka ,
                 0.005,             0.001,           0.001,            0.001,            0.001,              0.01,              0.01,            0.01);

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

boolean          Running        = false;     // Speed Control is running
float            TuningValue;
float            setPoint = 0;
float            Calibration = -2.8 ; //-3.2;
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 ;
int              FifoOverflowCnt;

JStickData JStick; // create Joy Stick data

MpuYawPitchRoll YawPitchRoll;  // create Structure YawPitchRoll

/**********************************************************************/
void setup()
/**********************************************************************/
{

  ProgStartTime = millis();

  Serial.begin  (115200); // initialize serial communication
  while (!Serial); //
  Serial.print("Sketch:   ");   Serial.println(__FILE__);
  Serial.print("Uploaded: ");   Serial.println(__DATE__);

  Serial1.begin  (9600);  // initialize Bluetooth communicat

  // 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..."));

  PidControler.changePIDparams(PidParams);       // PID Parameter setzen
  PidControlerPos.changePIDparams(PidParamsPos); // PID Parameter setzen

  // MP 6500 Stepper Motor Driver Carrier
  digitalWrite(PinSleepA, LOW);  //  The defaultSLEEPstate prevents the driver from operating
  digitalWrite(PinSleepB, LOW);  // this pin must be high to enable the driver

  /* Setup PWM
    Once the pwm object has been defined you start using it providing its
    PWM period and its initial duty value (pulse duration).
  */
  //-----------------------------------------------------
  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
  /**********************************************************************/
  YawPitchRoll.pitch = 0;
  Speed = 0;
  duty = period / 2;
  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
  pinMode(MpuIntPin, OUTPUT);
  Robot.init(  );  // Init Robot
  MpuInit();       // Init MPU
}
/**********************************************************************/
void loop()
/**********************************************************************/
{
  if (First) {
    setPoint = 0;
    First = false;
    YawPitchRoll.pitch = 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();
  }

  BTRead( JStick  ); // read JoyStick Data

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

  if ( ( abs(YawPitchRoll.pitch) > 15.0 ) && ( Running == true )) {
    ErrorHandler1();
  }
  // --------------------------------------------------------------
  // Run Robot
  // --------------------------------------------------------------
  if ( RunFlag ) {
    RunFlag = false;

    int PositionAtemp;
    int PositionBtemp;

    manuelPidTuning(); // PID Parameter tuning

    if (Running) {

      //because conflicting declaration 'volatile int PositionA'
      PositionBtemp = PositionB ;
      PositionAtemp = PositionA ;
      Robot.Run( YawPitchRoll, PositionAtemp , PositionBtemp,  JStick ); //   Robot.Run

      if (AutoTuning)  PIDAutoTuning(); // auto Tuning via Twiddle

    }
  }
  // --------------------------------------------------------------
  //  SeriellerPlotter
  // --------------------------------------------------------------
  if ( PlotterFlag ) {
    PlotterFlag = false;
    if (!(AutoTuning))   SeriellerPlotter(JStick);
  }
  // --------------------------------------------------------------
  // 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)) ;
  }
}

//---------------------------------------------------------------------/
// Interrupt Service Routinen
//---------------------------------------------------------------------/
// 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 (MotorA.DirForward )   {
    PositionA ++;
  } else {
    PositionA --;
  }
}
//---------------------------------------------------------------------/
void ISR_PWMB() {
  //  if ( PidControlerPos.DirForward ) {
  if ( MotorB.DirForward ) {
    PositionB ++;
  }  else  {
    PositionB --;
  }
}

// --------------------------------------------------------------
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;
  // PidControler.changePIDparams(PidParams); // PID Parameter setzen


  // manuel Tuning Position
  // PidParamsPos.Ki = TuningValue  = TuningValue / 10000;
  // PidParamsPos.Kp = TuningValue  = TuningValue / 5000;
  // PidParamsPos.Ka = TuningValue  = TuningValue / 5000;
  // PidParamsPos.Kd = TuningValue  = TuningValue / 10000;
  // PidControlerPos.changePIDparams(PidParamsPos); // PID Parameter setzen
}
// --------------------------------------------------------------
void  PIDAutoTuning()  //Twiddle Auto Tuning
// --------------------------------------------------------------
{
  static int skipT = 0;
  skipT++;
  if (skipT > 10) {
    skipT = 11;

    // average = Tuning.next( Robot.PositionAB, PidParams.Kp , PidParams.Ki , PidParams.Kd ,  PidParams.Ka , PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka );

    average = Tuning.next( (Robot.HoldPosition - Robot.PositionAB), PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka, PidParams.Kp , PidParams.Ki , PidParams.Kd ,  PidParams.Ka  );

    PidControler.changePIDparams(PidParams); // PID Parameter setzen
    PidControlerPos.changePIDparams(PidParamsPos); // 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"  );
  MotorA.SleepMode( );
  MotorB.SleepMode( );
  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
}
#else
#error Oops! Trying to include Robot to another device!?
#endif
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 "Config.h"
/**********************************************************************/
class Vehicle
/**********************************************************************/
{
  public:
    Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,
            PidControl * PidControler, PidControl * PidControlerPos, int iPinSleepSwitch);  // Constructor

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

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

    void Stop( );

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


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

};
#endif
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
  The robot is controlled by PID controllers.
  The PID controllers ensures that the robot remains upright.
  The PID controllers are implemented in the "PIDControl" class.
  The standard PID algorithm was slightly modified after longer test series. To the P-component the parameter Kx multiplied
  by the I-component was added.
*/
#include "Vehicle.h"

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

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

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

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

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

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

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

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

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

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

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

}

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

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

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

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

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

}
/**********************************************************************/
void Vehicle::Stop() {
  pMotorA->SleepMode( );
  pMotorB->SleepMode( );
}
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
PID auto Tuning with Twiddle

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

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


};
#endif
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]  , 4 );
  Serial.print(" P1 ");
  Serial.print(params[1]  , 4);
  Serial.print(" P2 ");
  Serial.print(params[2]  , 4 );
  Serial.print(" P3 ");
  Serial.print(params[3]  , 4 );
  Serial.print(" P4 ");
  Serial.print(params[4]  , 2 );
  Serial.print(" P5 ");
  Serial.print(params[5]  , 2);
  Serial.print(" P6 ");
  Serial.print(params[6]  , 2 );
  Serial.print(" P7 ");
  Serial.print(params[7]  , 4 );
  Serial.println(" ");
}
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( JStickData JStick)
// --------------------------------------------------------------
{
  // blau, rot, grn, gelb, lila, grau

  Serial.print(YawPitchRoll.pitch * 5);  //grau
  Serial.print(",");
  Serial.print(PidControler.pTerm );  //grau
  Serial.print(",");
  Serial.print(PidControler.dTerm / 10 );  //grau
  Serial.print(",");
  Serial.print(PidControler.iTerm * 10 );  //grau
  Serial.print(",");
  Serial.print(Robot.StepsA / 50 ); //grau
  Serial.println(" ");
  /*
    Serial.print(YawPitchRoll.pitch * 5);  //grau
    Serial.print(",");
    Serial.print(Robot.StepsA / 100 ); //grau
    Serial.print(",");
    Serial.print(Robot.PositionAB );  //grau
    Serial.print(",");
    Serial.print(Robot.HoldPosition );  //grau
    Serial.print(",");
    Serial.print(Robot.DeltaPos * 10 ); //grau
    Serial.print(" ");
    Serial.print(Robot.DeltaForward * 10 ); //grau
    Serial.println(" ");
  */
}
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.4282 ;
  float          Ki =  5.5223;
  float          Kd = 0.145;
  float          Ka = 0.1220;
  float          Kx = 0.0; //
};
// PidParameter Position
struct PidParameterPos {
  float          K = 1.0;
  float          Kp = 0.0;
  float          Ki = 0.0;
  float          Kd = 0.08 ;
  float          Ka = 0.0 ;
  float          Kx = 0.0;
};
#endif

//Step= 1 Ind= 0 av= 1.5500 besterr 0.0300 P0 9.4282 P1 5.5223 P2 0.1445 P3 0.1220 P4 0.03 P5 0.00 P6 0.02 P7 0.0000
//Step= 1 Ind= 0 av= 5.1000 besterr 0.0267 P0 0.0315 P1 0.0015 P2 0.0000 P3 0.0000 P4 9.43 P5 5.52 P6 0.14 P7 0.1220 
//Step= 1 Ind= 1 av= 4.6900 besterr 0.0133 P0 0.0336 P1 0.0025 P2 0.0010 P3 0.0000 P4 9.43 P5 5.52 P6 0.14 P7 0.1220 
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(PidParameterPos    Params) ;
    PidControl* getInstance();

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

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

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

#endif
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 (PidParameterPos Params)
/**********************************************************************/
{ // Constructor 3 Distance, different PidParameter
  K  = Params.K;
  Kp = Params.Kp;
  Kd = Params.Kd;
  Ki = Params.Ki;
  Ka = Params.Ka;
  Kx = Params.Kx;
  Last_error = 0;
  integrated_error = 0;
  first = true;
}
/**********************************************************************/
PidControl* PidControl::getInstance()
/**********************************************************************/
{
  pPID = this;
  return pPID;
}
/**********************************************************************/
void PidControl::test ()
/**********************************************************************/
{
  Serial.print("PID Test ");
  ptr = (int) this;
  Serial.print("PIDptr ");
  Serial.println(ptr , HEX);
}
/**********************************************************************/
float  PidControl::calculate (float iAngle, float isetPoint )
/**********************************************************************/
// new calculation of Steps per Second // PID algorithm
{
  Now = micros();
  if (first) {
    first = false;
    Last_time = Now;
    integrated_error = 0;
  }
  timeChange = (Now - Last_time)  ;
  timeChange = timeChange / 1000.0;  // in millisekunden
  error = isetPoint -  iAngle;

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

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

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

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

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

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

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


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

/**********************************************************************/
float PidControl::getSteps () {
  return eSpeed;
}
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>

    MPU-6050 Accelerometer + Gyro

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

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

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

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

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

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

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



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

  mpuIntStatus = mpu.getIntStatus();

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
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,  int iPinDir, int iPinStep,
          int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide);

    volatile bool  DirForward;

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

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

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

    unsigned long  lastTime;
    char          _MotorSide;
    DuePWMmod     *ptrpwm;


  private:
    bool MotorMode;
    int ptr;
};
#endif
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, int iPinDir, int iPinStep,
             int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide )
/**********************************************************************/
{
  _Ms1       =  iPinMs1;
  _Ms2       =  iPinMs2;
  _PinStep   = iPinStep;
  _PinDir    = iPinDir;
  _PinSleep  = iPinSleep;
  _MotorSide = iMotorSide;

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

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

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

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

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

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

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

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

    if ((abs(Steps) < 2.0))  Steps = 2.0;
    return Steps;
  }
  else SleepMode( );
}
/**********************************************************************/
void  Motor::MsFull ( ) {
  digitalWrite(_Ms1, LOW);
  digitalWrite(_Ms2, LOW);
  _Divisor = 1;
}
void  Motor::MsHalf ( ) {
  digitalWrite(_Ms1, LOW);
  digitalWrite(_Ms2, HIGH);
  _Divisor = 2;
}
void  Motor::MsQuater ( ) {
  digitalWrite(_Ms1, HIGH);
  digitalWrite(_Ms2, LOW);
  _Divisor = 4;
}
void  Motor::MsMicrostep ( ) {
  digitalWrite(_Ms1, HIGH);
  digitalWrite(_Ms2, HIGH);
  _Divisor = 8;
}
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("P        ");
    lcd.setCursor(2, 1);
    lcd.print(YawPitchRoll.pitch, 2);
    lcd.setCursor(8, 1);
    lcd.print("Y      ");
    lcd.setCursor(10, 1);
    lcd.print(YawPitchRoll.yaw, 2);
    lcd.setCursor(15, 1);

  } else { // it is Init Mode

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

    The Funduino Joystick Shield V1.A is used to control the robot in all directions. 
    The shield is using the Arduino Mega. The data from the shield are received 
    via a serial event over a Bluetooth connection. 
    The sketch JoyStickSlave01 sends data to the robot as soon as something has changed.
    The Robot receives the Date and stor the Data in the following structure.
    struct JStickData {int Xvalue, Yvalue, Up, Down, Left, Right, JButton;
};

*/

String inputString = "";         // a String to hold incoming data
bool   stringComplete = false;  // whether the string is complete
char   c = ' ';

/*
    SerialEvent occurs whenever a new data comes in the hardware serial RX. This
    routine is run between each time loop() runs, so using delay inside loop can
    delay response. Multiple bytes of data may be available.
*/
void serialEvent1() {
  while (Serial1.available()) {
    // get the new byte:
    char inChar = (char)Serial1.read();
    // add it to the inputString:
    if (!stringComplete) {
      inputString += inChar;
    }
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}

void BTRead( JStickData &JSData  ) {
  String       command;
  unsigned int j;
  long         value;


  // print the string when a newline arrives:
  if (stringComplete) {
    if (inputString.substring(0, 1) != "X")
    {
      Serial.print("Error reading Bluetooth ");
      Serial.println(inputString);
    } else {
      j = 0;
      for (unsigned int i = 0; i < inputString.length(); i++) {

        if (inputString.substring(i, i + 1) == "#") {
          command = inputString.substring(j, i);
          //Serial.print("Command: ");
          //Serial.print(command);
          j = i + 1;
          for (unsigned int i1 = j; i1 < inputString.length(); i1++) {
            if (inputString.substring(i1, i1 + 1) == "#") {
              value = inputString.substring(j, i1).toInt();
              //Serial.print(" Value: ");
              //Serial.println(value);
              i = i1;
              j = i + 1;
              assignValues(command, value, JSData);
              break;
            }
          }
        }
      }
    }
    inputString = "";
    stringComplete = false;
    // Serial.print(" Value: ");
    // Serial.println(JStick.Xvalue);
  }
}

void assignValues(String icommand, int ivalue, JStickData &Data  ) {

  if (icommand == "X")  Data.Xvalue  = ivalue;
  if (icommand == "Y")  Data.Yvalue  = ivalue;
  if (icommand == "B1") Data.JButton = ivalue;
  if (icommand == "Up") Data.Up      = ivalue;
  if (icommand == "Do") Data.Down    = ivalue;
  if (icommand == "Ri") Data.Right   = ivalue;
  if (icommand == "Le") Data.Left    = ivalue;

}
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
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::setFreq2(uint32_t  clock_freqA, uint32_t  clock_freqB)
{
  pwm_clockA_freq = pwm_max_duty_Ncount * clock_freqA;
  pwm_clockB_freq = pwm_max_duty_Ncount * clock_freqB;
  PWMC_ConfigureClocks( pwm_clockA_freq, pwm_clockB_freq, VARIANT_MCK );
/*  Serial.print(pwm_clockA_freq); //grau
  Serial.print(",");
  Serial.print(pwm_clockB_freq); //grau
  Serial.println(" ");
*/
}

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"
#include "Filter.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 TX1 BlueThooth
   D19 RX1 BlueThooth
   D36 InterruptStartButton
   D42 Test Pin
   D44 Test Pin

   D36 Motor A Direction
   D6  Motor A Step
   D40 Motor B Direction
   D7  Motor B Step

   D28 PinMs1MotorA
   D30 PinMs2MotorA
   D46 PinMs1MotorB
   D48 PinMs2MotorB

   D22 Sleep MotorA
   D24 Sleep MotorB
   D53 Sleep Switch Input

   A6  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

struct JStickData {
  int Xvalue, Yvalue, Up=1, Down=1, Left=1, Right=1, JButton=1;
};

struct MpuYawPitchRoll {
  float yaw, pitch, roll;
};


#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;
};
#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,2 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.8 ) {
    return false;
  }
  return true;
}

Schematics

sbrobotv03export_schaltplan_pepdyyzhln_PzBHNaZjSL.jpg
Sbrobotv03export schaltplan pepdyyzhln pzbhnazjsl

Comments

Similar projects you might like

two wheeled self balancing robot with stepper motor.

Project in progress by RolfK

  • 14,580 views
  • 4 comments
  • 34 respects

Arduino Balancing Robot

Project showcase by TEAM DIY

  • 10,681 views
  • 1 comment
  • 10 respects

Wifi Enabled 2/4 Wheeled Robot Platform Utilizing MKR1000

Project showcase by James Martel

  • 4,653 views
  • 6 comments
  • 12 respects

PHPoC - Arduino Self Balancing Robot with BT+Web Control

Project in progress by Suyog Gunjal

  • 7,432 views
  • 2 comments
  • 25 respects

CUTSIE WHUN Version 2 - The Ultimate Balancing Robot

Project in progress by Pigeon-Kicker

  • 2,945 views
  • 0 comments
  • 13 respects

Amazing 6WD Off-Road Robot | Arduino RC Robot

Project tutorial by Jithin Sanal

  • 12,383 views
  • 1 comment
  • 64 respects
Add projectSign up / Login