Two wheeled self balancing robot (redesign)

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4
5   Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev, 4248mm, 4V, 1.2 A/Phase
6  https://www.pololu.com/product/1200/faqs
7   This NEMA 17-size hybrid stepping motor can be used as a unipolar or bipolar stepper   motor
8  and has a 1.8 step angle (200 steps/revolution). Each phase draws 1.2   A at 4 V,
9  allowing for a holding torque of 3.2 kg-cm (44 oz-in).
10
11  Wheel   Durchmesser 88mm = > Distance per Pulse Dpp = d * pi / 200 =  1,3816 mm
12  Distance   per Revolution =  276,32 mm
13  Max 1000 Steps per Second = 5 Revolutions => 13816   mm Distance
14
15  Motion Control Modules  Stepper Motor Drivers  MP6500 Stepper   Motor Driver Carriers
16  https://www.pololu.com/product/2966https://www.pololu.com/product/2966
17   This breakout board for the MPS MP6500 microstepping bipolar stepper motor driver   has a
18  pinout and interface that are very similar to that of our popular A4988   carriers,
19  so it can be used as a drop-in replacement for those boards in many   applications.
20  The MP6500 offers up to 1/8-step microstepping, operates from   4.5 V to 35 V,
21  and can deliver up to approximately 1.5 A per phase continuously   without a heat sink
22  or forced air flow (up to 2.5 A peak).
23
24  MS1   MS2    Microstep Resolution
25  Low   Low   Full step
26  High  Low   Half (1/2) step
27   Low   High  Quarter (1/4) step
28  High  High  Eighth (1/8) step
29*/
30
31#include   "Motor.h"
32#include "Config.h"
33/**********************************************************************/
34Motor::Motor(DuePWMmod*   ipwm, int iPinDir, int iPinStep,
35             int iPinMs1, int iPinMs2, int iPinSleep,   char iMotorSide )
36/**********************************************************************/
37{
38   _Ms1       =  iPinMs1;
39  _Ms2       =  iPinMs2;
40  _PinStep   = iPinStep;
41   _PinDir    = iPinDir;
42  _PinSleep  = iPinSleep;
43  _MotorSide = iMotorSide;
44
45   // The default SLEEP state prevents the driver from operating;
46  // this pin   must be high to enable the driver
47  pinMode(_PinSleep, OUTPUT);
48  pinMode(_PinDir,   OUTPUT);
49  pinMode(_Ms1, OUTPUT);
50  pinMode(_Ms2, OUTPUT);
51
52  ptr =   (int) this;
53  ptrpwm = ipwm;
54}
55/**********************************************************************/
56Motor*   Motor::getInstance()
57/**********************************************************************/
58{
59   pMotor = this;
60  return pMotor;
61}
62/**********************************************************************/
63void   Motor::init ( )
64/**********************************************************************/
65{
66
67   Serial.print("Motor ");
68  Serial.print(ptr , HEX);
69  Serial.print(" Side   ");
70  Serial.print(_MotorSide);
71  Serial.print(" iPinStep ");
72  Serial.print(_PinStep);
73   Serial.print(" iPinSleep ");
74  Serial.print(_PinSleep);
75  Serial.println("   init...");
76  lastTime = millis();
77
78}
79/**********************************************************************/
80void   Motor::SleepMode( )
81/**********************************************************************/
82{
83   digitalWrite(_PinSleep, LOW);
84  MotorMode = false;
85}
86/**********************************************************************/
87void   Motor::RunMode( )
88/**********************************************************************/
89{
90   digitalWrite(_PinSleep, HIGH);
91  MotorMode = true;
92}
93/**********************************************************************/
94void   Motor::toggleMode( )
95/**********************************************************************/
96{
97   if ( MotorMode == false ) RunMode( );
98  else  SleepMode();
99}
100/**********************************************************************/
101float   Motor::Run(float Steps) {
102  /**********************************************************************/
103
104   //  Motor 20070C9C Side A iPinStep 6 iPinSleep 22  init...
105  // Motor 20070CF4   Side B iPinStep 7 iPinSleep 24  init...
106
107  if (!digitalRead(PinSleepSwitch))   {
108    RunMode( );
109
110    if (_MotorSide == rechterMotor) {
111      if (Steps   >= 0 ) {
112        digitalWrite(_PinDir, LOW);
113        DirForward = true ;
114       }
115      else {
116        digitalWrite(_PinDir, HIGH);
117        DirForward   = false ;
118      }
119    } else
120    {
121      if (Steps >= 0 ) {
122        digitalWrite(_PinDir,   HIGH);
123        DirForward = true ;
124      }
125      else {
126        digitalWrite(_PinDir,   LOW);
127        DirForward = false ;
128      }
129    }
130
131    if (_Divisor   > 0) {
132      Steps = Steps * _Divisor; // convert into Microsteps
133    }
134
135     if ((abs(Steps) < 2.0))  Steps = 2.0;
136    return Steps;
137  }
138  else   SleepMode( );
139}
140/**********************************************************************/
141void   Motor::MsFull ( ) {
142  digitalWrite(_Ms1, LOW);
143  digitalWrite(_Ms2, LOW);
144   _Divisor = 1;
145}
146void  Motor::MsHalf ( ) {
147  digitalWrite(_Ms1, LOW);
148   digitalWrite(_Ms2, HIGH);
149  _Divisor = 2;
150}
151void  Motor::MsQuater ( )   {
152  digitalWrite(_Ms1, HIGH);
153  digitalWrite(_Ms2, LOW);
154  _Divisor = 4;
155}
156void   Motor::MsMicrostep ( ) {
157  digitalWrite(_Ms1, HIGH);
158  digitalWrite(_Ms2,   HIGH);
159  _Divisor = 8;
160}
161

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    PID Controller
3    written by : Rolf Kurth in 2019
4     rolf.kurth@cron-consulting.de
5*/
6#include "PidControl.h"
7#include   <Arduino.h>
8
9/**********************************************************************/
10PidControl::PidControl   (float iK, float iKp, float iKi, float iKd, float iKa, float iKx)
11/**********************************************************************/
12{   // Constructor 1
13  K  = iK;
14  Kp = iKp;
15  Kd = iKd;
16  Ki = iKi;
17   Ka = iKa;
18  Kx = iKx;
19  Last_error = 0;
20  integrated_error = 0;
21  first   = true;
22}
23/**********************************************************************/
24PidControl::PidControl   (PidParameter Params)
25/**********************************************************************/
26{   // Constructor 2 Motor, different PidParameter
27  K  = Params.K;
28  Kp = Params.Kp;
29   Kd = Params.Kd;
30  Ki = Params.Ki;
31  Ka = Params.Ka;
32  Kx = Params.Kx;
33   Last_error = 0;
34  integrated_error = 0;
35  first = true;
36}
37/**********************************************************************/
38PidControl::PidControl   (PidParameterPos Params)
39/**********************************************************************/
40{   // Constructor 3 Distance, different PidParameter
41  K  = Params.K;
42  Kp =   Params.Kp;
43  Kd = Params.Kd;
44  Ki = Params.Ki;
45  Ka = Params.Ka;
46  Kx   = Params.Kx;
47  Last_error = 0;
48  integrated_error = 0;
49  first = true;
50}
51/**********************************************************************/
52PidControl*   PidControl::getInstance()
53/**********************************************************************/
54{
55   pPID = this;
56  return pPID;
57}
58/**********************************************************************/
59void   PidControl::test ()
60/**********************************************************************/
61{
62   Serial.print("PID Test ");
63  ptr = (int) this;
64  Serial.print("PIDptr   ");
65  Serial.println(ptr , HEX);
66}
67/**********************************************************************/
68float   PidControl::calculate (float iAngle, float isetPoint )
69/**********************************************************************/
70//   new calculation of Steps per Second // PID algorithm
71{
72  Now = micros();
73   if (first) {
74    first = false;
75    Last_time = Now;
76    integrated_error   = 0;
77  }
78  timeChange = (Now - Last_time)  ;
79  timeChange = timeChange   / 1000.0;  // in millisekunden
80  error = isetPoint -  iAngle;
81
82  if ( timeChange   != 0) {
83    dTerm =  1000.0 * Kd * (error - Last_error) /  timeChange  ;
84   }
85
86  integrated_error = integrated_error  + ( error * timeChange );
87   iTerm =   Ki * integrated_error / 1000.0;
88
89  pTerm =   Kp  * error + ( Ka   * integrated_error ); // modifying Kp
90
91  // Compute PID Output in Steps per   second
92  eSpeed = K * (pTerm + iTerm + dTerm) ;
93
94  /*Remember something*/
95   Last_time  = Now;
96  Last_error = error;
97
98  //  digitalWrite(TestPIDtime,   !digitalRead(TestPIDtime)); // Toggle  Pin for reading the Frequenzy
99  // eSpeed   = constrain (eSpeed , -500.0 , 500.0 ); // 10 Steps per Second because Microstep
100   
101 return eSpeed;  // Steps per Second
102}
103
104/**********************************************************************/
105void   PidControl::reset ()
106/**********************************************************************/
107{
108   integrated_error = 0.0;
109  Last_error = 0.0;
110}
111
112
113/**********************************************************************/
114void   PidControl::changePIDparams (PidParameter Params)
115// changePIDparams = different   PidParameter !!!!
116/**********************************************************************/
117{
118   K  = Params.K;
119  Kp = Params.Kp;
120  Kd = Params.Kd;
121  Ki = Params.Ki;
122   Ka = Params.Ka;
123  Kx = Params.Kx;
124}
125/**********************************************************************/
126void   PidControl::changePIDparams (PidParameterPos Params)
127// changePIDparams = different   PidParameter !!!!
128/**********************************************************************/
129{   // different PidParameter !!!!
130  K  = Params.K;
131  Kp = Params.Kp;
132  Kd   = Params.Kd;
133  Ki = Params.Ki;
134  Ka = Params.Ka;
135  Kx = Params.Kx;
136}
137
138/**********************************************************************/
139float   PidControl::getSteps () {
140  return eSpeed;
141}
142

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5#ifndef   Config_h
6#define  Config_h
7#include "Arduino.h"
8#include "Filter.h"
9
10
11/*   PIN Belegung
12   D2  Interrupt 0 for MPU6050
13   D4  LiquidCrystal DB4
14   D5   LiquidCrystal DB5
15   D9  LiquidCrystal rs grau
16   D8  LiquidCrystal enable   lila
17   D10 LiquidCrystal DB6 gelb >
18   D11 LiquidCrystal DB7 orange >
19    D18 TX1 BlueThooth
20   D19 RX1 BlueThooth
21   D36 InterruptStartButton
22    D42 Test Pin
23   D44 Test Pin
24
25   D36 Motor A Direction
26   D6  Motor   A Step
27   D40 Motor B Direction
28   D7  Motor B Step
29
30   D28 PinMs1MotorA
31    D30 PinMs2MotorA
32   D46 PinMs1MotorB
33   D48 PinMs2MotorB
34
35   D22   Sleep MotorA
36   D24 Sleep MotorB
37   D53 Sleep Switch Input
38
39   A6  Spannung   VoltPin
40   A7  Tuning Poti 10Kohm
41
42  LiquidCrystal(rs, enable, d4, d5,   d6, d7)
43  LiquidCrystal lcd(9, 8, 4, 5, 10, 11);
44    10K resistor:
45    ends   to +5V and ground
46    wiper (3) to LCD VO pin
47*/
48
49#define MpuInterruptPin   2       // use pin on Arduino for MPU Interrupt
50#define LED_PIN 13
51#define   MpuIntPin 27            //the interrupt is used to determine when new data is available.
52
53
54
55const   int  PinMs1MotorA  = 28;
56const int  PinMs2MotorA  = 30;
57const int  PinMs1MotorB   = 46;
58const int  PinMs2MotorB  = 48;
59const int  PinDirMotorA  = 36;
60const   int  PinDirMotorB  = 40;
61const int  PinStepMotorA  = 6;
62const int  PinStepMotorB   = 7;
63const int  PinSleepSwitch = 53;
64const int  PinSleepA      = 22;
65const   int  PinSleepB      = 24;
66const int  VoltPin        = A6; // Voltage VIN
67const   int  TuningPin      = A7; //Potentiometer
68
69struct JStickData {
70  int Xvalue,   Yvalue, Up=1, Down=1, Left=1, Right=1, JButton=1;
71};
72
73struct MpuYawPitchRoll   {
74  float yaw, pitch, roll;
75};
76
77
78#endif
79

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing  
2 *  written by : Rolf Kurth in 2019
3 *  rolf.kurth@cron-consulting.de
4   */
5 
6 // --------------------------------------------------------------
7void   LCD_show()
8// --------------------------------------------------------------
9{
10   static int Line = 0;
11  Line = !Line;
12  switch (Line) {
13    case 0:
14       LCD_showLine0();
15      break;
16    default:
17      LCD_showLine1();
18       break;
19  }
20}
21// --------------------------------------------------------------
22void   LCD_showLine1()
23// --------------------------------------------------------------
24{
25
26   static int delay;
27  if ( Running == true ) {
28    lcd.setCursor(0, 1);
29     lcd.print("P        ");
30    lcd.setCursor(2, 1);
31    lcd.print(YawPitchRoll.pitch,   2);
32    lcd.setCursor(8, 1);
33    lcd.print("Y      ");
34    lcd.setCursor(10,   1);
35    lcd.print(YawPitchRoll.yaw, 2);
36    lcd.setCursor(15, 1);
37
38   } else { // it is Init Mode
39
40    delay = (( millis() -  ProgStartTime )   ) ;
41    lcd.setCursor(0, 1);
42    lcd.print("P ");
43    lcd.setCursor(2,   1);
44    lcd.print(YawPitchRoll.pitch, 2);
45    lcd.setCursor(8, 1);
46    lcd.print("Y   ");
47    lcd.setCursor(10, 1);
48    lcd.print(YawPitchRoll.yaw, 2);
49  }
50}
51//   --------------------------------------------------------------
52void LCD_showLine0()
53//   --------------------------------------------------------------
54{
55  static   int delay;
56  if ( Running == true ) {
57    lcd.setCursor(0, 0);
58    lcd.print("Run   ");
59    lcd.setCursor(4, 0);
60    lcd.print("K ");
61    lcd.setCursor(5,   0);
62    lcd.print(TuningValue, 3);
63   // lcd.print(FifoOverflowCnt);
64    lcd.setCursor(11,   0);
65    lcd.print(Voltage, 1);
66    lcd.print("V");
67  } else { // it is   Init Mode
68    delay = ((StartDelay - ( millis() -  ProgStartTime)) / 1000 ) ;
69     lcd.setCursor(0, 0);
70    lcd.print("Init ");
71    lcd.setCursor(7, 0);
72     lcd.print("  ");
73    lcd.setCursor(7, 0);
74    lcd.print(delay);
75     lcd.setCursor(11, 0);
76    lcd.print(Voltage, 1);
77    lcd.print("V");
78   }
79}
80

1/* DuePWMmod
2  Purpose:
3   Setup one or two unique PWM frequenices  directly in sketch program,
4   set PWM duty cycle, and stop PWM function.
5   modified by : Rolf Kurth
6*/
7
8#include "DuePWMmod.h"
9#include "Arduino.h"
10
11
12DuePWMmod::DuePWMmod()
13{
14  /*
15    uint32_t pmc_enable_periph_clk  ( uint32_t  ul_id )
16    Enable the  specified peripheral clock.
17    Note
18    The ID must NOT be shifted (i.e.,  1 << ID_xxx).
19    Parameters
20    ul_id  Peripheral ID (ID_xxx).
21    Return  values
22    0 Success.
23    1 Invalid parameter.
24    http://asf.atmel.com/docs/latest/sam3x/html/group__sam__drivers__pmc__group.html#gad09de55bb493f4ebdd92305f24f27d62
25  */
26  pmc_enable_periph_clk( PWM_INTERFACE_ID );
27}
28
29// MAIN PWM INITIALIZATION
30//--------------------------------
31void  DuePWMmod::pinFreq( uint32_t  pin, char ClockAB )
32{
33
34  /*
35    uint32_t  pio_configure  ( Pio *   p_pio, const pio_type_t  ul_type, const uint32_t  ul_mask,  const uint32_t  ul_attribute)    )
36    Perform complete pin(s) configuration;  general attributes and PIO init if necessary.
37    Parameters
38    p_pio Pointer  to a PIO instance.
39    ul_type PIO type.
40    ul_mask Bitmask of one or more  pin(s) to configure.
41    ul_attribute  Pins attributes.
42    Returns
43    Whether  the pin(s) have been configured properly.
44    http://asf.atmel.com/docs/latest/sam3x/html/group__sam__drivers__pio__group.html#gad5f0174fb8a14671f06f44042025936e
45  */
46  uint32_t  chan = g_APinDescription[pin].ulPWMChannel;
47
48  PIO_Configure(  g_APinDescription[pin].pPort,  g_APinDescription[pin].ulPinType,
49                 g_APinDescription[pin].ulPin,  g_APinDescription[pin].ulPinConfiguration);
50  if ( ClockAB == rechterMotor)  {
51    PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
52  }
53  if ( ClockAB == linkerMotor) {
54    PWMC_ConfigureChannel(PWM_INTERFACE,  chan, PWM_CMR_CPRE_CLKB, 0, 0);
55  }
56  PWMC_SetPeriod(PWM_INTERFACE, chan,  pwm_max_duty_Ncount);
57  PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);  // The 0  is the initial duty cycle
58  PWMC_EnableChannel(PWM_INTERFACE, chan);
59}
60
61void  DuePWMmod::setFreq2(uint32_t  clock_freqA, uint32_t  clock_freqB)
62{
63  pwm_clockA_freq  = pwm_max_duty_Ncount * clock_freqA;
64  pwm_clockB_freq = pwm_max_duty_Ncount  * clock_freqB;
65  PWMC_ConfigureClocks( pwm_clockA_freq, pwm_clockB_freq, VARIANT_MCK  );
66/*  Serial.print(pwm_clockA_freq); //grau
67  Serial.print(",");
68  Serial.print(pwm_clockB_freq);  //grau
69  Serial.println(" ");
70*/
71}
72
73void DuePWMmod::setFreq(uint32_t  clock_freq, char ClockAB)
74{
75  if ( ClockAB == rechterMotor) {
76    pwm_clockA_freq  = pwm_max_duty_Ncount * clock_freq;
77  }
78  if ( ClockAB == linkerMotor) {
79    pwm_clockB_freq =  pwm_max_duty_Ncount * clock_freq;
80  }
81  /// void PWMC_ConfigureClocks(unsigned  int clka, unsigned int clkb, unsigned int mck)
82  /// Configures PWM clocks A  & B to run at the given frequencies. This function
83  /// finds the best MCK divisor  and prescaler values automatically.
84  /// \\param clka  Desired clock A frequency  (0 if not used).
85  /// \\param clkb  Desired clock B frequency (0 if not used).
86  /// \\param mck  Master clock frequency.
87  // #include "pwmc.h"
88
89  PWMC_ConfigureClocks(  pwm_clockA_freq, pwm_clockB_freq, VARIANT_MCK );
90}
91
92// WRITE DUTY CYCLE
93//--------------------------------
94void  DuePWMmod::pinDuty( uint32_t  pin,  uint32_t  duty )
95{
96  PWMC_SetDutyCycle(PWM_INTERFACE,  g_APinDescription[pin].ulPWMChannel, duty);
97}
98//--------------------------------
99void  DuePWMmod::EnableChannel( uint32_t  pin )
100//Enable the specified PWM channel.
101//  PWM_INTERFACE = Module hardware register base address pointer
102// g_APinDescription[pin].ulPWMChannel  = PWM channel number
103{
104  PWMC_EnableChannel (PWM_INTERFACE, g_APinDescription[pin].ulPWMChannel);
105}
106void  DuePWMmod::DisableChannel( uint32_t  pin )
107//Disable the specified PWM channel.
108//  PWM_INTERFACE = Module hardware register base address pointer
109// g_APinDescription[pin].ulPWMChannel  = PWM channel number
110{
111  PWMC_DisableChannel (PWM_INTERFACE, g_APinDescription[pin].ulPWMChannel);
112}
113

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4PID   auto Tuning with Twiddle
5
6    Twiddle is an algorithm that tries to find a   good choice of parameters for an algorithm. 
7    Also known as Hill climbing,   it is an analogy to a mountaineer who looks for the summit in dense 
8    fog   and steers his steps as steeply uphill as possible. If it only goes down in all   directions, 
9    he has arrived at a summit.The Twiddle algorithm is used for   auto tuning of PID parameter. 
10    First of all, parameters can be tested with   a manual tuning with a potentiometer.
11*/
12#ifndef Twiddle_h
13#define  Twiddle_h
14#include   "Arduino.h"
15//**********************************************************************   /
16class Twiddle
17///**********************************************************************/
18{
19   public:
20    Twiddle( int anzParams, float p0 , float p1, float p2, float p3,   float p4 , float p5, float p6, float p7,
21             float dp0, float dp1, float   dp2, float dp3 , float dp4, float dp5, float dp6 , float dp7)  ; // Constructor
22     Twiddle(int anzParams, float iparams[], float idparams[]  )  ; // Constructor
23
24     float next(float error, float &p0, float &p1, float &p2, float &p3, float &p4,   float &p5, float &p6, float &p7);
25    void calcCost(float average);
26    void   logging();
27    float params[8];
28    float dparams[8];
29    float besterr;
30     float lastcost;
31    float average;
32    int   index ;
33    int   AnzParams   = 8;
34    float sum_average;
35    unsigned int cnt_average;
36    int   nextStep;
37     int AnzahlElements = 8;
38
39
40};
41#endif
42

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5#ifndef   PidControl_h
6#define  PidControl_h
7#include "Arduino.h"
8
9#include   "PidParameter.h"
10
11/**********************************************************************/
12class   PidControl
13/**********************************************************************/
14{
15   public:
16
17    PidControl(float K, float Kp, float Ki, float Kd, float iKa   , float iKx) ;
18    PidControl(PidParameter      Params) ;
19    PidControl(PidParameterPos     Params) ;
20    PidControl* getInstance();
21
22    /* C++ Overloading //   changePIDparams = different PidParameter !!!!
23      An overloaded declaration   is a declaration that is declared with the same
24      name as a previously declared   declaration in the same scope, except that
25      both declarations have different   arguments and obviously different
26      definition (implementation).
27    */
28     void     changePIDparams(PidParameter Params);
29    void     changePIDparams(PidParameterPos   Params);
30
31    float    calculate (float iAngle, float isetPoint );
32    float     getSteps ();
33    void     reset ();
34    void     test      ( );
35    float     DeltaKp(float iError);
36    float    eSpeed;
37    float    pTerm;
38    float     iTerm;
39    float    dTerm;
40    float    error;
41    float    integrated_error;
42    // volatile bool  DirForward;
43    float    Last_error;
44
45  protected:
46     struct   PidParameter params;
47    float    LastK;
48    float    K;
49     float    Ki;
50    float    Kd;
51    float    Kp;
52    float    Ka;
53     float    Kx;
54    //   float    Last_angle;
55    float    timeChange ;
56     unsigned long Last_time;
57    unsigned long Now;
58    int      ptr;
59     PidControl* pPID;
60    bool first ;
61};
62
63#endif
64

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4     Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
5     by Jeff Rowberg <jeff@rowberg.net>
6
7    MPU-6050 Accelerometer + Gyro
8
9     The MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single   chip. 
10    It is very accurate, as it contains 16-bits analog to digital conversion   hardware for each channel. 
11    Therefor it captures the x, y, and z channel   at the same time. The sensor uses the I2C -bus to interface with the Arduino.
12
13     I used Digital Motion Processing with the MPU-6050 sensor, to do fast calculations   directly on the chip. 
14    This reduces the load for the Arduino.  
15
16    https://playground.arduino.cc/Main/MPU-6050
17
18     Because of the orientation of my board, I used yaw/pitch/roll angles (in degrees)   calculated from the quaternions coming from the FIFO. 
19    By reading Euler angle   I got problems with Gimbal lock.    
20*/
21
22/*    0x02,   0x16,   0x02,   0x00,   0x07                // D_0_22 inv_set_fifo_rate
23
24    // This very last 0x01   WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
25    // 0x01   is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
26     // DMP output frequency is calculated easily using this equation: (200Hz / (1   + value))
27
28    // It is important to make sure the host processor can keep   up with reading and processing
29    // the FIFO output at the desired rate. Handling   FIFO overflow cleanly is also a good idea.
30*/
31// ------------------------------------------------------------------------
32//   orientation/motion vars
33// ------------------------------------------------------------------------
34Quaternion   q;           // [w, x, y, z]         quaternion container
35VectorInt16 aa;         //   [x, y, z]            accel sensor measurements
36VectorInt16 aaReal;     // [x,   y, z]            gravity-free accel sensor measurements
37VectorInt16 aaWorld;     // [x, y, z]            world-frame accel sensor measurements
38VectorFloat   gravity;    // [x, y, z]            gravity vector
39float euler[3];         //   [psi, theta, phi]    Euler angle container
40float ypr[3];           // [yaw, pitch,   roll]   yaw/pitch/roll container and gravity vector
41// ------------------------------------------------------------------------
42//   MPU control/status vars
43// ------------------------------------------------------------------------
44bool      dmpReady = false;  // set true if DMP init was successful
45uint8_t  mpuIntStatus;       // holds actual interrupt status byte from MPU
46uint8_t  devStatus;         //   return status after each device operation (0 = success, !0 = error)
47uint16_t   packetSize;        // expected DMP packet size (default is 42 bytes)
48uint16_t   fifoCount;         // count of all bytes currently in FIFO
49uint8_t  fifoBuffer[64];     // FIFO storage buffer
50
51
52
53// --------------------- Sensor aquisition   -------------------------
54MpuYawPitchRoll ReadMpuRunRobot()
55// ---------------------   Sensor aquisition  -------------------------
56// wait for MPU interrupt or extra   packet(s) available
57// --------------------------------------------------------------------
58{
59   if (mpuInterrupt or fifoCount >= packetSize) {
60    if  (mpuInterrupt) mpuInterrupt   = false;  // reset interrupt flag
61    digitalWrite(LED_PIN, HIGH); // blink LED   to indicate activity
62    //    Angle = ReadMpuRunRobot6050() - CalibrationAngle;
63     YawPitchRoll = ReadMpuRunRobot6050();
64    YawPitchRoll.pitch  +=  Calibration;
65     // blinkState = !blinkState;
66    digitalWrite(LED_PIN, LOW);
67  }
68  return   YawPitchRoll ;
69}
70// --------------------------------------------------------------------   -
71MpuYawPitchRoll ReadMpuRunRobot6050()
72// --------------------------------------------------------------------   -
73{
74  static float pitch_old;
75  static float yaw_old;
76  static float   yaw_tmp;
77  static float yaw_delta;
78  //  static float pitch;
79
80  mpuIntStatus   = mpu.getIntStatus();
81
82  // get current FIFO count
83  fifoCount = mpu.getFIFOCount();
84   // check for overflow (this should never happen unless our code is too inefficient)
85   if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
86    // reset so we can continue   cleanly
87    //    mpu.setDMPEnabled(false);
88    mpu.resetFIFO();
89    FifoOverflowCnt   ++;
90    fifoCount = 0;
91    YawPitchRoll.pitch = pitch_old;
92    return YawPitchRoll;
93
94   }
95  // otherwise, check for DMP data ready interrupt (this should happen frequently)
96   // Register 58  lnterrupt Status INT_STATUS
97  // MPU-6500 Register Map and   Descriptions Revision  2.1
98  // Bit [1] DMP_INT This bit automatically sets to   1 when the DMP interrupt has been generated.
99  // Bit [0] RAW_DATA_RDY_INT1 Sensor   Register Raw Data sensors are updated and Ready to be read.
100  if ((mpuIntStatus)   & 0x02 || (mpuIntStatus & 0x01)) {
101    // wait for correct available data length,   should be a VERY short wait
102    while (fifoCount < packetSize)  fifoCount = mpu.getFIFOCount();
103
104     while (fifoCount > 0) {
105      // read a packet from FIFO
106      mpu.getFIFOBytes(fifoBuffer,   packetSize);
107
108      // track FIFO count here in case there is > 1 packet available
109       // (this lets us immediately read more without waiting for an interrupt)
110       fifoCount = mpu.getFIFOCount();
111      //      fifoCount -= packetSize;
112     }
113    // the yaw/pitch/roll angles (in degrees) calculated from the quaternions   coming
114    // from the FIFO. Note this also requires gravity vector calculations.
115     // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
116    //   more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
117
118    mpu.dmpGetQuaternion(&q,   fifoBuffer);
119    mpu.dmpGetGravity(&gravity, &q);
120    // mpu.dmpGetEuler(euler,   &q);
121    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
122
123    YawPitchRoll.pitch   = -(ypr[1] * 180 / M_PI); //pitch
124
125    yaw_tmp  = (abs(ypr[0] * 180 / M_PI));
126     yaw_delta = yaw_tmp - yaw_old;
127    yaw_old = yaw_tmp;
128    YawPitchRoll.yaw   += yaw_delta;
129
130    // actual quaternion components in a [w, x, y, z]
131     // YawPitchRoll.pitch = (q.y) * 180;
132    // YawPitchRoll.yaw = (q.z );
133     // YawPitchRoll.yaw =  mapfloat(YawPitchRoll.yaw , -1.0, 1.0, 0.0, 360.0);
134   }
135  pitch_old = YawPitchRoll.pitch ;
136  return  YawPitchRoll ;
137}
138//   --------------------------------------------------------------
139void MpuInit()
140//   --------------------------------------------------------------
141// MPU6050_6Axis_MotionApps20.h
142//   0x02,   0x16,   0x02,   0x00, 0x09  => 50msec 20 Hz
143// This very last 0x01 WAS   a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
144// 0x01 is 100Hz.   Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
145// DMP   output frequency is calculated easily using this equation: (200Hz / (1 + value))
146//   It is important to make sure the host processor can keep up with reading and processing
147//   the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good   idea.
148
149{
150  // after Reset of Arduino there is no Reset of MPU
151  pinMode(MpuInterruptPin,   INPUT);
152
153  // verify connection
154  Serial.println(F("Testing device connections..."));
155   Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050   connection failed"));
156
157  // load and configure the DMP
158  Serial.println(F("Initializing   DMP..."));
159  devStatus = mpu.dmpInitialize();
160  // supply your own gyro offsets   here, scaled for min sensitivity
161  mpu.setXGyroOffset(220);
162  mpu.setYGyroOffset(76);
163   mpu.setZGyroOffset(-84);
164  mpu.setZAccelOffset(1788); //
165
166  // make sure   it worked (returns 0 if so)
167  if (devStatus == 0) {
168    // turn on the DMP,   now that it's ready
169    Serial.println(F("Enabling DMP..."));
170    mpu.setDMPEnabled(true);
171
172     // enable Arduino interrupt detection
173    Serial.println(F("Enabling interrupt   detection (Arduino external interrupt 0)..."));
174    attachInterrupt(digitalPinToInterrupt(MpuInterruptPin),   dmpDataReady, RISING);
175    mpuIntStatus = mpu.getIntStatus();
176
177    // set   our DMP Ready flag so the main loop() function knows it's okay to use it
178    Serial.println(F("DMP   ready! Waiting for first interrupt..."));
179    dmpReady = true;
180
181    //   get expected DMP packet size for later comparison
182    packetSize = mpu.dmpGetFIFOPacketSize();
183     //    mpu.resetFIFO();  // after Reset importand
184
185  } else {
186    //   ERROR!
187    // 1 = initial memory load failed
188    // 2 = DMP configuration   updates failed
189    // (if it's going to break, usually the code will be 1)
190     Serial.print(F("DMP Initialization failed (code "));
191    Serial.print(devStatus);
192     Serial.println(F(")"));
193    lcd.clear();
194    lcd.setCursor(0, 0);
195     lcd.print( "Error MPU6050 "  );
196    do  {} while ( 1 == 1);
197  }
198}
199
200float   mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
201{
202   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
203}
204

1/* Self balancing Robot via Stepper Motor with microstepping and Digital  Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5#ifndef  Motor_h
6#define  Motor_h
7#include "Arduino.h"
8#include "PidControl.h"
9#include  "DuePWMmod.h"
10
11// ------------------------------------------------------------------------
12class  Motor
13// ------------------------------------------------------------------------
14
15{  public:
16    Motor(DuePWMmod* ipwm,  int iPinDir, int iPinStep,
17          int  iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide);
18
19    volatile bool  DirForward;
20
21   // struct PidParameter params;
22    void init() ;
23    Motor* getInstance();
24    void SleepMode ( );
25    void RunMode ( );
26    void toggleMode ( );
27    float Run(float Steps);
28
29    // Four different  step resolutions: full-step, half-step, 1/4-step, and 1/8-step
30    void MsFull  ( );
31    void MsHalf ( );
32    void MsQuater ( );
33    void MsMicrostep (  );
34
35    int _Ms1, _Ms2, _PinDir, _PinStep, _PinSleep;
36    int _Divisor;
37    Motor* pMotor;
38
39    unsigned long  lastTime;
40    char          _MotorSide;
41    DuePWMmod     *ptrpwm;
42
43
44  private:
45    bool MotorMode;
46    int  ptr;
47};
48#endif
49

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4     The Joy Stick
5
6    The Funduino Joystick Shield V1.A is used to control   the robot in all directions. 
7    The shield is using the Arduino Mega. The data   from the shield are received 
8    via a serial event over a Bluetooth connection.   
9    The sketch JoyStickSlave01 sends data to the robot as soon as something   has changed.
10    The Robot receives the Date and stor the Data in the following   structure.
11    struct JStickData {int Xvalue, Yvalue, Up, Down, Left, Right,   JButton;
12};
13
14*/
15
16String inputString = "";         // a String to   hold incoming data
17bool   stringComplete = false;  // whether the string is complete
18char    c = ' ';
19
20/*
21    SerialEvent occurs whenever a new data comes in the   hardware serial RX. This
22    routine is run between each time loop() runs, so   using delay inside loop can
23    delay response. Multiple bytes of data may be   available.
24*/
25void serialEvent1() {
26  while (Serial1.available()) {
27     // get the new byte:
28    char inChar = (char)Serial1.read();
29    // add   it to the inputString:
30    if (!stringComplete) {
31      inputString += inChar;
32     }
33    // if the incoming character is a newline, set a flag so the main loop   can
34    // do something about it:
35    if (inChar == '\
36') {
37      stringComplete   = true;
38    }
39  }
40}
41
42void BTRead( JStickData &JSData  ) {
43  String        command;
44  unsigned int j;
45  long         value;
46
47
48  // print   the string when a newline arrives:
49  if (stringComplete) {
50    if (inputString.substring(0,   1) != "X")
51    {
52      Serial.print("Error reading Bluetooth ");
53      Serial.println(inputString);
54     } else {
55      j = 0;
56      for (unsigned int i = 0; i < inputString.length();   i++) {
57
58        if (inputString.substring(i, i + 1) == "#") {
59          command   = inputString.substring(j, i);
60          //Serial.print("Command: ");
61          //Serial.print(command);
62           j = i + 1;
63          for (unsigned int i1 = j; i1 < inputString.length();   i1++) {
64            if (inputString.substring(i1, i1 + 1) == "#") {
65              value   = inputString.substring(j, i1).toInt();
66              //Serial.print(" Value:   ");
67              //Serial.println(value);
68              i = i1;
69              j   = i + 1;
70              assignValues(command, value, JSData);
71              break;
72             }
73          }
74        }
75      }
76    }
77    inputString   = "";
78    stringComplete = false;
79    // Serial.print(" Value: ");
80     // Serial.println(JStick.Xvalue);
81  }
82}
83
84void assignValues(String   icommand, int ivalue, JStickData &Data  ) {
85
86  if (icommand == "X")  Data.Xvalue   = ivalue;
87  if (icommand == "Y")  Data.Yvalue  = ivalue;
88  if (icommand   == "B1") Data.JButton = ivalue;
89  if (icommand == "Up") Data.Up      = ivalue;
90   if (icommand == "Do") Data.Down    = ivalue;
91  if (icommand == "Ri") Data.Right    = ivalue;
92  if (icommand == "Le") Data.Left    = ivalue;
93
94}
95

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5#include   "Twiddle.h"
6/**********************************************************************/
7Twiddle::Twiddle(   int anzParams, float p0 , float p1, float p2, float p3, float p4 ,
8                  float   p5, float p6, float p7,
9                  float dp0, float dp1, float dp2, float   dp3 , float dp4,
10                  float dp5, float dp6 , float dp7)
11/**********************************************************************/
12{
13   params[0]  = p0;
14  params[1]  = p1;
15  params[2]  = p2;
16  params[3]  =   p3;
17  params[4]  = p4;
18  params[5]  = p5;
19  params[6]  = p6;
20  params[7]   = p7;
21  dparams[0] = dp0;
22  dparams[1] = dp1;
23  dparams[2] = dp2;
24   dparams[3] = dp3;
25  dparams[4] = dp4;
26  dparams[5] = dp5;
27  dparams[6]   = dp6;
28  dparams[7] = dp7;
29  index = 0;
30  besterr = 9999.99;
31  nextStep   = 1;
32  AnzahlElements = anzParams;
33}
34/**********************************************************************/
35Twiddle::Twiddle(   int anzParams, float iparams[], float idparams[]  )   // Constructor
36/**********************************************************************/
37{   index = 0;
38  besterr = 9999.99;
39  nextStep = 1;
40  AnzahlElements = anzParams;
41}
42/**********************************************************************/
43float   Twiddle::next(float error,  float &p0, float &p1, float &p2, float &p3,
44                    float   &p4, float &p5, float &p6, float &p7)
45/**********************************************************************/
46{
47   static int skip = 0;
48
49  sum_average += abs(error);
50  cnt_average ++;
51
52   if (skip > 5 ||  // for collection some data
53      skip == 0 ) { //first Time
54     skip = 1;
55    if ( cnt_average > 0 ) {
56      average = sum_average / cnt_average;
57       sum_average = 0;
58      cnt_average = 0;
59    }
60  }
61  else {
62     skip ++;
63    return average;
64  }
65
66  if (( dparams[0] +  dparams[1]   +  dparams[2] +  dparams[3] +  ( dparams[4] +  dparams[5] +  dparams[6] +  dparams[7]))   < 0.03 ) {
67    //   Serial.println(" erledigt ");
68    p0 = params[0];
69     p1 = params[1];
70    p2 = params[2];
71    p3 = params[3];
72    p4 = params[4];
73     p5 = params[5];
74    p6 = params[6];
75    p7 = params[7];
76
77    //   try again
78    dparams[0] *= 4.0;
79    dparams[1] *= 4.0;
80    dparams[2]   *= 4.0;
81    dparams[3] *= 4.0;
82    dparams[4] *= 4.0;
83    dparams[5] *=   4.0;
84    dparams[6] *= 4.0;
85    dparams[7] *= 4.0;
86    besterr = 9999.99;
87
88     return average;  // it is done
89  }
90
91
92  if (  nextStep == 3 ) {
93     nextStep = 1;
94    if (index == AnzahlElements - 1) {
95      index = 0;
96     } else {
97      index ++;
98    }
99    params[index] +=  dparams[index];
100   }
101
102  logging(); // last step
103
104  calcCost(average);
105
106  p0 = params[0];
107   p1 = params[1];
108  p2 = params[2];
109  p3 = params[3];
110  p4 = params[4];
111   p5 = params[5];
112  p6 = params[6];
113  p7 = params[7];
114  return average;
115}
116//----------------------------------------------------------------
117void   Twiddle::calcCost(float average)
118//----------------------------------------------------------------
119//   Dieser Algorithmus sucht nun den Parameterraum intelligent ab und
120// variiert   die Schrittweite der Suche, je nachdem ob man in der Nhe
121// eines Maxima bzw.   Minima ist.
122{
123
124  switch (nextStep) {
125    case 1:
126      if (average   < besterr) {
127        // aktuelle Kosten < besterr # There was some improvement
128         besterr = average;
129        //mit grerem Schritt probieren
130        dparams[index]   *= 1.1;
131        nextStep = 3;
132      } else // # There was no improvement
133       {
134        // # Go into the other direction
135        params[index] =  params[index]   - (2 * dparams[index]);
136        nextStep = 2;
137      }
138      break;
139
140     case 2:
141      if (average < besterr) {
142        // aktuelle Kosten < besterr   # There was some improvement
143        besterr = average;
144        //mit grerem   Schritt probieren
145        dparams[index] *= 1.05;
146        nextStep = 1;
147       } else {
148        // As there was no improvement, the step size in either
149         // direction, the step size might simply be too big.
150        params[index]   += dparams[index];
151        dparams[index] *=  0.95;//an sonsten kleineren Schritt
152         nextStep = 3;
153      }
154      break;
155  }
156}
157/*------------------------------------------------------------
158   # Choose an initialization parameter vector
159  p = [0, 0, 0]
160  # Define potential   changes
161  dp = [1, 1, 1]
162  # Calculate the error
163  best_err = A(p)
164   threshold = 0.001
165  while sum(dp) > threshold:
166    for i in range(len(p)):
167         p[i] += dp[i]
168        err = A(p)
169        if err < best_err:  # There   was some improvement
170            best_err = err
171            dp[i] *= 1.1
172         else:  # There was no improvement
173            p[i] -= 2*dp[i]  # Go into   the other direction
174            err = A(p)
175            if err < best_err:   # There was an improvement
176                best_err = err
177                dp[i]   *= 1.05
178            else  # There was no improvement
179                p[i]   += dp[i]
180                # As there was no improvement, the step size in either
181                 # direction, the step size might simply be too big.
182                dp[i]   *= 0.95
183
184  https://www.gomatlab.de/twiddle-algorithmus-zum-optimieren-von-parametern-t24517.html
185    % Maximierung der Kostenfunktion!
186  while sum(dparams) > 0.01
187    for i=1:length(params)   % alle Parameter durch gehen
188        params(i)=params(i)+dparams(i);
189        %Kostenfunktion   ausrechnen
190        cfzz(it) = calcCost(params(1),params(2));
191        if cfzz(it)   > besterr
192            besterr = cfzz(it);
193            dparams(i)= dparams(i)*1.05;
194         else
195            % in andere Richtung suchen
196            params(i)=params(i)-   2*dparams(i);
197            cfzz(it) = calcCost(params(1),params(2));
198            if   cfzz(it) > besterr %wenn aktuelle Kosten hher (=gut)
199                besterr   = cfzz(it);
200                dparams(i) = dparams(i)*1.05; %mit grerem Schritt   probieren
201            else
202                params(i)=params(i)+dparams(i);
203                 dparams(i)=dparams(i)*0.95; % an sonsten kleineren Schritt
204            end
205         end
206    it = it+1;
207   end
208*/
209
210//----------------------------------------------------------------
211void   Twiddle::logging()
212//----------------------------------------------------------------
213{
214
215   Serial.print(" Step= ");
216  Serial.print(nextStep );
217  Serial.print("   Ind= ");
218  Serial.print(index );
219  Serial.print(" av= ");
220  Serial.print(average   , 4 );
221  Serial.print(" besterr ");
222  Serial.print(besterr , 4 );
223  Serial.print("   P0 ");
224  Serial.print(params[0]  , 4 );
225  Serial.print(" P1 ");
226  Serial.print(params[1]   , 4);
227  Serial.print(" P2 ");
228  Serial.print(params[2]  , 4 );
229  Serial.print("   P3 ");
230  Serial.print(params[3]  , 4 );
231  Serial.print(" P4 ");
232  Serial.print(params[4]   , 2 );
233  Serial.print(" P5 ");
234  Serial.print(params[5]  , 2);
235  Serial.print("   P6 ");
236  Serial.print(params[6]  , 2 );
237  Serial.print(" P7 ");
238  Serial.print(params[7]   , 4 );
239  Serial.println(" ");
240}
241

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4   The robot is controlled by PID controllers.
5  The PID controllers ensures that   the robot remains upright.
6  The PID controllers are implemented in the "PIDControl"   class.
7  The standard PID algorithm was slightly modified after longer test series.   To the P-component the parameter Kx multiplied
8  by the I-component was added.
9*/
10#include   "Vehicle.h"
11
12/**********************************************************************/
13Vehicle::Vehicle(DuePWMmod*   ipwm, Motor * MotorA, Motor * MotorB,  // Constructor
14                 PidControl   * PidControler, PidControl * PidControlerPos, int iPinSleepSwitch)
15/**********************************************************************/
16{
17   PinSleepSwitch = iPinSleepSwitch;
18  pinMode(PinSleepSwitch, INPUT_PULLUP);
19   pMotorA            = MotorA;
20  pMotorB            = MotorB;
21  pPidControler       = PidControler;
22  pPidControlerPos   = PidControlerPos;
23  ptrpwm             =   ipwm;
24  firstRun           = true;
25}
26
27/**********************************************************************/
28void   Vehicle::Run(MpuYawPitchRoll YawPitchRoll , int &iPositionA, int &iPositionB, JStickData   JStick)
29/**********************************************************************/
30{   /*
31    Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev => 1.8 degree per step
32     Wheel Diameter 88mm = > Distance per Pulse Dpp = d * pi / 200 =  1,3816 mm
33     Distance per Revolution =  276,32 mm
34    Max 1000 Steps per Second = 5 Revolutions   => 13816 mm Distance
35
36    "iPositionA" in  microsteps
37    8 microsteps   = 1 full Step
38    1 microstepp = 0,125 full steps
39    after division one change   in "PositionA" = 0.01 microstepp and 0,0125 full steps = 0,01727 mm
40  */
41   const int     tDelay = 10;
42
43  PositionAB = ((float(iPositionA ) + float(iPositionB   )) / 100);
44/*
45  Serial.print(iPositionA );  //grau
46  Serial.print(",");
47   Serial.print(iPositionB );  //grau
48  Serial.print(",");
49  Serial.print(HoldPosition   );  //grau
50  Serial.println(" ");
51*/
52  /********************* driverless   **************************************/
53  if (firstRun) {
54    firstRun = false;
55     skipPos = - (2 * tDelay); // first time wait a little bit longer
56    CalJstickX   = JStick.Xvalue;  // calibrate Joy Stick
57    CalJstickY = JStick.Yvalue;
58     spinningOld = false;
59    pMotorA->RunMode();
60    pMotorB->RunMode();
61     HoldPosition = PositionAB;
62  }
63  JStick.Xvalue = JStick.Xvalue - CalJstickX   ;
64  JStick.Yvalue = JStick.Yvalue - CalJstickY;
65
66  DeltaForward = float(JStick.Yvalue)   / 100.0 ;
67  DeltaTurning = float(JStick.Xvalue ) / 4.0;
68
69  if ((abs(JStick.Xvalue)   > 5 ) || (abs(JStick.Yvalue) > 5 ) )  {
70    spinning = true;
71    if (!spinningOld)   {  // changing to spinning
72    }
73    spinningOld = true;
74    HoldPosition   = PositionAB;
75  } else {
76    spinning = false;
77    if (spinningOld) {  //   changing to not spinning
78      skipPos = - (2 * tDelay); // wait a little bit   longer
79    }
80    spinningOld = false;
81  }
82
83  if (!spinning) {
84     if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be   10 times lower than Motor Control
85      skipPos = 0;
86      // PID calculation   Delta Position
87      // DeltaPos is necessary for the robot to keep its position   clean after moving forward or backward
88      DeltaPos =  pPidControlerPos->calculate(HoldPosition,   PositionAB);
89      // slowly adjust the hold position to the current position
90       // so, DeltaPos is slowly reduced and the pPidControler
91      // works   again without offset
92      HoldPosition = (0.9 * HoldPosition) + ( 0.1 * PositionAB);
93     }
94  }
95
96  // PID calculation of new steps
97  StepsA = pPidControler->calculate(YawPitchRoll.pitch,   -DeltaForward + DeltaPos );
98  StepsB = StepsA;
99
100  StepsA =  StepsA  + DeltaTurning;   // Moving right or left
101  StepsB =  StepsB  - DeltaTurning;
102
103  StepsA   = pMotorA->Run(StepsA);  // run the motors
104  StepsB = pMotorB->Run(StepsB);
105
106   uint32_t freqA_abs = abs(StepsA); // only positive Steps
107  uint32_t freqB_abs   = abs(StepsB);  // direction via PinDir
108  ptrpwm->setFreq2( freqA_abs, freqB_abs   );
109
110}
111
112/**********************************************************************/
113void   Vehicle::init()
114/**********************************************************************/
115{
116   Serial.println("Vehicle Init Motor Pointer....");
117  int ptr1 = (int) pMotorA;
118   int ptr2 = (int) pMotorB;
119  Serial.println(ptr1 , HEX);
120  Serial.println(ptr2   , HEX);
121
122  Serial.println("Vehicle Init PID Pointer....");
123  int ptr3   = (int) pPidControler;
124  int ptr4 = (int) pPidControlerPos;
125  Serial.println(ptr3   , HEX);
126  Serial.println(ptr4 , HEX);
127
128  pMotorA->init( );
129  pMotorB->init();
130
131   pMotorA->MsMicrostep();  // set microstepping
132  pMotorB->MsMicrostep();
133
134   pMotorA->SleepMode();
135  pMotorB->SleepMode();
136
137}
138/**********************************************************************/
139void   Vehicle::Stop() {
140  pMotorA->SleepMode( );
141  pMotorB->SleepMode( );
142}
143

1/* Self balancing Robot via Stepper Motor with microstepping and Digital  Motion Processing
2    Auto Tuning via Twiddle Algorithmus
3    Task Dispatcher  (function handler) via Interrupt
4    PWM Controller
5
6    written by : Rolf  Kurth in 2019
7    rolf.kurth@cron-consulting.de
8
9    The robot consists  of the main sketch and the following classes/includes:
10    -PidControl
11    -Battery
12    -DuePWMmod
13    -DueTimer
14    -Twiddle
15    -Motor
16    -Vehicle
17    -MyMPU
18    and the following includes:
19    -Config
20    -LCD
21    -PidParameter
22    -Plotter
23
24    Features of the Robot
25    -Control of the robot via Joy  Stick using Bluetooth
26    -Stepper Motor, Unipolar/Bipolar, 200 Steps/Rev, 4248mm,  4V, 1.2 A/Phase
27    -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.
28    -Task Dispatcher via Interrupt
29    -PWM Controller
30    -MPU-6050  sensor with accelerometer and gyro, using Digital Motion Processing with MPU-6050
31    -Auto Tuning via Twiddle Algorithmus
32    -Battery Control
33
34    Stepper  Motors
35
36    I decided to use Stepper engines because they offer the following  advantages:
37    -Exact positioning, no accumulated errors
38    -Holding torque  in rest position
39    -No deceleration/lag due to the moment of inertia of the  motor
40    -simple position sensing by counting PWM signal
41
42
43    Components
44    -Arduino Due
45    -SparkFun Triple Axis Accelerometer and Gyro Breakout -  MPU-6050
46     The InvenSense MPU-6050 sensor contains a MEMS accelerometer and  a MEMS gyro in a single chip, with Digital Motion Processing Unit
47    -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.
48    -Adafruit RGB Backlight  LCD - 16x2
49    -HC-05 Bluetooth Module
50    - 7.4V 2S 2200mAh 35C Li-Polymer  Lipo
51    -Joystick Shield
52    -Arduino Mega 2560 & Genuino Mega 2560 used  for the joy stick shield
53    -OpenBuilds NEMA 17 Stepper Motor Unipolar/Bipolar,  200 Steps/Rev, 42x48mm, 4V, 1200mA  https://www.pololu.com/product/1200/
54
55    Restrictions: Running only at Arduino Due
56
57   This program is free software;  you can redistribute it and/or modify it under the terms of
58   the GNU General  Public License as published by the Free Software Foundation;
59   either version  3 of the License, or (at your option) any later version.
60   This program is distributed  in the hope that it will be useful,
61   but WITHOUT ANY WARRANTY; without even  the implied warranty of MERCHANTABILITY
62   or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
63   see <http://www.gnu.org/licenses/>.
64*/
65#ifdef  __arm__
66
67// ------------------------------------------------------------------------
68//  https://www.arduino.cc/en/Reference/HomePage
69// ------------------------------------------------------------------------
70/*  MP6500 Stepper Motor Driver Carrier
71    The MP6500 offers up to 1/8-step microstepping,  operates from 4.5 V to 35 V,
72    and can deliver up to approximately 1.5 A per  phase continuously without a heat
73    sink or forced air flow (up to 2.5 A peak).  This version of the board uses an
74    on-board trimmer potentiometer for setting  the current limit.
75    https://www.pololu.com/product/2966
76*/
77/* Wireless  Bluetooth Transceiver Module mini HC-05 (Host-Slave Integration)
78*/
79
80//  ------------------------------------------------------------------------
81// Libraries
82/*Timer  Library fully implemented for Arduino DUE
83   to call a function handler every  1000 microseconds:
84   Timer3.attachInterrupt(handler).start(1000);
85   There  are 9 Timer objects already instantiated for you: Timer0, Timer1, Timer2, Timer3,  Timer4, Timer5, Timer6, Timer7 and Timer8.
86   from https://github.com/ivanseidel/DueTimer
87*/
88#include  "DueTimer.h"
89/*MPU-6050 Accelerometer + Gyro
90  The InvenSense MPU-6050 sensor  contains a MEMS accelerometer and a MEMS gyro in a single chip.
91  It is very  accurate, as it contains 16-bits analog to digital conversion hardware for each  channel.
92  Therefor it captures the x, y, and z channel at the same time. The  sensor uses the
93  I2C-bus to interface with the Arduino.
94  The sensor has  a "Digital Motion Processor" (DMP), also called a "Digital Motion Processing  Unit".
95  This DMP can be programmed with firmware and is able to do complex  calculations with the sensor values.
96  The DMP ("Digital Motion Processor")  can do fast calculations directly on the chip.
97  This reduces the load for the  microcontroller (like the Arduino).
98  I2Cdev and MPU6050 must be installed as  libraries
99*/
100/* MPU-6050 Accelerometer + Gyro
101  The InvenSense MPU-6050  sensor contains a MEMS accelerometer and a MEMS
102  gyro in a single chip. It is  very accurate, as it contains 16-bits analog
103  to digital conversion hardware  for each channel. Therefor it captures
104  the x, y, and z channel at the same  time.
105  The sensor uses the I2C-bus to interface with the Arduino.
106  The sensor  has a "Digital Motion Processor" (DMP), also called a
107  "Digital Motion Processing  Unit".
108  The DMP ("Digital Motion Processor") can do fast calculations directly  on the chip.
109  This reduces the load for the Arduino.
110  DMP is used in this  Program
111  https://playground.arduino.cc/Main/MPU-6050
112  MPU-6050 I2Cdev library  collection
113  MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation#
114  Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
115  by Jeff Rowberg <jeff@rowberg.net> */
116#include "I2Cdev.h"
117#include "MPU6050_6Axis_MotionApps20.h"
118#if  I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
119#include "Wire.h"
120#endif
121MPU6050  mpu; // create object mpu
122// ------------------------------------------------------------------------
123/*  Create PWM Controller
124   Purpose: for Stepper PWM Control
125   Setup one or  two unique PWM frequenices directly in sketch program,
126   set PWM duty cycle,  and stop PWM function.
127   Applies to Arduino-Due board, PWM pins 6, 7, 8 & 9,  all others ignored
128   Written By:  randomvibe
129   modified by : Rolf Kurth
130   https://github.com/cloud-rocket/DuePWM
131*/
132// ------------------------------------------------------------------------
133#include  "DuePWMmod.h"
134DuePWMmod pwm; // create object pwm
135// ------------------------------------------------------------------------
136#include  <LiquidCrystal.h> // LiquidCrystal(rs, enable, d4, d5, d6, d7)
137LiquidCrystal  lcd(9, 8, 4, 5, 10, 11); //create LiquidCrystal object
138// ------------------------------------------------------------------------
139//  own classes in Tabs
140// ------------------------------------------------------------------------
141#include  "Config.h"
142#include "Battery.h"  // Object Measurement
143Battery myBattery(VoltPin);  // create Object Measurement
144// ------------------------------------------------------------------------
145//  create PID Controller
146// ------------------------------------------------------------------------
147#include  "PidControl.h"
148PidParameter PidParams;
149PidParameterPos PidParamsPos;
150
151PidControl  PidControler(PidParams);
152PidControl PidControlerPos(PidParamsPos);
153// ------------------------------------------------------------------------
154//  create objects for Motor 1 and Motor 2
155// ------------------------------------------------------------------------
156#include  "Motor.h"
157Motor MotorA(&pwm, PinDirMotorA, PinStepMotorA, PinMs1MotorA,
158             PinMs2MotorA, PinSleepA, rechterMotor); // create MotorA
159Motor MotorB(&pwm,  PinDirMotorB, PinStepMotorB, PinMs1MotorB,
160             PinMs2MotorB, PinSleepB,  linkerMotor); // create MotorB
161
162// ------------------------------------------------------------------------
163//  create Robot
164// ------------------------------------------------------------------------
165#include  "Vehicle.h"
166Vehicle Robot = Vehicle(&pwm, &MotorA, &MotorB, &PidControler,  &PidControlerPos, PinSleepSwitch);
167
168// ------------------------------------------------------------------------
169/*  Twiddle Auto Tuning ( params,  dparams);
170    https://martin-thoma.com/twiddle/
171    Twiddle is an algorithm that tries to find a good choice of parameters p for  an algorithm A that returns an error.
172*/
173#include "Twiddle.h"
174//const  bool AutoTuning = true;  // sets Twiddle on
175const bool AutoTuning = false;
176//  Auto Tuning both PID Controler
177//Twiddle Tuning ( 7, PidParams.Kp , PidParams.Ki  , PidParams.Kd ,  PidParams.Ka , PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd  , PidParamsPos.Ka,
178//                 0.1,             0.1,           0.01,            0.01,            0.01,              0.01,              0.01,            0.01);
179//  Auto Tuning only Position Controler
180Twiddle Tuning ( 3,  PidParamsPos.Kp , PidParamsPos.Ki  , PidParamsPos.Kd , PidParamsPos.Ka, PidParams.Kp , PidParams.Ki , PidParams.Kd  ,  PidParams.Ka ,
181                 0.005,             0.001,           0.001,            0.001,            0.001,              0.01,              0.01,            0.01);
182
183//  ------------------------------------------------------------------------
184// Declaration
185//  ------------------------------------------------------------------------
186int             LoopTimeMsec = 12;
187float           LoopTimeMicrosec = LoopTimeMsec  * 1000;
188unsigned long   ProgStartTime;  //general Start Time
189const int       StartDelay  = 21000; // msec
190unsigned long   CheckTimeStart;
191int             CheckTimeEnd;
192
193boolean          Running        = false;     // Speed Control is running
194float            TuningValue;
195float            setPoint = 0;
196float            Calibration = -2.8 ; //-3.2;
197float            Voltage;
198float            error ;
199float            average;
200
201volatile  bool    mpuInterrupt = false;  // indicates whether MPU interrupt pin has gone high
202volatile  boolean PlotterFlag;           // Interrupt serieller Plotte
203volatile boolean  RunFlag;               // Interrupt Vicle run
204volatile boolean LcdFlag;               //  Interrupt LCD Display
205volatile int     PositionA;
206volatile int     PositionB;
207boolean          First = true;
208
209uint32_t         duty;
210uint32_t         period;
211uint32_t         Speed ;
212int              FifoOverflowCnt;
213
214JStickData JStick;  // create Joy Stick data
215
216MpuYawPitchRoll YawPitchRoll;  // create Structure  YawPitchRoll
217
218/**********************************************************************/
219void  setup()
220/**********************************************************************/
221{
222
223  ProgStartTime = millis();
224
225  Serial.begin  (115200); // initialize serial  communication
226  while (!Serial); //
227  Serial.print("Sketch:   ");   Serial.println(__FILE__);
228  Serial.print("Uploaded: ");   Serial.println(__DATE__);
229
230  Serial1.begin  (9600);  // initialize Bluetooth communicat
231
232  // join I2C bus (I2Cdev library  doesn't do this automatically)
233  Wire.begin();
234  Wire.setClock(400000); //  400kHz I2C clock. Comment this line if having compilation difficulties
235
236  Serial.println(F("Setup..."));
237
238  // LCD initialisieren
239  lcd.begin(16, 2);   lcd.clear( );
240
241  pinMode(PinSleepSwitch,  INPUT_PULLUP);
242
243  // initialize device
244  Serial.println(F("Initializing  I2C devices..."));
245
246  PidControler.changePIDparams(PidParams);       // PID  Parameter setzen
247  PidControlerPos.changePIDparams(PidParamsPos); // PID Parameter  setzen
248
249  // MP 6500 Stepper Motor Driver Carrier
250  digitalWrite(PinSleepA,  LOW);  //  The defaultSLEEPstate prevents the driver from operating
251  digitalWrite(PinSleepB,  LOW);  // this pin must be high to enable the driver
252
253  /* Setup PWM
254    Once  the pwm object has been defined you start using it providing its
255    PWM period  and its initial duty value (pulse duration).
256  */
257  //-----------------------------------------------------
258  pwm.pinFreq( PinStepMotorA, rechterMotor);  // Pin 6 freq set to "pwm_freq1"  on clock A
259  pwm.pinFreq( PinStepMotorB, linkerMotor);   // Pin 7 freq set to  "pwm_freq2" on clock B
260
261  // Write PWM Duty Cycle Anytime After PWM Setup
262  //-----------------------------------------------------
263  uint32_t pwm_duty  = 127; // 50% duty cycle
264  pwm.pinDuty( PinStepMotorA, pwm_duty );  // 50% duty  cycle on Pin 6
265  pwm.pinDuty( PinStepMotorB, pwm_duty );  // 50% duty cycle on  Pin 7
266
267  /**********************************************************************/
268  // for Checking the position from Stepper Motor
269  attachInterrupt(digitalPinToInterrupt(PinStepMotorA),  ISR_PWMA, RISING );
270  attachInterrupt(digitalPinToInterrupt(PinStepMotorB), ISR_PWMB,  RISING );
271  /**********************************************************************/
272  /* Timer Library fully implemented for Arduino DUE
273    https://github.com/ivanseidel/DueTimer
274    To call a function handler every 1000 microseconds:
275    Timer3.attachInterrupt(handler).start(1000);
276    or:
277    Timer3.attachInterrupt(handler).setPeriod(1000).start();
278    or,  to select whichever available timer:
279    Timer.getAvailable().attachInterrupt(handler).start(1000);
280    To call a function handler 10 times a second:
281    Timer3.attachInterrupt(handler).setFrequency(10).start();
282
283    Dispatching taks for Plotter, LCD Display and Robot
284  */
285  Timer4.attachInterrupt(PlotterISR).setPeriod(8000).start();  // Plotter
286  Timer3.attachInterrupt(LcdTimer).setPeriod(500000).start(); // LCD  Display 500 msec
287  Timer6.attachInterrupt(RobotFlag).setPeriod(LoopTimeMicrosec).start();  // RobotFlag  10 msec
288  /**********************************************************************/
289  YawPitchRoll.pitch = 0;
290  Speed = 0;
291  duty = period / 2;
292  // configure  LED for output
293  pinMode(LED_PIN, OUTPUT);
294  pinMode(MpuIntPin, OUTPUT);
295  Robot.init(  );  // Init Robot
296  MpuInit();       // Init MPU
297}
298/**********************************************************************/
299void  loop()
300/**********************************************************************/
301{
302  if (First) {
303    setPoint = 0;
304    First = false;
305    YawPitchRoll.pitch  = 0;
306    MotorA.SleepMode();
307    MotorB.SleepMode();
308    PositionA = 0;
309    PositionB = 0;
310    if (!( myBattery.VoltageCheck()))  ErrorVoltage(); //  Check Voltage of Batterie
311  }
312  // If PinSleepSwitch is on, release Motors
313  if (!digitalRead(PinSleepSwitch)) {
314    MotorA.RunMode();
315    MotorB.RunMode();
316  } else {
317    MotorA.SleepMode();
318    MotorB.SleepMode();
319  }
320
321  BTRead( JStick  ); // read JoyStick Data
322
323  // --------------------- Sensor  aquisition  -------------------------
324  YawPitchRoll = ReadMpuRunRobot() ; //  wait for MPU interrupt or extra packet(s) available
325  // --------------------------------------------------------------#
326  if (!Running) {
327    if ( ( abs(YawPitchRoll.pitch) < 15.0 )  && ( millis()  > ( ProgStartTime +  StartDelay)))  {
328      Running = true; // after Delay time  set Running true
329      MotorA.RunMode();
330      MotorB.RunMode();
331    }
332  }
333
334  if ( ( abs(YawPitchRoll.pitch) > 15.0 ) && ( Running == true )) {
335    ErrorHandler1();
336  }
337  // --------------------------------------------------------------
338  // Run Robot
339  // --------------------------------------------------------------
340  if ( RunFlag ) {
341    RunFlag = false;
342
343    int PositionAtemp;
344    int  PositionBtemp;
345
346    manuelPidTuning(); // PID Parameter tuning
347
348    if  (Running) {
349
350      //because conflicting declaration 'volatile int PositionA'
351      PositionBtemp = PositionB ;
352      PositionAtemp = PositionA ;
353      Robot.Run(  YawPitchRoll, PositionAtemp , PositionBtemp,  JStick ); //   Robot.Run
354
355      if  (AutoTuning)  PIDAutoTuning(); // auto Tuning via Twiddle
356
357    }
358  }
359  // --------------------------------------------------------------
360  //  SeriellerPlotter
361  // --------------------------------------------------------------
362  if ( PlotterFlag  ) {
363    PlotterFlag = false;
364    if (!(AutoTuning))   SeriellerPlotter(JStick);
365  }
366  // --------------------------------------------------------------
367  //  Show Data via LCD and Check Battery
368  // --------------------------------------------------------------
369  if (LcdFlag) {
370    LcdFlag = false;
371    Voltage =  myBattery.Voltage;
372    if (!( myBattery.VoltageCheck()))  ErrorVoltage();
373    CheckTimeStart = micros();  // Test Timing
374    LCD_show();   // Testausgaben LCD
375    CheckTimeEnd =  ( (micros() - CheckTimeStart)) ;
376  }
377}
378
379//---------------------------------------------------------------------/
380//  Interrupt Service Routinen
381//---------------------------------------------------------------------/
382//  LCD Display
383//  ---------------------------------------------------------------------*/
384void  LcdTimer() {
385  LcdFlag = true;
386}
387/**********************************************************************/
388//  Plotter ISR Routine
389/**********************************************************************/
390void  PlotterISR() {
391  PlotterFlag = true;
392}
393/**********************************************************************/
394//  Timer6 Robot ISR Routine
395/**********************************************************************/
396void  RobotFlag()  {
397  RunFlag = true;
398}
399// ------------------------------------------------------------------------
400/*  MPU 6050    ISR Routine
401  The FIFO buffer is used together with the interrupt  signal.
402  If the MPU-6050 places data in the FIFO buffer, it signals the Arduino
403  with the interrupt signal so the Arduino knows that there is data in
404  the  FIFO buffer waiting to be read.*/
405void dmpDataReady() {
406  mpuInterrupt = true;  // indicates whether MPU interrupt pin has gone high
407  digitalWrite(MpuIntPin,  !digitalRead(MpuIntPin)); // Toggle  Pin for reading the Frequenzy
408}
409
410//---------------------------------------------------------------------/
411void  ISR_PWMA() {
412  if (MotorA.DirForward )   {
413    PositionA ++;
414  } else {
415    PositionA --;
416  }
417}
418//---------------------------------------------------------------------/
419void  ISR_PWMB() {
420  //  if ( PidControlerPos.DirForward ) {
421  if ( MotorB.DirForward  ) {
422    PositionB ++;
423  }  else  {
424    PositionB --;
425  }
426}
427
428//  --------------------------------------------------------------
429void  manuelPidTuning()
430//  --------------------------------------------------------------
431// remove comment  to get Tuning Value via Poti 10Kohm
432{
433  TuningValue = (float(analogRead(TuningPin)));
434  // manuel Tuning Motor
435  // PidParams.Kp = TuningValue  = TuningValue / 50;
436  // PidParams.Kd = TuningValue  = TuningValue / 5000;
437  // PidParams.Ki   =  TuningValue  = TuningValue / 100;
438  // PidParams.Ka = TuningValue  = TuningValue  / 500;
439  // PidControler.changePIDparams(PidParams); // PID Parameter setzen
440
441
442  // manuel Tuning Position
443  // PidParamsPos.Ki = TuningValue  = TuningValue  / 10000;
444  // PidParamsPos.Kp = TuningValue  = TuningValue / 5000;
445  // PidParamsPos.Ka  = TuningValue  = TuningValue / 5000;
446  // PidParamsPos.Kd = TuningValue  = TuningValue  / 10000;
447  // PidControlerPos.changePIDparams(PidParamsPos); // PID Parameter  setzen
448}
449// --------------------------------------------------------------
450void  PIDAutoTuning()  //Twiddle Auto Tuning
451// --------------------------------------------------------------
452{
453  static int skipT = 0;
454  skipT++;
455  if (skipT > 10) {
456    skipT = 11;
457
458    // average = Tuning.next( Robot.PositionAB, PidParams.Kp , PidParams.Ki , PidParams.Kd  ,  PidParams.Ka , PidParamsPos.Kp , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka  );
459
460    average = Tuning.next( (Robot.HoldPosition - Robot.PositionAB), PidParamsPos.Kp  , PidParamsPos.Ki , PidParamsPos.Kd , PidParamsPos.Ka, PidParams.Kp , PidParams.Ki  , PidParams.Kd ,  PidParams.Ka  );
461
462    PidControler.changePIDparams(PidParams);  // PID Parameter setzen
463    PidControlerPos.changePIDparams(PidParamsPos); //  PID Parameter setzen
464  }
465}
466
467/**********************************************************************/
468void  ErrorVoltage()
469/**********************************************************************/
470{
471  lcd.clear();
472  lcd.setCursor(0, 0);
473  lcd.print( "Akku Low!! "  );
474  lcd.setCursor(0, 1);
475  lcd.print( myBattery.Voltage );
476  lcd.print( " Volt"  );
477  MotorA.SleepMode( );
478  MotorB.SleepMode( );
479  do  {} while ( 1 ==  1); // never ending
480}
481// --------------------------------------------------------------
482void  ErrorHandler1()
483// --------------------------------------------------------------
484{
485  Serial.println(F("Robot tilt!"));
486  lcd.clear();
487  lcd.setCursor(0, 0);
488  lcd.print("Robot tilt!");
489  lcd.setCursor(0, 1);
490  lcd.print("please  Restart!");
491  MotorA.SleepMode( );
492  MotorB.SleepMode( );
493  do {} while  (1 == 1); // never ending
494}
495#else
496#error Oops! Trying to include Robot  to another device!?
497#endif
498

1/* Self balancing Robot via Stepper Motor with microstepping and Digital   Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4*/
5
6#ifndef   Vehicle_h
7#define  Vehicle_h
8#include "Motor.h"
9#include "Arduino.h"
10#include   "Config.h"
11/**********************************************************************/
12class   Vehicle
13/**********************************************************************/
14{
15   public:
16    Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB,
17            PidControl   * PidControler, PidControl * PidControlerPos, int iPinSleepSwitch);  // Constructor
18
19     Motor *pMotorA;
20    Motor *pMotorB;
21    PidControl *pPidControler;
22     PidControl *pPidControlerPos;
23
24    void Run(MpuYawPitchRoll YawPitchRoll   ,  int &PositionA, int &PositionB, JStickData JStick);
25    void init();
26
27     void Stop( );
28
29    float DeltaPos       = 0.0;
30    float DeltaForward    = 0.0;
31    float DeltaTurning   = 0.0;
32    float PositionAB     = 0.0;
33     float HoldPosition   = 0.0;
34    float StepsA         = 0.0;
35    float   StepsB         = 0.0;
36    float CalJstickX     = 0;
37    float CalJstickY     =   0;
38    bool  spinning       = false;
39    bool  spinningOld    = false ;
40     bool  moving         = false;
41    int   skipPos; // wait befor starting position   controll
42    int freqA;
43    int freqB;
44
45
46  protected:
47    DuePWMmod   *ptrpwm;
48    int PinSleepSwitch;
49    bool firstRun = true;
50
51};
52#endif
53

1/* Self balancing Robot via Stepper Motor with microstepping and Digital
2  Motion Processing  
3 *  written by : Rolf Kurth in 2019
4 *  rolf.kurth@cron-consulting.de
5
6  */
7 
8#ifndef Battery_h
9#define  Battery_h
10#include "Arduino.h"
11//**********************************************************************
12  /
13class Battery
14///**********************************************************************/
15{
16
17  public:
18    Battery(int PIN)  ;  // Constructor
19    //   Battery(int _Pin)
20  ;  // Constructor
21    bool    VoltageCheck();
22    float   VoltageRead();
23
24    float   Voltage;
25    int     _VoltPin;
26};
27#endif
28

1/* Self balancing Robot via Stepper Motor with microstepping and Digital  Motion Processing
2    written by : Rolf Kurth in 2019
3    rolf.kurth@cron-consulting.de
4    Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
5    by Jeff Rowberg <jeff@rowberg.net>
6
7    MPU-6050 Accelerometer + Gyro
8
9    The MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single  chip. 
10    It is very accurate, as it contains 16-bits analog to digital conversion  hardware for each channel. 
11    Therefor it captures the x, y, and z channel  at the same time. The sensor uses the I2C -bus to interface with the Arduino.
12
13    I used Digital Motion Processing with the MPU-6050 sensor, to do fast calculations  directly on the chip. 
14    This reduces the load for the Arduino.  
15
16    https://playground.arduino.cc/Main/MPU-6050
17
18    Because of the orientation of my board, I used yaw/pitch/roll angles (in degrees)  calculated from the quaternions coming from the FIFO. 
19    By reading Euler angle  I got problems with Gimbal lock.    
20*/
21
22/*    0x02,   0x16,   0x02,   0x00,  0x07                // D_0_22 inv_set_fifo_rate
23
24    // This very last 0x01  WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
25    // 0x01  is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
26    // DMP output frequency is calculated easily using this equation: (200Hz / (1  + value))
27
28    // It is important to make sure the host processor can keep  up with reading and processing
29    // the FIFO output at the desired rate. Handling  FIFO overflow cleanly is also a good idea.
30*/
31// ------------------------------------------------------------------------
32//  orientation/motion vars
33// ------------------------------------------------------------------------
34Quaternion  q;           // [w, x, y, z]         quaternion container
35VectorInt16 aa;         //  [x, y, z]            accel sensor measurements
36VectorInt16 aaReal;     // [x,  y, z]            gravity-free accel sensor measurements
37VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
38VectorFloat  gravity;    // [x, y, z]            gravity vector
39float euler[3];         //  [psi, theta, phi]    Euler angle container
40float ypr[3];           // [yaw, pitch,  roll]   yaw/pitch/roll container and gravity vector
41// ------------------------------------------------------------------------
42//  MPU control/status vars
43// ------------------------------------------------------------------------
44bool     dmpReady = false;  // set true if DMP init was successful
45uint8_t  mpuIntStatus;      // holds actual interrupt status byte from MPU
46uint8_t  devStatus;         //  return status after each device operation (0 = success, !0 = error)
47uint16_t  packetSize;        // expected DMP packet size (default is 42 bytes)
48uint16_t  fifoCount;         // count of all bytes currently in FIFO
49uint8_t  fifoBuffer[64];    // FIFO storage buffer
50
51
52
53// --------------------- Sensor aquisition  -------------------------
54MpuYawPitchRoll ReadMpuRunRobot()
55// ---------------------  Sensor aquisition  -------------------------
56// wait for MPU interrupt or extra  packet(s) available
57// --------------------------------------------------------------------
58{
59  if (mpuInterrupt or fifoCount >= packetSize) {
60    if  (mpuInterrupt) mpuInterrupt  = false;  // reset interrupt flag
61    digitalWrite(LED_PIN, HIGH); // blink LED  to indicate activity
62    //    Angle = ReadMpuRunRobot6050() - CalibrationAngle;
63    YawPitchRoll = ReadMpuRunRobot6050();
64    YawPitchRoll.pitch  +=  Calibration;
65    // blinkState = !blinkState;
66    digitalWrite(LED_PIN, LOW);
67  }
68  return  YawPitchRoll ;
69}
70// --------------------------------------------------------------------  -
71MpuYawPitchRoll ReadMpuRunRobot6050()
72// --------------------------------------------------------------------  -
73{
74  static float pitch_old;
75  static float yaw_old;
76  static float  yaw_tmp;
77  static float yaw_delta;
78  //  static float pitch;
79
80  mpuIntStatus  = mpu.getIntStatus();
81
82  // get current FIFO count
83  fifoCount = mpu.getFIFOCount();
84  // check for overflow (this should never happen unless our code is too inefficient)
85  if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
86    // reset so we can continue  cleanly
87    //    mpu.setDMPEnabled(false);
88    mpu.resetFIFO();
89    FifoOverflowCnt  ++;
90    fifoCount = 0;
91    YawPitchRoll.pitch = pitch_old;
92    return YawPitchRoll;
93
94  }
95  // otherwise, check for DMP data ready interrupt (this should happen frequently)
96  // Register 58  lnterrupt Status INT_STATUS
97  // MPU-6500 Register Map and  Descriptions Revision  2.1
98  // Bit [1] DMP_INT This bit automatically sets to  1 when the DMP interrupt has been generated.
99  // Bit [0] RAW_DATA_RDY_INT1 Sensor  Register Raw Data sensors are updated and Ready to be read.
100  if ((mpuIntStatus)  & 0x02 || (mpuIntStatus & 0x01)) {
101    // wait for correct available data length,  should be a VERY short wait
102    while (fifoCount < packetSize)  fifoCount = mpu.getFIFOCount();
103
104    while (fifoCount > 0) {
105      // read a packet from FIFO
106      mpu.getFIFOBytes(fifoBuffer,  packetSize);
107
108      // track FIFO count here in case there is > 1 packet available
109      // (this lets us immediately read more without waiting for an interrupt)
110      fifoCount = mpu.getFIFOCount();
111      //      fifoCount -= packetSize;
112    }
113    // the yaw/pitch/roll angles (in degrees) calculated from the quaternions  coming
114    // from the FIFO. Note this also requires gravity vector calculations.
115    // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
116    //  more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
117
118    mpu.dmpGetQuaternion(&q,  fifoBuffer);
119    mpu.dmpGetGravity(&gravity, &q);
120    // mpu.dmpGetEuler(euler,  &q);
121    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
122
123    YawPitchRoll.pitch  = -(ypr[1] * 180 / M_PI); //pitch
124
125    yaw_tmp  = (abs(ypr[0] * 180 / M_PI));
126    yaw_delta = yaw_tmp - yaw_old;
127    yaw_old = yaw_tmp;
128    YawPitchRoll.yaw  += yaw_delta;
129
130    // actual quaternion components in a [w, x, y, z]
131    // YawPitchRoll.pitch = (q.y) * 180;
132    // YawPitchRoll.yaw = (q.z );
133    // YawPitchRoll.yaw =  mapfloat(YawPitchRoll.yaw , -1.0, 1.0, 0.0, 360.0);
134  }
135  pitch_old = YawPitchRoll.pitch ;
136  return  YawPitchRoll ;
137}
138//  --------------------------------------------------------------
139void MpuInit()
140//  --------------------------------------------------------------
141// MPU6050_6Axis_MotionApps20.h
142//  0x02,   0x16,   0x02,   0x00, 0x09  => 50msec 20 Hz
143// This very last 0x01 WAS  a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
144// 0x01 is 100Hz.  Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
145// DMP  output frequency is calculated easily using this equation: (200Hz / (1 + value))
146//  It is important to make sure the host processor can keep up with reading and processing
147//  the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good  idea.
148
149{
150  // after Reset of Arduino there is no Reset of MPU
151  pinMode(MpuInterruptPin,  INPUT);
152
153  // verify connection
154  Serial.println(F("Testing device connections..."));
155  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050  connection failed"));
156
157  // load and configure the DMP
158  Serial.println(F("Initializing  DMP..."));
159  devStatus = mpu.dmpInitialize();
160  // supply your own gyro offsets  here, scaled for min sensitivity
161  mpu.setXGyroOffset(220);
162  mpu.setYGyroOffset(76);
163  mpu.setZGyroOffset(-84);
164  mpu.setZAccelOffset(1788); //
165
166  // make sure  it worked (returns 0 if so)
167  if (devStatus == 0) {
168    // turn on the DMP,  now that it's ready
169    Serial.println(F("Enabling DMP..."));
170    mpu.setDMPEnabled(true);
171
172    // enable Arduino interrupt detection
173    Serial.println(F("Enabling interrupt  detection (Arduino external interrupt 0)..."));
174    attachInterrupt(digitalPinToInterrupt(MpuInterruptPin),  dmpDataReady, RISING);
175    mpuIntStatus = mpu.getIntStatus();
176
177    // set  our DMP Ready flag so the main loop() function knows it's okay to use it
178    Serial.println(F("DMP  ready! Waiting for first interrupt..."));
179    dmpReady = true;
180
181    //  get expected DMP packet size for later comparison
182    packetSize = mpu.dmpGetFIFOPacketSize();
183    //    mpu.resetFIFO();  // after Reset importand
184
185  } else {
186    //  ERROR!
187    // 1 = initial memory load failed
188    // 2 = DMP configuration  updates failed
189    // (if it's going to break, usually the code will be 1)
190    Serial.print(F("DMP Initialization failed (code "));
191    Serial.print(devStatus);
192    Serial.println(F(")"));
193    lcd.clear();
194    lcd.setCursor(0, 0);
195    lcd.print( "Error MPU6050 "  );
196    do  {} while ( 1 == 1);
197  }
198}
199
200float  mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
201{
202  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
203}
204