two wheeled self balancing robot with stepper motor.

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 Battery_h
7#define  Battery_h
8#include "Arduino.h"
9//**********************************************************************  /
10class Battery
11///**********************************************************************/
12{
13  public:
14    Battery(int PIN)  ;  // Constructor
15    //   Battery(int _Pin)  ;  // Constructor
16    bool    VoltageCheck();
17    float   VoltageRead();
18    float   Voltage;
19    int     _VoltPin;
20};
21#endif
22

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(PidParameterDi     Params) ;
20    PidControl(PidParameterRot   Params) ;
21    PidControl* getInstance();
22
23     /* C++ Overloading // changePIDparams = different PidParameter !!!!
24      An   overloaded declaration is a declaration that is declared with the same
25      name   as a previously declared declaration in the same scope, except that
26      both   declarations have different arguments and obviously different
27      definition   (implementation).
28    */
29    void     changePIDparams(PidParameter Params);
30     void     changePIDparams(PidParameterDi Params);
31    void     changePIDparams(PidParameterRot   Params);
32
33    float    calculate (float iAngle, float isetPoint );
34    float     getSteps ();
35    void     reset ();
36    void     test      ( );
37    float     DeltaKp(float iError);
38    float    eSpeed;
39    float    pTerm;
40    float     iTerm;
41    float    dTerm;
42    float    error;
43    float    integrated_error;
44     volatile bool  DirForward;
45    float    Last_error;
46
47  protected:
48     struct   PidParameter params;
49    float    LastK;
50    float    K;
51     float    Ki;
52    float    Kd;
53    float    Kp;
54    float    Ka;
55     float    Kx;
56    //   float    Last_angle;
57    float    timeChange ;
58     unsigned long Last_time;
59    unsigned long Now;
60    int      ptr;
61     PidControl* pPID;
62    bool first ;
63};
64
65#endif
66

1/*
2  DueTimer.cpp - Implementation of Timers defined on DueTimer.h
3   For instructions, go to https://github.com/ivanseidel/DueTimer
4
5  Created   by Ivan Seidel Gomes, March, 2013.
6  Modified by Philipp Klaus, June 2013.
7   Thanks to stimmer (from Arduino forum), for coding the "timer soul" (Register   stuff)
8  Released into the public domain.
9*/
10
11#include <Arduino.h>
12#if   defined(_SAM3XA_)
13#include "DueTimer.h"
14
15const DueTimer::Timer DueTimer::Timers[NUM_TIMERS]   = {
16  {TC0, 0, TC0_IRQn},
17  {TC0, 1, TC1_IRQn},
18  {TC0, 2, TC2_IRQn},
19   {TC1, 0, TC3_IRQn},
20  {TC1, 1, TC4_IRQn},
21  {TC1, 2, TC5_IRQn},
22  {TC2,   0, TC6_IRQn},
23  {TC2, 1, TC7_IRQn},
24  {TC2, 2, TC8_IRQn},
25};
26
27//   Fix for compatibility with Servo library
28#ifdef USING_SERVO_LIB
29// Set callbacks   as used, allowing DueTimer::getAvailable() to work
30void (*DueTimer::callbacks[NUM_TIMERS])()   = {
31  (void (*)()) 1, // Timer 0 - Occupied
32  (void (*)()) 0, // Timer 1
33   (void (*)()) 1, // Timer 2 - Occupied
34  (void (*)()) 1, // Timer 3 - Occupied
35   (void (*)()) 1, // Timer 4 - Occupied
36  (void (*)()) 1, // Timer 5 - Occupied
37   (void (*)()) 0, // Timer 6
38  (void (*)()) 0, // Timer 7
39  (void (*)()) 0   // Timer 8
40};
41#else
42void (*DueTimer::callbacks[NUM_TIMERS])() = {};
43#endif
44float   DueTimer::_frequency[NUM_TIMERS] = { -1, -1, -1, -1, -1, -1, -1, -1, -1};
45
46/*
47   Initializing all timers, so you can use them like this: Timer0.start();
48*/
49DueTimer   Timer(0);
50
51DueTimer Timer1(1);
52// Fix for compatibility with Servo library
53#ifndef   USING_SERVO_LIB
54DueTimer Timer0(0);
55DueTimer Timer2(2);
56DueTimer Timer3(3);
57DueTimer   Timer4(4);
58DueTimer Timer5(5);
59#endif
60DueTimer Timer6(6);
61DueTimer Timer7(7);
62DueTimer   Timer8(8);
63
64DueTimer::DueTimer(unsigned short _timer) : timer(_timer) {
65   /*
66    The constructor of the class DueTimer
67  */
68}
69
70DueTimer DueTimer::getAvailable(void)   {
71  /*
72    Return the first timer with no callback set
73  */
74
75  for   (int i = 0; i < NUM_TIMERS; i++) {
76    if (!callbacks[i])
77      return DueTimer(i);
78   }
79  // Default, return Timer0;
80  return DueTimer(0);
81}
82
83DueTimer&   DueTimer::attachInterrupt(void (*isr)()) {
84  /*
85    Links the function passed   as argument to the timer of the object
86  */
87
88  callbacks[timer] = isr;
89
90   return *this;
91}
92
93DueTimer& DueTimer::detachInterrupt(void) {
94  /*
95     Links the function passed as argument to the timer of the object
96  */
97
98   stop(); // Stop the currently running timer
99
100  callbacks[timer] = NULL;
101
102   return *this;
103}
104
105DueTimer& DueTimer::start(float  microseconds) {
106   /*
107    Start the timer
108    If a period is set, then sets the period and   start the timer
109  */
110
111  if (microseconds > 0)
112    setPeriod(microseconds);
113
114   if (_frequency[timer] <= 0)
115    setFrequency(1);
116
117  NVIC_ClearPendingIRQ(Timers[timer].irq);
118   NVIC_EnableIRQ(Timers[timer].irq);
119
120  TC_Start(Timers[timer].tc, Timers[timer].channel);
121
122   return *this;
123}
124
125DueTimer& DueTimer::stop(void) {
126  /*
127    Stop   the timer
128  */
129
130  NVIC_DisableIRQ(Timers[timer].irq);
131
132  TC_Stop(Timers[timer].tc,   Timers[timer].channel);
133
134  return *this;
135}
136
137uint8_t DueTimer::bestClock(float   frequency, uint32_t& retRC) {
138  /*
139    Pick the best Clock, thanks to Ogle   Basil Hall!
140
141    Timer   Definition
142    TIMER_CLOCK1  MCK /  2
143    TIMER_CLOCK2   MCK /  8
144    TIMER_CLOCK3  MCK / 32
145    TIMER_CLOCK4  MCK /128
146  */
147   const struct {
148    uint8_t flag;
149    uint8_t divisor;
150  } clockConfig[]   = {
151    { TC_CMR_TCCLKS_TIMER_CLOCK1,   2 },
152    { TC_CMR_TCCLKS_TIMER_CLOCK2,    8 },
153    { TC_CMR_TCCLKS_TIMER_CLOCK3,  32 },
154    { TC_CMR_TCCLKS_TIMER_CLOCK4,   128 }
155  };
156  float ticks;
157  float error;
158  int clkId = 3;
159  int bestClock   = 3;
160  float bestError = 9.999e99;
161  do
162  {
163    ticks = (float) VARIANT_MCK   / frequency / (float) clockConfig[clkId].divisor;
164    // error = abs(ticks -   round(ticks));
165    error = clockConfig[clkId].divisor * abs(ticks - round(ticks));   // Error comparison needs scaling
166    if (error < bestError)
167    {
168      bestClock   = clkId;
169      bestError = error;
170    }
171  } while (clkId-- > 0);
172  ticks   = (float) VARIANT_MCK / frequency / (float) clockConfig[bestClock].divisor;
173   retRC = (uint32_t) round(ticks);
174   return clockConfig[bestClock].flag;  //Rolf
175   
176}
177
178
179DueTimer& DueTimer::setFrequency(float  frequency) {
180  /*
181     Set the timer frequency (in Hz)
182  */
183
184  // Prevent negative frequencies
185   if (frequency <= 0) {
186    frequency = 1;
187  }
188
189  // Remember the frequency   � see below how the exact frequency is reported instead
190  //_frequency[timer]   = frequency;
191
192  // Get current timer configuration
193  Timer t = Timers[timer];
194
195   uint32_t rc = 0;
196  uint8_t clock;
197
198  // Tell the Power Management Controller   to disable
199  // the write protection of the (Timer/Counter) registers:
200  pmc_set_writeprotect(false);
201
202   // Enable clock for the timer
203  pmc_enable_periph_clk((uint32_t)t.irq);
204
205   // Find the best clock for the wanted frequency
206  clock = bestClock(frequency,   rc);
207
208  switch (clock) {
209    case TC_CMR_TCCLKS_TIMER_CLOCK1:
210      _frequency[timer]   = (float )VARIANT_MCK / 2.0 / (float )rc;
211      break;
212    case TC_CMR_TCCLKS_TIMER_CLOCK2:
213       _frequency[timer] = (float )VARIANT_MCK / 8.0 / (float )rc;
214      break;
215     case TC_CMR_TCCLKS_TIMER_CLOCK3:
216      _frequency[timer] = (float )VARIANT_MCK   / 32.0 / (float )rc;
217      break;
218    default: // TC_CMR_TCCLKS_TIMER_CLOCK4
219       _frequency[timer] = (float )VARIANT_MCK / 128.0 / (float )rc;
220      break;
221   }
222
223  // Set up the Timer in waveform mode which creates a PWM
224  // in   UP mode with automatic trigger on RC Compare
225  // and sets it up with the determined   internal clock as clock input.
226  TC_Configure(t.tc, t.channel, TC_CMR_WAVE |   TC_CMR_WAVSEL_UP_RC | clock);
227  // Reset counter and fire interrupt when RC value   is matched:
228  TC_SetRC(t.tc, t.channel, rc);
229  // Enable the RC Compare Interrupt...
230   t.tc->TC_CHANNEL[t.channel].TC_IER = TC_IER_CPCS;
231  // ... and disable all   others.
232  t.tc->TC_CHANNEL[t.channel].TC_IDR = ~TC_IER_CPCS;
233
234  return   *this;
235}
236
237DueTimer& DueTimer::setPeriod(float  microseconds) {
238  /*
239     Set the period of the timer (in microseconds)
240  */
241
242  // Convert period   in microseconds to frequency in Hz
243  float  frequency = 1000000.0 / microseconds;
244   setFrequency(frequency);
245  return *this;
246}
247
248float  DueTimer::getFrequency(void)   const {
249  /*
250    Get current time frequency
251  */
252
253  return _frequency[timer];
254}
255
256float   DueTimer::getPeriod(void) const {
257  /*
258    Get current time period
259  */
260
261   return 1.0 / getFrequency() * 1000000;
262}
263
264
265/*
266  Implementation   of the timer callbacks defined in
267  arduino-1.5.2/hardware/arduino/sam/system/CMSIS/Device/ATMEL/sam3xa/include/sam3x8e.h
268*/
269//   Fix for compatibility with Servo library
270#ifndef USING_SERVO_LIB
271void TC0_Handler(void)   {
272  TC_GetStatus(TC0, 0);
273  DueTimer::callbacks[0]();
274}
275#endif
276void   TC1_Handler(void) {
277  TC_GetStatus(TC0, 1);
278  DueTimer::callbacks[1]();
279}
280//   Fix for compatibility with Servo library
281#ifndef USING_SERVO_LIB
282void TC2_Handler(void)   {
283  TC_GetStatus(TC0, 2);
284  DueTimer::callbacks[2]();
285}
286void TC3_Handler(void)   {
287  TC_GetStatus(TC1, 0);
288  DueTimer::callbacks[3]();
289}
290void TC4_Handler(void)   {
291  TC_GetStatus(TC1, 1);
292  DueTimer::callbacks[4]();
293}
294void TC5_Handler(void)   {
295  TC_GetStatus(TC1, 2);
296  DueTimer::callbacks[5]();
297}
298#endif
299void   TC6_Handler(void) {
300  TC_GetStatus(TC2, 0);
301  DueTimer::callbacks[6]();
302}
303void   TC7_Handler(void) {
304  TC_GetStatus(TC2, 1);
305  DueTimer::callbacks[7]();
306}
307void   TC8_Handler(void) {
308  TC_GetStatus(TC2, 2);
309  DueTimer::callbacks[8]();
310}
311#endif
312

1/* DuePWMmod
2
3  Library:     pwm01.h (version 01)
4  Date:        2/11/2013
5   Written By:  randomvibe
6  modified by : Rolf Kurth
7
8  Purpose:
9   Setup   one or two unique PWM frequenices directly in sketch program,
10   set PWM duty   cycle, and stop PWM function.
11
12  Notes:
13   - Applies to Arduino-Due board,   PWM pins 6, 7, 8 & 9, all others ignored
14   - Libary Does not operate on the   TIO pins.
15   - Unique frequencies set via PWM Clock-A ("CLKA") and Clock-B   ("CLKB")
16     Therefore, up to two unique frequencies allowed.
17   - Set   max duty cycle counts (pwm_max_duty_Ncount) equal to 255
18     per Arduino approach.   This value is best SUITED for low frequency
19     applications (2hz to 40,000hz)   such as PWM motor drivers,
20     38khz infrared transmitters, etc.
21   - Future   library versions will address high frequency applications.
22   - Arduino's "wiring_analog.c"   function was very helpful in this effort.
23
24*/
25
26#ifndef DuePWMmod_h
27#define   DuePWMmod_h
28#define rechterMotor 'A'
29#define linkerMotor 'B'
30
31#include   <Arduino.h>
32
33class DuePWMmod
34{
35  public:
36    DuePWMmod();
37
38     // MAIN PWM INITIALIZATION
39    //--------------------------------
40    void   pinFreq( uint32_t  pin, char ClockAB );
41
42    // WRITE DUTY CYCLE
43    //--------------------------------
44     void  pinDuty( uint32_t  pin,  uint32_t  duty );
45
46    //void pwm_set_resolution(int   res);
47    void  setFreq(uint32_t  clock_freq, char ClockAB);
48
49    void   DisableChannel( uint32_t  pin );
50    void  EnableChannel( uint32_t  pin );
51
52   protected:
53    uint32_t  pwm_clockA_freq;
54    uint32_t  pwm_clockB_freq;
55   private:
56    static  const uint32_t  pwm_max_duty_Ncount = 255;
57};
58#endif
59
60

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/* 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   (PidParameterDi 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   (PidParameterRot Params)
53/**********************************************************************/
54{   // Constructor 3 Distance, different PidParameter
55  K  = Params.K;
56  Kp =   Params.Kp;
57  Kd = Params.Kd;
58  Ki = Params.Ki;
59  Ka = Params.Ka;
60  Kx   = Params.Kx;
61  Last_error = 0;
62  integrated_error = 0;
63  first = true;
64}
65/**********************************************************************/
66PidControl*   PidControl::getInstance()
67/**********************************************************************/
68{
69   pPID = this;
70  return pPID;
71}
72/**********************************************************************/
73void   PidControl::test ()
74/**********************************************************************/
75{
76   Serial.print("PID Test ");
77  ptr = (int) this;
78  Serial.print("PIDptr   ");
79  Serial.println(ptr , HEX);
80}
81/**********************************************************************/
82float   PidControl::calculate (float iAngle, float isetPoint )
83/**********************************************************************/
84//   new calculation of Steps per Second // PID algorithm
85{
86  Now = micros();
87   if (first) {
88    first = false;
89    Last_time = Now;
90    integrated_error   = 0;
91  }
92  timeChange = (Now - Last_time)  ;
93  timeChange = timeChange   / 1000.0;  // in millisekunden
94  error = isetPoint -  iAngle;
95
96  if ( timeChange   != 0) {
97    dTerm =  1000.0 * Kd * (error - Last_error) /  timeChange  ;
98   }
99
100  integrated_error = integrated_error  + ( error * timeChange );
101   iTerm =   Ki * integrated_error / 1000.0;
102
103  pTerm =   Kp  * error + ( Ka   * integrated_error ); // modifying Kp
104
105  // Compute PID Output in Steps per   second
106  eSpeed = K * (pTerm + iTerm + dTerm) ;
107
108  /*Remember something*/
109   Last_time  = Now;
110  Last_error = error;
111
112  //  digitalWrite(TestPIDtime,   !digitalRead(TestPIDtime)); // Toggle  Pin for reading the Frequenzy
113  // eSpeed   = constrain (eSpeed , -500.0 , 500.0 ); // 10 Steps per Second because Microstep
114   if (eSpeed > 0 ) {
115    DirForward = true ;
116  } else {
117    DirForward   = false;
118  }
119  return eSpeed;  // Steps per Second
120}
121
122/**********************************************************************/
123void   PidControl::reset ()
124/**********************************************************************/
125{
126   integrated_error = 0.0;
127  Last_error = 0.0;
128}
129
130
131/**********************************************************************/
132void   PidControl::changePIDparams (PidParameter Params)
133// changePIDparams = different   PidParameter !!!!
134/**********************************************************************/
135{
136   K  = Params.K;
137  Kp = Params.Kp;
138  Kd = Params.Kd;
139  Ki = Params.Ki;
140   Ka = Params.Ka;
141  Kx = Params.Kx;
142}
143/**********************************************************************/
144void   PidControl::changePIDparams (PidParameterDi Params)
145// changePIDparams = different   PidParameter !!!!
146/**********************************************************************/
147{   // different PidParameter !!!!
148  K  = Params.K;
149  Kp = Params.Kp;
150  Kd   = Params.Kd;
151  Ki = Params.Ki;
152  Ka = Params.Ka;
153  Kx = Params.Kx;
154}
155/**********************************************************************/
156void   PidControl::changePIDparams (PidParameterRot Params)
157// changePIDparams = different   PidParameter !!!!
158/**********************************************************************/
159{   // different PidParameter !!!!
160  K  = Params.K;
161  Kp = Params.Kp;
162  Kd   = Params.Kd;
163  Ki = Params.Ki;
164  Ka = Params.Ka;
165  Kx = Params.Kx;
166}
167/**********************************************************************/
168float   PidControl::getSteps () {
169  return eSpeed;
170}
171

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, PidControl* Pid,  int iPinDir, int iPinStep,
17           int iPinMs1, int iPinMs2, int iPinSleep, char iMotorSide);
18
19    struct   PidParameter params;
20    PidControl *ptrPid;
21    void init() ;
22    Motor*   getInstance();
23    void SleepMode ( );
24    void RunMode ( );
25    void toggleMode   ( );
26    float Calculate(float angle, float Setpoint);
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     float      Steps;
41    float      SetpointTmp;
42    float     Steps_tmp;
43     char _MotorSide;
44    DuePWMmod *ptrpwm;
45
46
47  private:
48    bool   MotorMode;
49    int ptr;
50};
51#endif
52

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 Nähe
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 größerem 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 größerem  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 höher (=gut)
199                besterr  = cfzz(it);
200                dparams(i) = dparams(i)*1.05; %mit größerem 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/*
242  // Serial.println("{Message|data|Hello World!}");
243  Serial.print("{MESSAGE:"  );
244  Serial.print("Log" );
245  Serial.print("|data|" );
246  Serial.print(average,  4 );
247  Serial.print(",  ");
248  Serial.print(besterr, 4 );
249  Serial.print(",  ");
250  Serial.print(params[0], 2 );
251  Serial.print(",  ");
252  Serial.print(params[1],  2 );
253  Serial.print(",  ");
254  Serial.print(params[2], 2 );
255  Serial.print(",  ");
256  Serial.print(params[3], 2);
257  Serial.print(",  ");
258  Serial.print(params[4],  2);
259  Serial.print(",  ");
260  Serial.print(params[5], 2);
261  Serial.print(",  ");
262  Serial.print(params[6], 2);
263  Serial.println("}");
264*/
265}

1/*
2  DueTimer.h - DueTimer header file, definition of methods and attributes...
3   For instructions, go to https://github.com/ivanseidel/DueTimer
4
5  Created   by Ivan Seidel Gomes, March, 2013.
6  Modified by Philipp Klaus, June 2013.
7   Released into the public domain.
8*/
9
10#ifdef __arm__
11
12#ifndef DueTimer_h
13#define   DueTimer_h
14
15#include "Arduino.h"
16
17#include <inttypes.h>
18
19/*
20   This fixes compatibility for Arduono Servo Library.
21  Uncomment to make it   compatible.
22
23  Note that:
24    + Timers: 0,2,3,4,5 WILL NOT WORK, and will
25           neither be accessible by Timer0,...
26*/
27// #define USING_SERVO_LIB   true
28
29#ifdef USING_SERVO_LIB
30#warning "HEY! You have set flag USING_SERVO_LIB.   Timer0, 2,3,4 and 5 are not available"
31#endif
32
33
34#define NUM_TIMERS   9
35
36class DueTimer
37{
38  protected:
39
40    // Represents the timer   id (index for the array of Timer structs)
41    const unsigned short timer;
42
43     // Stores the object timer frequency
44    // (allows to access current timer   period and frequency):
45    static float  _frequency[NUM_TIMERS];
46
47    //   Picks the best clock to lower the error
48    static uint8_t bestClock(float  frequency,   uint32_t& retRC);
49
50    // Make Interrupt handlers friends, so they can use   callbacks
51    friend void TC0_Handler(void);
52    friend void TC1_Handler(void);
53     friend void TC2_Handler(void);
54    friend void TC3_Handler(void);
55    friend   void TC4_Handler(void);
56    friend void TC5_Handler(void);
57    friend void   TC6_Handler(void);
58    friend void TC7_Handler(void);
59    friend void TC8_Handler(void);
60
61     static void (*callbacks[NUM_TIMERS])();
62
63    struct Timer
64    {
65       Tc *tc;
66      uint32_t channel;
67      IRQn_Type irq;
68    };
69
70     // Store timer configuration (static, as it's fixed for every object)
71    static   const Timer Timers[NUM_TIMERS];
72
73  public:
74
75    static DueTimer getAvailable(void);
76
77     DueTimer(unsigned short _timer);
78    DueTimer& attachInterrupt(void (*isr)());
79     DueTimer& detachInterrupt(void);
80    DueTimer& start(float  microseconds   = -1);
81    DueTimer& stop(void);
82    DueTimer& setFrequency(float  frequency);
83     DueTimer& setPeriod(float  microseconds);
84
85    float  getFrequency(void)   const;
86    float  getPeriod(void) const;
87};
88
89// Just to call Timer.getAvailable   instead of Timer::getAvailable() :
90extern DueTimer Timer;
91
92extern DueTimer   Timer1;
93// Fix for compatibility with Servo library
94#ifndef USING_SERVO_LIB
95extern   DueTimer Timer0;
96extern DueTimer Timer2;
97extern DueTimer Timer3;
98extern   DueTimer Timer4;
99extern DueTimer Timer5;
100#endif
101extern DueTimer Timer6;
102extern   DueTimer Timer7;
103extern DueTimer Timer8;
104
105#endif
106
107#else
108#error   Oops! Trying to include DueTimer on another device!?
109#endif
110

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

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
43
44

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
4Cascaded  PID Controler
5
6The robot is controlled by cascaded PID controllers. Two controllers  are responsible for driving the motors (right and left side). 
7Two additional  controllers for controlling the position of the robot. The motor controller ensures  that the robot remains upright. 
8The Position Controler ensures that the robot  maintains its correct setpoint prescribed position. 
9Cascade control is a cascading  of several controllers; the associated control loops are nested in each other. 
10The  controller output variable of one controller (master controller, Position) serves  as the reference variable 
11for another (slave controller, Motor).
12The PID  controllers are implemented in the "PIDControl" class. 4 instances of the class  are instantiated for the 4 controllers.
13The standard PID algorithm was slightly  modified after longer test series. To the P-component the parameter Kx multiplied  
14by the I-component was added.
15*/
16#include "Vehicle.h"
17
18/**********************************************************************/
19Vehicle::Vehicle(DuePWMmod*  ipwm, Motor * MotorA, Motor * MotorB,  // Constructor
20                 PidControl  * PidMotorA, PidControl * PidMotorB, PidControl * PidDistA,
21                 PidControl  * PidDistB, PidControl * PidRotation, int iPinSleepSwitch)
22/**********************************************************************/
23{
24  PinSleepSwitch = iPinSleepSwitch;
25  pinMode(PinSleepSwitch, INPUT_PULLUP);
26  pMotorA    = MotorA;
27  pMotorB    = MotorB;
28  pPidMotorA = PidMotorA;
29  pPidMotorB = PidMotorB;
30  pPidDistA  = PidDistA;
31  pPidDistB  = PidDistB;
32  pPidRotation  = PidRotation;
33  ptrpwm     = ipwm;
34  firstRun   = true;
35  StopDriving = false;
36}
37
38/**********************************************************************/
39void  Vehicle::Run(MpuYawPitchRoll YawPitchRoll , int &iPositionA, int &iPositionB, JStickData  JStick)
40/**********************************************************************/
41{  /*
42    Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev => 1.8 degree per step
43    Wheel Diameter 88mm = > Distance per Pulse Dpp = d * pi / 200 =  1,3816 mm
44    Distance per Revolution =  276,32 mm
45    Max 1000 Steps per Second = 5 Revolutions  => 13816 mm Distance
46
47    "iPositionA" in  microsteps
48    8 microsteps  = 1 full Step
49    1 microstepp = 0,125 full steps
50    after division one change  in "PositionA" = 0.01 microstepp and 0,0125 full steps = 0,01727 mm
51  */
52  const int     tDelay = 10;
53  const float   MaxSpeed = 0.8; 
54  const float   MaxDistance = 1.0; // per tDelay
55
56  PositionA = (float(iPositionA ) / 100  );
57  PositionB = (float(iPositionB ) / 100 );
58
59  /*********************  driverless **************************************/
60  if (firstRun) {
61    firstRun  = false;
62    skipPos = - (2 * tDelay); // first time wait a little bit longer
63    CalJstickX = JStick.Xvalue;  // calibrate Joy Stick
64    CalJstickY = JStick.Yvalue;
65    actYawPosition = -YawPitchRoll.yaw;
66  }
67  JStick.Xvalue = JStick.Xvalue  - CalJstickX ;
68  JStick.Yvalue = JStick.Yvalue - CalJstickY;
69
70  /**********************************************************************/
71
72  if (++skipPos  >= tDelay) { // to delay the calls, Position Control should be  10 times lower than Motor Control
73    skipPos = 0;
74    //After the robot has  moved to the right or left, slight deviations occur during the next straight ride.
75    //workaound: when driving straight forwad or backwad(!spinning), the equality  of the steps is forced.
76
77    if (abs(JStick.Xvalue) > 5) {
78      spinning  = true;
79      spinningOld = true;
80    } else {
81      spinning = false;
82      if (spinningOld) {
83        resetPIDs( iPositionA, iPositionB);
84        actYawPosition  = YawPitchRoll.yaw;
85      }
86      spinningOld = false;
87    }
88    DeltaPosA  = (float((JStick.Yvalue)) / 40.0) - (float(JStick.Xvalue ) / 350.0) ;
89    DeltaPosB  = (float((JStick.Yvalue)) / 40.0) + (float(JStick.Xvalue ) / 350.0);
90
91    if  (abs(DeltaPosA) < 0.05) {
92      DeltaPosA = 0.0;
93    }
94    if (abs(DeltaPosB)  < 0.05) {
95      DeltaPosB = 0.0;
96    }
97
98    SetpointPositionA = constrain((SetpointPositionA  + DeltaPosA  ), (SetpointPositionA - MaxDistance), (SetpointPositionA + MaxDistance));  
99    SetpointPositionB = constrain((SetpointPositionB + DeltaPosB  ), (SetpointPositionA  - MaxDistance), (SetpointPositionA + MaxDistance)) ;  
100    SetpointA = constrain(  (pPidDistA->calculate (PositionA , SetpointPositionA)), (LastSetpointA - MaxSpeed),  (LastSetpointA + MaxSpeed)); // calculate pid seoint
101    SetpointB = constrain(  (pPidDistB->calculate (PositionB , SetpointPositionB)), (LastSetpointB - MaxSpeed),  (LastSetpointB + MaxSpeed));
102    LastSetpointPositionA = SetpointPositionA;
103    LastSetpointPositionB = SetpointPositionB;
104    LastDeltaPosA = DeltaPosA;
105    LastDeltaPosB = DeltaPosB;
106  }
107  SetpointA = ( SetpointA * 0.7) + (LastSetpointA  * 0.3); //low-pass filter
108  SetpointB = ( SetpointB * 0.7) + (LastSetpointB *  0.3);
109  LastSetpointA = SetpointA ;
110  LastSetpointB = SetpointB ;
111
112  // PID calculation of new steps
113  StepsA = pMotorA->Calculate(YawPitchRoll.pitch,  -SetpointA);
114  StepsB = pMotorB->Calculate(YawPitchRoll.pitch, -SetpointB);
115
116  //After the robot has moved to the right or left, slight deviations occur during  the next straight ride.
117  //workaound: when driving straight forwad or backwad(!spinning),  the equality of the steps is forced.
118
119  if (!spinning && !auto_spinning)  {
120    ABdiff = StepsA - StepsB;
121  }
122  // run the Motors, here Steps = full steps
123  StepsA = StepsA - (ABdiff / 2);
124  StepsB = StepsB + (ABdiff / 2);
125  // pMotorA->Run(StepsA  );
126  // pMotorB->Run(StepsB);
127
128  // run the Motors, here Steps = full steps
129  int freqA;
130  int freqB;
131
132  freqA = int(pMotorA->Run(StepsA ));
133  freqB  = int(pMotorB->Run(StepsB));
134  ptrpwm->setFreq2( freqA, freqB  );
135}
136
137/**********************************************************************/
138float  Vehicle::getNewYaw()
139/**********************************************************************/
140{
141  float tmp;
142  //  tmp = NewYawPos;
143  //  NewYawPos = 0.0;
144  return tmp;
145}
146
147/**********************************************************************/
148void  Vehicle::resetPIDs( int &iPositionA, int &iPositionB )
149/**********************************************************************/
150{
151  iPositionA = iPositionB = 0;
152  LastSetpointPositionA = LastSetpointPositionB  = 0.0;
153  pPidDistA->reset();
154  pPidDistB->reset();
155  pPidMotorA->reset();
156  pPidMotorB->reset();
157  pPidRotation->reset();
158  LastDeltaPosA = LastDeltaPosB  = 0.0;
159  LastSetpointA = LastSetpointB = 0.0;
160}
161
162/**********************************************************************/
163void  Vehicle::init()
164/**********************************************************************/
165{
166  Serial.println("Vehicle Init Motor Pointer....");
167  int ptr1 = (int) pMotorA;
168  int ptr2 = (int) pMotorB;
169  Serial.println(ptr1 , HEX);
170  Serial.println(ptr2  , HEX);
171
172  Serial.println("Vehicle Init PID Pointer....");
173  int ptr3  = (int) pPidMotorA;
174  int ptr4 = (int) pPidMotorB;
175  Serial.println(ptr3 ,  HEX);
176  Serial.println(ptr4 , HEX);
177
178  Serial.println("Vehicle Init PID  Pointer....");
179  int ptr5 = (int) pPidDistA;
180  int ptr6 = (int) pPidDistB;
181  Serial.println(ptr5 , HEX);
182  Serial.println(ptr6 , HEX);
183
184  pMotorA->init(  );
185  pMotorB->init();
186
187  pMotorA->MsMicrostep();  // set microstepping
188  pMotorB->MsMicrostep();
189
190  pMotorA->SleepMode();
191  pMotorB->SleepMode();
192
193}
194/**********************************************************************/
195void  Vehicle::Stop() {
196  pMotorA->SleepMode( );
197  pMotorB->SleepMode( );
198}
199/**********************************************************************/
200void  Vehicle::changePIDparams(PidParameter Params) {
201  pPidMotorA->changePIDparams(Params);
202  pPidMotorB->changePIDparams(Params);
203  //  Serial.println("Vehicle PID Params  changed! ");
204}
205void Vehicle::changePIDparams(PidParameterDi Params) {
206  pPidDistA->changePIDparams(Params);
207  pPidDistB->changePIDparams(Params);
208  //  Serial.println("Vehicle PID Params changed! ");
209}
210void Vehicle::changePIDparams(PidParameterRot  Params) {
211  pPidRotation->changePIDparams(Params);
212  pPidRotation->changePIDparams(Params);
213  //  Serial.println("Vehicle PID Params changed! ");
214}
215