Components and supplies
RGB Backlight LCD - 16x2
NEMA 17 Stepper Motor
HC-05 Bluetooth Module
MP6500 Stepper Motor Driver Carrier
Joystick Shield
7.4V 2S 3300mAh 35C Li-Polymer Lipo
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
Arduino Mega 2560
Arduino Due
Project description
Code
Battery.h
c_cpp
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
PidControl.h
c_cpp
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
DuePWMmod.cpp
c_cpp
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
DuePWMmod.h
c_cpp
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
LCD.ino
c_cpp
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
PidControl.cpp
c_cpp
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
Motor.h
c_cpp
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
Twiddle.cpp
c_cpp
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}
DueTimer.h
c_cpp
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
JoyStick.ino
c_cpp
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
SBRobotDueDP6500_NewPWM_55.ino
c_cpp
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
Twiddle.h
c_cpp
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
Vehicle.cpp
c_cpp
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
JoyStickSlave01.ino
c_cpp
1// ADDR:98d3:37:88fa 2 3/* On Uno, Nano, Mini, and Mega, pins 0 and 1 are used for communication with the computer. 4 Connecting anything to these pins can interfere with that communication, 5 including causing failed uploads to the board. 6*/ 7int const UP_BTN = 2; 8int const DOWN_BTN = 4; 9int const LEFT_BTN = 5; 10int const RIGHT_BTN = 3; 11int const E_BTN = 6; 12int const F_BTN = 7; 13 14int const JOYSTICK_BTN = 8; 15int const JOYSTICK_AXIS_X = A0; 16int const JOYSTICK_AXIS_Y = A1; 17 18char c = ' '; 19boolean NL = true; 20long randNumber; 21 22 23int Xvalue, Yvalue, Up, Down, Left , Right, JButton; 24 25 26void setup() 27{ 28 Serial.begin(9600); 29 Serial.print("Sketch: "); Serial.println(__FILE__); 30 Serial.print("Uploaded: "); Serial.println(__DATE__); 31 Serial.println(" "); 32 33} 34 35void loop() 36{ 37 38 if ((Xvalue == map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100)) && 39 (Yvalue == map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100)) && 40 (JButton == digitalRead(JOYSTICK_BTN)) && 41 (Up == digitalRead(UP_BTN)) && 42 (Down == digitalRead(DOWN_BTN)) && 43 (Left == digitalRead(LEFT_BTN)) && 44 (Right == digitalRead(RIGHT_BTN))) return; 45 46 Xvalue = map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100); 47 Yvalue = map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100); 48 JButton = digitalRead(JOYSTICK_BTN); 49 Up = digitalRead(UP_BTN); 50 Down = digitalRead(DOWN_BTN); 51 Left = digitalRead(LEFT_BTN); 52 Right = digitalRead(RIGHT_BTN); 53 54 55 Serial.print("X"); 56 Serial.print("#"); 57 Serial.print(Xvalue); 58 Serial.print("#"); 59 Serial.print("Y"); 60 Serial.print("#"); 61 Serial.print(Yvalue); 62 Serial.print("#"); 63 Serial.print("B1"); 64 Serial.print("#"); 65 Serial.print(JButton); 66 Serial.print("#"); 67 Serial.print("Up"); 68 Serial.print("#"); 69 Serial.print(Up); 70 Serial.print("#"); 71 Serial.print("Do"); 72 Serial.print("#"); 73 Serial.print(Down); 74 Serial.print("#"); 75 Serial.print("Le"); 76 Serial.print("#"); 77 Serial.print(Left); 78 Serial.print("#"); 79 Serial.print("Ri"); 80 Serial.print("#"); 81 Serial.print(Right); 82 Serial.print("#"); 83 Serial.print('\ 84'); 85 delay(100); 86} 87 88
LCD.ino
c_cpp
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
PidParameter.h
c_cpp
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 PidParameter_h 6#define PidParameter_h 7#include "PidParameter.h" 8 9// PidParameter Motor 10struct PidParameter { 11 float K = 5.0; 12 float Kp = 9.3 ; 13 float Ki = 5.27; //6.01; 14 float Kd = 0.13; //0.12 ; 15 float Ka = 0.1007; //0.1246; 16 float Kx = 0.0; // 17}; 18// PidParameter Position 19struct PidParameterDi { 20 float K = 1.0; 21 float Kp = 0.18; 22 float Ki = 0.0; 23 float Kd = 0.4 ; 24 float Ka = 0.0 ; 25 float Kx = 0.0; 26}; 27// PidParameter Position 28struct PidParameterRot { //not yet used 29 float K = 1.0; 30 float Kp = 0.01 ; //0.0037; 31 float Ki = 0.000; 32 float Kd = 0.00; 33 float Ka = 0.0 ; 34 float Kx = 0.0; 35}; 36#endif 37
DueTimer.cpp
c_cpp
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 313
JoyStick.ino
c_cpp
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 == '\n') { 36 stringComplete = true; 37 } 38 } 39} 40 41void BTRead( JStickData &JSData ) { 42 String command; 43 unsigned int j; 44 long value; 45 46 47 // print the string when a newline arrives: 48 if (stringComplete) { 49 if (inputString.substring(0, 1) != "X") 50 { 51 Serial.print("Error reading Bluetooth "); 52 Serial.println(inputString); 53 } else { 54 j = 0; 55 for (unsigned int i = 0; i < inputString.length(); i++) { 56 57 if (inputString.substring(i, i + 1) == "#") { 58 command = inputString.substring(j, i); 59 //Serial.print("Command: "); 60 //Serial.print(command); 61 j = i + 1; 62 for (unsigned int i1 = j; i1 < inputString.length(); i1++) { 63 if (inputString.substring(i1, i1 + 1) == "#") { 64 value = inputString.substring(j, i1).toInt(); 65 //Serial.print(" Value: "); 66 //Serial.println(value); 67 i = i1; 68 j = i + 1; 69 assignValues(command, value, JSData); 70 break; 71 } 72 } 73 } 74 } 75 } 76 inputString = ""; 77 stringComplete = false; 78 // Serial.print(" Value: "); 79 // Serial.println(JStick.Xvalue); 80 } 81} 82 83void assignValues(String icommand, int ivalue, JStickData &Data ) { 84 85 if (icommand == "X") Data.Xvalue = ivalue; 86 if (icommand == "Y") Data.Yvalue = ivalue; 87 if (icommand == "B1") Data.JButton = ivalue; 88 if (icommand == "Up") Data.Up = ivalue; 89 if (icommand == "Do") Data.Down = ivalue; 90 if (icommand == "Ri") Data.Right = ivalue; 91 if (icommand == "Le") Data.Left = ivalue; 92 93} 94
Vehicle.h
c_cpp
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 "PidControl.h" 11#include "PidParameter.h" 12#include "Config.h" 13/**********************************************************************/ 14class Vehicle 15/**********************************************************************/ 16{ 17#define forward HIGH 18#define backward LOW 19 public: 20 Vehicle(DuePWMmod* ipwm, Motor * MotorA, Motor * MotorB, 21 PidControl * PidMotorA, PidControl * PidMotorB, PidControl * PidDistA, 22 PidControl * PidDistB, PidControl * PidRotation, int iPinSleepSwitch); // Constructor 23 24 Motor *pMotorA; 25 Motor *pMotorB; 26 PidControl *pPidMotorA; 27 PidControl *pPidMotorB; 28 PidControl *pPidDistA; 29 PidControl *pPidDistB; 30 PidControl *pPidRotation; 31 32 void Run(MpuYawPitchRoll YawPitchRoll , int &PositionA, int &PositionB, JStickData JStick); 33 void init(); 34 35 void changePIDparams(PidParameter Params); 36 void changePIDparams(PidParameterDi Params); 37 void changePIDparams(PidParameterRot Params); 38 39 float getNewYaw(); 40 41 void resetPIDs( int &iPositionA, int &iPositionB ); 42 void Stop( ); 43 // Microstep Resolution 44 45 float actYawPosition = 0.0; 46 float SetpointA = 0.0; 47 float SetpointB = 0.0; 48 float LastSetpointA = 0.0; 49 float LastSetpointB = 0.0; 50 float DeltaPosA = 0.0; 51 float DeltaPosB = 0.0; 52 float LastDeltaPosA = 0.0; 53 float LastDeltaPosB = 0.0; 54 float autoDeltaPosA = 0.0; 55 float autoDeltaPosB = 0.0; 56 float PositionA = 0.0; 57 float PositionB = 0.0; 58 float SetpointPositionA = 0.0; 59 float SetpointPositionB = 0.0; 60 float LastSetpointPositionA = 0.0; 61 float LastSetpointPositionB = 0.0; 62 float StepsA = 0.0; 63 float StepsB = 0.0; 64 float CalJstickX = 0; 65 float CalJstickY = 0; 66 bool spinning = false; 67 bool auto_spinning = false; 68 bool spinningOld = false ; 69 bool StopDriving = false; 70 int skipPos; // wait befor starting position controll 71 float Direction; 72 float ABdiff = 0; 73 74 75 protected: 76 DuePWMmod *ptrpwm; 77 int PinSleepSwitch; 78 bool firstRun = true; 79 // ------------------------------------------------------------------------ 80 // Reading serial Data 81 // ------------------------------------------------------------------------ 82 String InString = ""; // a string to hold incoming data 83 boolean BTReady = false; // whether the string is complete 84 85}; 86#endif 87
DuePWMmod.h
c_cpp
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
Twiddle.h
c_cpp
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
Battery.cpp
c_cpp
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 #include "Battery.h" 7#include <Arduino.h> 8 9/**********************************************************************/ 10Battery::Battery(int PIN) 11/**********************************************************************/ 12{ 13 _VoltPin = PIN; 14 pinMode(_VoltPin, INPUT); 15} 16 17// ----------------------------------------------------------------------------- 18float Battery::VoltageRead() 19// -------------------------------------------------------------- 20{ 21 Voltage = ( analogRead(_VoltPin)) * 0.018 ; 22 /*Serial.print("Voltage "); 23 Serial.print(Voltage); 24 Serial.println(VoltPin );*/ 25 return Voltage; 26} 27// ----------------------------------------------------------------------------- 28bool Battery::VoltageCheck() 29// -------------------------------------------------------------- 30{ 31 // Lipo Nennspannung von 3,7 Volt 32 // die Spannung der einzelnen Zellen nicht unter 3,2 Volt sinkt. 33 // voltage divider: 34 // 21 Kohm 4,7 Kohm maximal 12 Volt 35 // Uinp = U * R1 / ( R1 + R2 ) = 0,183 * U 36 // 1024 (resolution) 37 // analogRead(VoltPin)) * 5.0VoltRef / 1024.0 / 0.183 = 0.027 38 // in case of Resistor toleranz 0.0225 39 40 // Voltage = ( analogRead(VoltPin)) * 0.0225 ; 41 Voltage = VoltageRead(); 42 43 if (Voltage > 4.8 && Voltage < 6.4 ) { 44 return false; 45 } 46 return true; 47} 48
Motor.cpp
c_cpp
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, 42×48mm, 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, PidControl * Pid, 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 // WrapperMode = true; 54 ptrPid = Pid; 55 ptrpwm = ipwm; 56} 57/**********************************************************************/ 58Motor* Motor::getInstance() 59/**********************************************************************/ 60{ 61 pMotor = this; 62 return pMotor; 63} 64/**********************************************************************/ 65void Motor::init ( ) 66/**********************************************************************/ 67{ 68 69 Serial.print("Motor "); 70 Serial.print(ptr , HEX); 71 Serial.print(" Side "); 72 Serial.print(_MotorSide); 73 Serial.print(" iPinStep "); 74 Serial.print(_PinStep); 75 Serial.print(" iPinSleep "); 76 Serial.print(_PinSleep); 77 Serial.println(" init..."); 78 lastTime = millis(); 79 80 ptrPid->changePIDparams( params); 81} 82/**********************************************************************/ 83void Motor::SleepMode( ) 84/**********************************************************************/ 85{ 86 digitalWrite(_PinSleep, LOW); 87 MotorMode = false; 88} 89/**********************************************************************/ 90void Motor::RunMode( ) 91/**********************************************************************/ 92{ 93 digitalWrite(_PinSleep, HIGH); 94 MotorMode = true; 95} 96/**********************************************************************/ 97void Motor::toggleMode( ) 98/**********************************************************************/ 99{ 100 if ( MotorMode == false ) RunMode( ); 101 else SleepMode(); 102} 103/**********************************************************************/ 104float Motor::Calculate(float angle, float Setpoint) 105/**********************************************************************/ 106{ 107 Steps = ptrPid->calculate (angle, Setpoint ); 108 return Steps; 109} 110/**********************************************************************/ 111float Motor::Run(float Steps) { 112 /**********************************************************************/ 113 if (!digitalRead(PinSleepSwitch)) { 114 RunMode( ); 115 if (Steps >= 0 ) { 116 if (_MotorSide == rechterMotor) { 117 digitalWrite(_PinDir, LOW); 118 } 119 else { 120 digitalWrite(_PinDir, HIGH); 121 } 122 } else 123 { 124 if (_MotorSide == rechterMotor) { 125 digitalWrite(_PinDir, HIGH); 126 } 127 else { 128 digitalWrite(_PinDir, LOW); 129 } 130 } 131 132 if (_Divisor > 0) { 133 Steps = Steps * _Divisor; // convert into Microsteps 134 } 135 136 Steps_tmp = Steps; 137 Steps = abs(Steps_tmp); 138 139 if ( Steps < 2.0) Steps = 2.0; // Microsteps 140 return Steps; 141 } else SleepMode( ); 142 143} 144/**********************************************************************/ 145void Motor::MsFull ( ) { 146 digitalWrite(_Ms1, LOW); 147 digitalWrite(_Ms2, LOW); 148 _Divisor = 1; 149} 150void Motor::MsHalf ( ) { 151 digitalWrite(_Ms1, LOW); 152 digitalWrite(_Ms2, HIGH); 153 _Divisor = 2; 154} 155void Motor::MsQuater ( ) { 156 digitalWrite(_Ms1, HIGH); 157 digitalWrite(_Ms2, LOW); 158 _Divisor = 4; 159} 160void Motor::MsMicrostep ( ) { 161 digitalWrite(_Ms1, HIGH); 162 digitalWrite(_Ms2, HIGH); 163 _Divisor = 8; 164 // Serial.println("MsMicrostep"); 165} 166
Config.h
c_cpp
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
MyMPU.ino
c_cpp
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 // if (Running) Robot.Run( Angle, SetpointA, SetpointB ); // Robot.Run( Angle, Speed, Speed ); 68 } 69 return YawPitchRoll ; 70} 71// -------------------------------------------------------------------- - 72MpuYawPitchRoll ReadMpuRunRobot6050() 73// -------------------------------------------------------------------- - 74{ 75 static float pitch_old; 76 static float yaw_old; 77 static float yaw_tmp; 78 static float yaw_delta; 79 // static float pitch; 80 81 mpuIntStatus = mpu.getIntStatus(); 82 83 // get current FIFO count 84 fifoCount = mpu.getFIFOCount(); 85 // check for overflow (this should never happen unless our code is too inefficient) 86 if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 87 // reset so we can continue cleanly 88 // mpu.setDMPEnabled(false); 89 mpu.resetFIFO(); 90 FifoOverflowCnt ++; 91 fifoCount = 0; 92 YawPitchRoll.pitch = pitch_old; 93 return YawPitchRoll; 94 95 } 96 // otherwise, check for DMP data ready interrupt (this should happen frequently) 97 // Register 58 â€â€\x9D lnterrupt Status INT_STATUS 98 // MPU-6500 Register Map and Descriptions Revision 2.1 99 // Bit [1] DMP_INT This bit automatically sets to 1 when the DMP interrupt has been generated. 100 // Bit [0] RAW_DATA_RDY_INT1 Sensor Register Raw Data sensors are updated and Ready to be read. 101 if ((mpuIntStatus) & 0x02 || (mpuIntStatus & 0x01)) { 102 // wait for correct available data length, should be a VERY short wait 103 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 104 105 while (fifoCount > 0) { 106 // read a packet from FIFO 107 mpu.getFIFOBytes(fifoBuffer, packetSize); 108 109 // track FIFO count here in case there is > 1 packet available 110 // (this lets us immediately read more without waiting for an interrupt) 111 fifoCount = mpu.getFIFOCount(); 112 // fifoCount -= packetSize; 113 } 114 // the yaw/pitch/roll angles (in degrees) calculated from the quaternions coming 115 // from the FIFO. Note this also requires gravity vector calculations. 116 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 117 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 118 119 mpu.dmpGetQuaternion(&q, fifoBuffer); 120 mpu.dmpGetGravity(&gravity, &q); 121 // mpu.dmpGetEuler(euler, &q); 122 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 123 124 YawPitchRoll.pitch = -(ypr[1] * 180 / M_PI); //pitch 125 126 yaw_tmp = (abs(ypr[0] * 180 / M_PI)); 127 yaw_delta = yaw_tmp - yaw_old; 128 yaw_old = yaw_tmp; 129 YawPitchRoll.yaw += yaw_delta; 130 131 // actual quaternion components in a [w, x, y, z] 132 // YawPitchRoll.pitch = (q.y) * 180; 133 // YawPitchRoll.yaw = (q.z ); 134 // YawPitchRoll.yaw = mapfloat(YawPitchRoll.yaw , -1.0, 1.0, 0.0, 360.0); 135 } 136 pitch_old = YawPitchRoll.pitch ; 137 return YawPitchRoll ; 138} 139// -------------------------------------------------------------- 140void MpuInit() 141// -------------------------------------------------------------- 142// MPU6050_6Axis_MotionApps20.h 143// 0x02, 0x16, 0x02, 0x00, 0x09 => 50msec 20 Hz 144// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 145// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 146// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 147// It is important to make sure the host processor can keep up with reading and processing 148// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 149 150{ 151 // after Reset of Arduino there is no Reset of MPU 152 pinMode(MpuInterruptPin, INPUT); 153 154 // verify connection 155 Serial.println(F("Testing device connections...")); 156 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 157 158 // load and configure the DMP 159 Serial.println(F("Initializing DMP...")); 160 devStatus = mpu.dmpInitialize(); 161 // supply your own gyro offsets here, scaled for min sensitivity 162 mpu.setXGyroOffset(220); 163 mpu.setYGyroOffset(76); 164 mpu.setZGyroOffset(-84); 165 mpu.setZAccelOffset(1788); // 166 167 // make sure it worked (returns 0 if so) 168 if (devStatus == 0) { 169 // turn on the DMP, now that it's ready 170 Serial.println(F("Enabling DMP...")); 171 mpu.setDMPEnabled(true); 172 173 // enable Arduino interrupt detection 174 Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 175 attachInterrupt(digitalPinToInterrupt(MpuInterruptPin), dmpDataReady, RISING); 176 mpuIntStatus = mpu.getIntStatus(); 177 178 // set our DMP Ready flag so the main loop() function knows it's okay to use it 179 Serial.println(F("DMP ready! Waiting for first interrupt...")); 180 dmpReady = true; 181 182 // get expected DMP packet size for later comparison 183 packetSize = mpu.dmpGetFIFOPacketSize(); 184 // mpu.resetFIFO(); // after Reset importand 185 186 } else { 187 // ERROR! 188 // 1 = initial memory load failed 189 // 2 = DMP configuration updates failed 190 // (if it's going to break, usually the code will be 1) 191 Serial.print(F("DMP Initialization failed (code ")); 192 Serial.print(devStatus); 193 Serial.println(F(")")); 194 lcd.clear(); 195 lcd.setCursor(0, 0); 196 lcd.print( "Error MPU6050 " ); 197 do {} while ( 1 == 1); 198 } 199} 200 201float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) 202{ 203 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 204} 205
Motor.h
c_cpp
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
DueTimer.h
c_cpp
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
Vehicle.h
c_cpp
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#ifndef 8 Vehicle_h 9#define Vehicle_h 10#include "Motor.h" 11#include "Arduino.h" 12#include 13 "PidControl.h" 14#include "PidParameter.h" 15#include "Config.h" 16/**********************************************************************/ 17class 18 Vehicle 19/**********************************************************************/ 20{ 21#define 22 forward HIGH 23#define backward LOW 24 public: 25 Vehicle(DuePWMmod* ipwm, 26 Motor * MotorA, Motor * MotorB, 27 PidControl * PidMotorA, PidControl 28 * PidMotorB, PidControl * PidDistA, 29 PidControl * PidDistB, PidControl 30 * PidRotation, int iPinSleepSwitch); // Constructor 31 32 Motor *pMotorA; 33 34 Motor *pMotorB; 35 PidControl *pPidMotorA; 36 PidControl *pPidMotorB; 37 38 PidControl *pPidDistA; 39 PidControl *pPidDistB; 40 PidControl *pPidRotation; 41 42 43 void Run(MpuYawPitchRoll YawPitchRoll , int &PositionA, int &PositionB, JStickData 44 JStick); 45 void init(); 46 47 void changePIDparams(PidParameter Params); 48 49 void changePIDparams(PidParameterDi Params); 50 void changePIDparams(PidParameterRot 51 Params); 52 53 float getNewYaw(); 54 55 void resetPIDs( int &iPositionA, 56 int &iPositionB ); 57 void Stop( ); 58 // Microstep Resolution 59 60 61 float actYawPosition = 0.0; 62 float SetpointA = 0.0; 63 float SetpointB 64 = 0.0; 65 float LastSetpointA = 0.0; 66 float LastSetpointB = 0.0; 67 float 68 DeltaPosA = 0.0; 69 float DeltaPosB = 0.0; 70 float LastDeltaPosA = 0.0; 71 72 float LastDeltaPosB = 0.0; 73 float autoDeltaPosA = 0.0; 74 float autoDeltaPosB 75 = 0.0; 76 float PositionA = 0.0; 77 float PositionB = 0.0; 78 float 79 SetpointPositionA = 0.0; 80 float SetpointPositionB = 0.0; 81 float LastSetpointPositionA 82 = 0.0; 83 float LastSetpointPositionB = 0.0; 84 float StepsA = 0.0; 85 86 float StepsB = 0.0; 87 float CalJstickX = 0; 88 float CalJstickY = 0; 89 90 bool spinning = false; 91 bool auto_spinning = false; 92 bool 93 spinningOld = false ; 94 bool StopDriving = false; 95 int skipPos; 96 // wait befor starting position controll 97 float Direction; 98 float ABdiff 99 = 0; 100 101 102 protected: 103 DuePWMmod *ptrpwm; 104 int PinSleepSwitch; 105 106 bool firstRun = true; 107 // ------------------------------------------------------------------------ 108 109 // Reading serial Data 110 // ------------------------------------------------------------------------ 111 112 String InString = ""; // a string to hold incoming data 113 boolean 114 BTReady = false; // whether the string is complete 115 116}; 117#endif 118
PidControl.h
c_cpp
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
MyMPU.ino
c_cpp
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 Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 7 8 by Jeff Rowberg <jeff@rowberg.net> 9 10 MPU-6050 Accelerometer + Gyro 11 12 13 The MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single 14 chip. 15 It is very accurate, as it contains 16-bits analog to digital conversion 16 hardware for each channel. 17 Therefor it captures the x, y, and z channel 18 at the same time. The sensor uses the I2C -bus to interface with the Arduino. 19 20 21 I used Digital Motion Processing with the MPU-6050 sensor, to do fast calculations 22 directly on the chip. 23 This reduces the load for the Arduino. 24 25 https://playground.arduino.cc/Main/MPU-6050 26 27 28 Because of the orientation of my board, I used yaw/pitch/roll angles (in degrees) 29 calculated from the quaternions coming from the FIFO. 30 By reading Euler angle 31 I got problems with Gimbal lock. 32*/ 33 34/* 0x02, 0x16, 0x02, 0x00, 35 0x07 // D_0_22 inv_set_fifo_rate 36 37 // This very last 0x01 38 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 39 // 0x01 40 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 41 42 // DMP output frequency is calculated easily using this equation: (200Hz / (1 43 + value)) 44 45 // It is important to make sure the host processor can keep 46 up with reading and processing 47 // the FIFO output at the desired rate. Handling 48 FIFO overflow cleanly is also a good idea. 49*/ 50// ------------------------------------------------------------------------ 51// 52 orientation/motion vars 53// ------------------------------------------------------------------------ 54Quaternion 55 q; // [w, x, y, z] quaternion container 56VectorInt16 aa; // 57 [x, y, z] accel sensor measurements 58VectorInt16 aaReal; // [x, 59 y, z] gravity-free accel sensor measurements 60VectorInt16 aaWorld; 61 // [x, y, z] world-frame accel sensor measurements 62VectorFloat 63 gravity; // [x, y, z] gravity vector 64float euler[3]; // 65 [psi, theta, phi] Euler angle container 66float ypr[3]; // [yaw, pitch, 67 roll] yaw/pitch/roll container and gravity vector 68// ------------------------------------------------------------------------ 69// 70 MPU control/status vars 71// ------------------------------------------------------------------------ 72bool 73 dmpReady = false; // set true if DMP init was successful 74uint8_t mpuIntStatus; 75 // holds actual interrupt status byte from MPU 76uint8_t devStatus; // 77 return status after each device operation (0 = success, !0 = error) 78uint16_t 79 packetSize; // expected DMP packet size (default is 42 bytes) 80uint16_t 81 fifoCount; // count of all bytes currently in FIFO 82uint8_t fifoBuffer[64]; 83 // FIFO storage buffer 84 85 86 87// --------------------- Sensor aquisition 88 ------------------------- 89MpuYawPitchRoll ReadMpuRunRobot() 90// --------------------- 91 Sensor aquisition ------------------------- 92// wait for MPU interrupt or extra 93 packet(s) available 94// -------------------------------------------------------------------- 95{ 96 97 if (mpuInterrupt or fifoCount >= packetSize) { 98 if (mpuInterrupt) mpuInterrupt 99 = false; // reset interrupt flag 100 digitalWrite(LED_PIN, HIGH); // blink LED 101 to indicate activity 102 // Angle = ReadMpuRunRobot6050() - CalibrationAngle; 103 104 YawPitchRoll = ReadMpuRunRobot6050(); 105 YawPitchRoll.pitch += Calibration; 106 107 // blinkState = !blinkState; 108 digitalWrite(LED_PIN, LOW); 109 // if 110 (Running) Robot.Run( Angle, SetpointA, SetpointB ); // Robot.Run( Angle, 111 Speed, Speed ); 112 } 113 return YawPitchRoll ; 114} 115// -------------------------------------------------------------------- 116 - 117MpuYawPitchRoll ReadMpuRunRobot6050() 118// -------------------------------------------------------------------- 119 - 120{ 121 static float pitch_old; 122 static float yaw_old; 123 static float 124 yaw_tmp; 125 static float yaw_delta; 126 // static float pitch; 127 128 mpuIntStatus 129 = mpu.getIntStatus(); 130 131 // get current FIFO count 132 fifoCount = mpu.getFIFOCount(); 133 134 // check for overflow (this should never happen unless our code is too inefficient) 135 136 if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 137 // reset so we can continue 138 cleanly 139 // mpu.setDMPEnabled(false); 140 mpu.resetFIFO(); 141 FifoOverflowCnt 142 ++; 143 fifoCount = 0; 144 YawPitchRoll.pitch = pitch_old; 145 return YawPitchRoll; 146 147 148 } 149 // otherwise, check for DMP data ready interrupt (this should happen frequently) 150 151 // Register 58 â€â€\x9D lnterrupt Status INT_STATUS 152 // MPU-6500 Register 153 Map and Descriptions Revision 2.1 154 // Bit [1] DMP_INT This bit automatically 155 sets to 1 when the DMP interrupt has been generated. 156 // Bit [0] RAW_DATA_RDY_INT1 157 Sensor Register Raw Data sensors are updated and Ready to be read. 158 if ((mpuIntStatus) 159 & 0x02 || (mpuIntStatus & 0x01)) { 160 // wait for correct available data length, 161 should be a VERY short wait 162 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 163 164 165 while (fifoCount > 0) { 166 // read a packet from FIFO 167 mpu.getFIFOBytes(fifoBuffer, 168 packetSize); 169 170 // track FIFO count here in case there is > 1 packet available 171 172 // (this lets us immediately read more without waiting for an interrupt) 173 174 fifoCount = mpu.getFIFOCount(); 175 // fifoCount -= packetSize; 176 177 } 178 // the yaw/pitch/roll angles (in degrees) calculated from the quaternions 179 coming 180 // from the FIFO. Note this also requires gravity vector calculations. 181 182 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 183 // 184 more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 185 186 mpu.dmpGetQuaternion(&q, 187 fifoBuffer); 188 mpu.dmpGetGravity(&gravity, &q); 189 // mpu.dmpGetEuler(euler, 190 &q); 191 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 192 193 YawPitchRoll.pitch 194 = -(ypr[1] * 180 / M_PI); //pitch 195 196 yaw_tmp = (abs(ypr[0] * 180 / M_PI)); 197 198 yaw_delta = yaw_tmp - yaw_old; 199 yaw_old = yaw_tmp; 200 YawPitchRoll.yaw 201 += yaw_delta; 202 203 // actual quaternion components in a [w, x, y, z] 204 205 // YawPitchRoll.pitch = (q.y) * 180; 206 // YawPitchRoll.yaw = (q.z ); 207 208 // YawPitchRoll.yaw = mapfloat(YawPitchRoll.yaw , -1.0, 1.0, 0.0, 360.0); 209 210 } 211 pitch_old = YawPitchRoll.pitch ; 212 return YawPitchRoll ; 213} 214// 215 -------------------------------------------------------------- 216void MpuInit() 217// 218 -------------------------------------------------------------- 219// MPU6050_6Axis_MotionApps20.h 220// 221 0x02, 0x16, 0x02, 0x00, 0x09 => 50msec 20 Hz 222// This very last 0x01 WAS 223 a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 224// 0x01 is 100Hz. 225 Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 226// DMP 227 output frequency is calculated easily using this equation: (200Hz / (1 + value)) 228// 229 It is important to make sure the host processor can keep up with reading and processing 230// 231 the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good 232 idea. 233 234{ 235 // after Reset of Arduino there is no Reset of MPU 236 pinMode(MpuInterruptPin, 237 INPUT); 238 239 // verify connection 240 Serial.println(F("Testing device connections...")); 241 242 Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 243 connection failed")); 244 245 // load and configure the DMP 246 Serial.println(F("Initializing 247 DMP...")); 248 devStatus = mpu.dmpInitialize(); 249 // supply your own gyro offsets 250 here, scaled for min sensitivity 251 mpu.setXGyroOffset(220); 252 mpu.setYGyroOffset(76); 253 254 mpu.setZGyroOffset(-84); 255 mpu.setZAccelOffset(1788); // 256 257 // make sure 258 it worked (returns 0 if so) 259 if (devStatus == 0) { 260 // turn on the DMP, 261 now that it's ready 262 Serial.println(F("Enabling DMP...")); 263 mpu.setDMPEnabled(true); 264 265 266 // enable Arduino interrupt detection 267 Serial.println(F("Enabling interrupt 268 detection (Arduino external interrupt 0)...")); 269 attachInterrupt(digitalPinToInterrupt(MpuInterruptPin), 270 dmpDataReady, RISING); 271 mpuIntStatus = mpu.getIntStatus(); 272 273 // set 274 our DMP Ready flag so the main loop() function knows it's okay to use it 275 Serial.println(F("DMP 276 ready! Waiting for first interrupt...")); 277 dmpReady = true; 278 279 // 280 get expected DMP packet size for later comparison 281 packetSize = mpu.dmpGetFIFOPacketSize(); 282 283 // mpu.resetFIFO(); // after Reset importand 284 285 } else { 286 // 287 ERROR! 288 // 1 = initial memory load failed 289 // 2 = DMP configuration 290 updates failed 291 // (if it's going to break, usually the code will be 1) 292 293 Serial.print(F("DMP Initialization failed (code ")); 294 Serial.print(devStatus); 295 296 Serial.println(F(")")); 297 lcd.clear(); 298 lcd.setCursor(0, 0); 299 300 lcd.print( "Error MPU6050 " ); 301 do {} while ( 1 == 1); 302 } 303} 304 305float 306 mapfloat(float x, float in_min, float in_max, float out_min, float out_max) 307{ 308 309 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 310} 311
JoyStickSlave01.ino
c_cpp
1// ADDR:98d3:37:88fa 2 3/* On Uno, Nano, Mini, and Mega, pins 0 and 1 are used for communication with the computer. 4 Connecting anything to these pins can interfere with that communication, 5 including causing failed uploads to the board. 6*/ 7int const UP_BTN = 2; 8int const DOWN_BTN = 4; 9int const LEFT_BTN = 5; 10int const RIGHT_BTN = 3; 11int const E_BTN = 6; 12int const F_BTN = 7; 13 14int const JOYSTICK_BTN = 8; 15int const JOYSTICK_AXIS_X = A0; 16int const JOYSTICK_AXIS_Y = A1; 17 18char c = ' '; 19boolean NL = true; 20long randNumber; 21 22 23int Xvalue, Yvalue, Up, Down, Left , Right, JButton; 24 25 26void setup() 27{ 28 Serial.begin(9600); 29 Serial.print("Sketch: "); Serial.println(__FILE__); 30 Serial.print("Uploaded: "); Serial.println(__DATE__); 31 Serial.println(" "); 32 33} 34 35void loop() 36{ 37 38 if ((Xvalue == map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100)) && 39 (Yvalue == map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100)) && 40 (JButton == digitalRead(JOYSTICK_BTN)) && 41 (Up == digitalRead(UP_BTN)) && 42 (Down == digitalRead(DOWN_BTN)) && 43 (Left == digitalRead(LEFT_BTN)) && 44 (Right == digitalRead(RIGHT_BTN))) return; 45 46 Xvalue = map(analogRead(JOYSTICK_AXIS_X), 0, 1023, -100, 100); 47 Yvalue = map(analogRead(JOYSTICK_AXIS_Y), 0, 1023, -100, 100); 48 JButton = digitalRead(JOYSTICK_BTN); 49 Up = digitalRead(UP_BTN); 50 Down = digitalRead(DOWN_BTN); 51 Left = digitalRead(LEFT_BTN); 52 Right = digitalRead(RIGHT_BTN); 53 54 55 Serial.print("X"); 56 Serial.print("#"); 57 Serial.print(Xvalue); 58 Serial.print("#"); 59 Serial.print("Y"); 60 Serial.print("#"); 61 Serial.print(Yvalue); 62 Serial.print("#"); 63 Serial.print("B1"); 64 Serial.print("#"); 65 Serial.print(JButton); 66 Serial.print("#"); 67 Serial.print("Up"); 68 Serial.print("#"); 69 Serial.print(Up); 70 Serial.print("#"); 71 Serial.print("Do"); 72 Serial.print("#"); 73 Serial.print(Down); 74 Serial.print("#"); 75 Serial.print("Le"); 76 Serial.print("#"); 77 Serial.print(Left); 78 Serial.print("#"); 79 Serial.print("Ri"); 80 Serial.print("#"); 81 Serial.print(Right); 82 Serial.print("#"); 83 Serial.print('\n'); 84 delay(100); 85} 86 87
Plotter.ino
c_cpp
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// -------------------------------------------------------------- 6void SeriellerPlotter( JStickData JStick) 7// -------------------------------------------------------------- 8{ 9 10#ifdef TPlotter 11 MyPlot.SendData("YawPitchRoll.pitch", YawPitchRoll.pitch ); 12 MyPlot.SendData("Delta Pos", Robot.DeltaPosA ); 13 MyPlot.SendData("SetpPos", Robot.SetpointPositionA ); 14 MyPlot.SendData("Setpoint ", (Robot.SetpointA / 10)); 15 MyPlot.SendData("Position", (Robot.PositionA / 10)); 16 MyPlot.SendData("Steps", (Robot.StepsA / 80 )); 17#else 18#ifdef xyPlotter 19 static int xyPlot = 0; 20 if (++xyPlot >= 4) { 21 xyPlot = 0; 22 MyPlot.SendData("ADCValue", YawPitchRoll.pitch, PidMotorA.dTerm ); 23 } 24#else 25#ifdef MSGLog 26 27 MyCSVMessage.Begin(); 28 Serial.print(YawPitchRoll.pitch ); // blau 29 Serial.print(","); 30 Serial.print(Robot.SetpointPositionA ); //grün 31 Serial.print(","); 32 Serial.print(Robot.SetpointA ); //gelb 33 Serial.print(","); 34 Serial.print(PidMotorA.pTerm ); //gelb 35 Serial.print(","); 36 Serial.print(PidMotorA.iTerm ); //gelb 37 Serial.print(","); 38 Serial.print(PidMotorA.dTerm); //gelb 39 Serial.print(","); 40 Serial.print(Robot.StepsA); //grau 41 Serial.print(","); 42 MyCSVMessage.End(); 43 44#else 45 46 Serial.print(Robot.DeltaPosA ); // blau 47 Serial.print(","); 48 Serial.print(Robot.SetpointPositionA ); //grün 49 Serial.print(","); 50 Serial.print(Robot.SetpointA ); //gelb 51/* Serial.print(","); 52 Serial.print(YawPitchRoll.pitch ); // blau 53 Serial.print(","); 54 Serial.print(PidMotorA.pTerm ); //gelb 55 Serial.print(","); 56 Serial.print(PidMotorA.iTerm ); //gelb 57 Serial.print(","); 58 Serial.print(PidMotorA.dTerm); //gelb 59 Serial.print(","); 60 Serial.print(Robot.StepsA); //grau */ 61 Serial.println(" "); 62 63 64#endif 65#endif 66#endif 67} 68
Vehicle.cpp
c_cpp
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 5Cascaded 6 PID Controler 7 8The robot is controlled by cascaded PID controllers. Two controllers 9 are responsible for driving the motors (right and left side). 10Two additional 11 controllers for controlling the position of the robot. The motor controller ensures 12 that the robot remains upright. 13The Position Controler ensures that the robot 14 maintains its correct setpoint prescribed position. 15Cascade control is a cascading 16 of several controllers; the associated control loops are nested in each other. 17The 18 controller output variable of one controller (master controller, Position) serves 19 as the reference variable 20for another (slave controller, Motor). 21The PID 22 controllers are implemented in the "PIDControl" class. 4 instances of the class 23 are instantiated for the 4 controllers. 24The standard PID algorithm was slightly 25 modified after longer test series. To the P-component the parameter Kx multiplied 26 27by the I-component was added. 28*/ 29#include "Vehicle.h" 30 31/**********************************************************************/ 32Vehicle::Vehicle(DuePWMmod* 33 ipwm, Motor * MotorA, Motor * MotorB, // Constructor 34 PidControl 35 * PidMotorA, PidControl * PidMotorB, PidControl * PidDistA, 36 PidControl 37 * PidDistB, PidControl * PidRotation, int iPinSleepSwitch) 38/**********************************************************************/ 39{ 40 41 PinSleepSwitch = iPinSleepSwitch; 42 pinMode(PinSleepSwitch, INPUT_PULLUP); 43 44 pMotorA = MotorA; 45 pMotorB = MotorB; 46 pPidMotorA = PidMotorA; 47 48 pPidMotorB = PidMotorB; 49 pPidDistA = PidDistA; 50 pPidDistB = PidDistB; 51 52 pPidRotation = PidRotation; 53 ptrpwm = ipwm; 54 firstRun = true; 55 56 StopDriving = false; 57} 58 59/**********************************************************************/ 60void 61 Vehicle::Run(MpuYawPitchRoll YawPitchRoll , int &iPositionA, int &iPositionB, JStickData 62 JStick) 63/**********************************************************************/ 64{ 65 /* 66 Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev => 1.8 degree per step 67 68 Wheel Diameter 88mm = > Distance per Pulse Dpp = d * pi / 200 = 1,3816 mm 69 70 Distance per Revolution = 276,32 mm 71 Max 1000 Steps per Second = 5 Revolutions 72 => 13816 mm Distance 73 74 "iPositionA" in microsteps 75 8 microsteps 76 = 1 full Step 77 1 microstepp = 0,125 full steps 78 after division one change 79 in "PositionA" = 0.01 microstepp and 0,0125 full steps = 0,01727 mm 80 */ 81 82 const int tDelay = 10; 83 const float MaxSpeed = 0.8; 84 const float 85 MaxDistance = 1.0; // per tDelay 86 87 PositionA = (float(iPositionA ) / 100 88 ); 89 PositionB = (float(iPositionB ) / 100 ); 90 91 /********************* 92 driverless **************************************/ 93 if (firstRun) { 94 firstRun 95 = false; 96 skipPos = - (2 * tDelay); // first time wait a little bit longer 97 98 CalJstickX = JStick.Xvalue; // calibrate Joy Stick 99 CalJstickY = JStick.Yvalue; 100 101 actYawPosition = -YawPitchRoll.yaw; 102 } 103 JStick.Xvalue = JStick.Xvalue 104 - CalJstickX ; 105 JStick.Yvalue = JStick.Yvalue - CalJstickY; 106 107 /**********************************************************************/ 108 109 110 if (++skipPos >= tDelay) { // to delay the calls, Position Control should be 111 10 times lower than Motor Control 112 skipPos = 0; 113 //After the robot has 114 moved to the right or left, slight deviations occur during the next straight ride. 115 116 //workaound: when driving straight forwad or backwad(!spinning), the equality 117 of the steps is forced. 118 119 if (abs(JStick.Xvalue) > 5) { 120 spinning 121 = true; 122 spinningOld = true; 123 } else { 124 spinning = false; 125 126 if (spinningOld) { 127 resetPIDs( iPositionA, iPositionB); 128 actYawPosition 129 = YawPitchRoll.yaw; 130 } 131 spinningOld = false; 132 } 133 DeltaPosA 134 = (float((JStick.Yvalue)) / 40.0) - (float(JStick.Xvalue ) / 350.0) ; 135 DeltaPosB 136 = (float((JStick.Yvalue)) / 40.0) + (float(JStick.Xvalue ) / 350.0); 137 138 if 139 (abs(DeltaPosA) < 0.05) { 140 DeltaPosA = 0.0; 141 } 142 if (abs(DeltaPosB) 143 < 0.05) { 144 DeltaPosB = 0.0; 145 } 146 147 SetpointPositionA = constrain((SetpointPositionA 148 + DeltaPosA ), (SetpointPositionA - MaxDistance), (SetpointPositionA + MaxDistance)); 149 150 SetpointPositionB = constrain((SetpointPositionB + DeltaPosB ), (SetpointPositionA 151 - MaxDistance), (SetpointPositionA + MaxDistance)) ; 152 SetpointA = constrain( 153 (pPidDistA->calculate (PositionA , SetpointPositionA)), (LastSetpointA - MaxSpeed), 154 (LastSetpointA + MaxSpeed)); // calculate pid seoint 155 SetpointB = constrain( 156 (pPidDistB->calculate (PositionB , SetpointPositionB)), (LastSetpointB - MaxSpeed), 157 (LastSetpointB + MaxSpeed)); 158 LastSetpointPositionA = SetpointPositionA; 159 160 LastSetpointPositionB = SetpointPositionB; 161 LastDeltaPosA = DeltaPosA; 162 163 LastDeltaPosB = DeltaPosB; 164 } 165 SetpointA = ( SetpointA * 0.7) + (LastSetpointA 166 * 0.3); //low-pass filter 167 SetpointB = ( SetpointB * 0.7) + (LastSetpointB * 168 0.3); 169 LastSetpointA = SetpointA ; 170 LastSetpointB = SetpointB ; 171 172 173 // PID calculation of new steps 174 StepsA = pMotorA->Calculate(YawPitchRoll.pitch, 175 -SetpointA); 176 StepsB = pMotorB->Calculate(YawPitchRoll.pitch, -SetpointB); 177 178 179 //After the robot has moved to the right or left, slight deviations occur during 180 the next straight ride. 181 //workaound: when driving straight forwad or backwad(!spinning), 182 the equality of the steps is forced. 183 184 if (!spinning && !auto_spinning) { 185 186 ABdiff = StepsA - StepsB; 187 } 188 // run the Motors, here Steps = full steps 189 190 StepsA = StepsA - (ABdiff / 2); 191 StepsB = StepsB + (ABdiff / 2); 192 // pMotorA->Run(StepsA 193 ); 194 // pMotorB->Run(StepsB); 195 196 // run the Motors, here Steps = full steps 197 198 int freqA; 199 int freqB; 200 201 freqA = int(pMotorA->Run(StepsA )); 202 freqB 203 = int(pMotorB->Run(StepsB)); 204 ptrpwm->setFreq2( freqA, freqB ); 205} 206 207/**********************************************************************/ 208float 209 Vehicle::getNewYaw() 210/**********************************************************************/ 211{ 212 213 float tmp; 214 // tmp = NewYawPos; 215 // NewYawPos = 0.0; 216 return tmp; 217} 218 219/**********************************************************************/ 220void 221 Vehicle::resetPIDs( int &iPositionA, int &iPositionB ) 222/**********************************************************************/ 223{ 224 225 iPositionA = iPositionB = 0; 226 LastSetpointPositionA = LastSetpointPositionB 227 = 0.0; 228 pPidDistA->reset(); 229 pPidDistB->reset(); 230 pPidMotorA->reset(); 231 232 pPidMotorB->reset(); 233 pPidRotation->reset(); 234 LastDeltaPosA = LastDeltaPosB 235 = 0.0; 236 LastSetpointA = LastSetpointB = 0.0; 237} 238 239/**********************************************************************/ 240void 241 Vehicle::init() 242/**********************************************************************/ 243{ 244 245 Serial.println("Vehicle Init Motor Pointer...."); 246 int ptr1 = (int) pMotorA; 247 248 int ptr2 = (int) pMotorB; 249 Serial.println(ptr1 , HEX); 250 Serial.println(ptr2 251 , HEX); 252 253 Serial.println("Vehicle Init PID Pointer...."); 254 int ptr3 255 = (int) pPidMotorA; 256 int ptr4 = (int) pPidMotorB; 257 Serial.println(ptr3 , 258 HEX); 259 Serial.println(ptr4 , HEX); 260 261 Serial.println("Vehicle Init PID 262 Pointer...."); 263 int ptr5 = (int) pPidDistA; 264 int ptr6 = (int) pPidDistB; 265 266 Serial.println(ptr5 , HEX); 267 Serial.println(ptr6 , HEX); 268 269 pMotorA->init( 270 ); 271 pMotorB->init(); 272 273 pMotorA->MsMicrostep(); // set microstepping 274 275 pMotorB->MsMicrostep(); 276 277 pMotorA->SleepMode(); 278 pMotorB->SleepMode(); 279 280} 281/**********************************************************************/ 282void 283 Vehicle::Stop() { 284 pMotorA->SleepMode( ); 285 pMotorB->SleepMode( ); 286} 287/**********************************************************************/ 288void 289 Vehicle::changePIDparams(PidParameter Params) { 290 pPidMotorA->changePIDparams(Params); 291 292 pPidMotorB->changePIDparams(Params); 293 // Serial.println("Vehicle PID Params 294 changed! "); 295} 296void Vehicle::changePIDparams(PidParameterDi Params) { 297 298 pPidDistA->changePIDparams(Params); 299 pPidDistB->changePIDparams(Params); 300 301 // Serial.println("Vehicle PID Params changed! "); 302} 303void Vehicle::changePIDparams(PidParameterRot 304 Params) { 305 pPidRotation->changePIDparams(Params); 306 pPidRotation->changePIDparams(Params); 307 308 // Serial.println("Vehicle PID Params changed! "); 309} 310
Twiddle.cpp
c_cpp
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#include 7 "Twiddle.h" 8/**********************************************************************/ 9Twiddle::Twiddle( 10 int anzParams, float p0 , float p1, float p2, float p3, float p4 , 11 float 12 p5, float p6, float p7, 13 float dp0, float dp1, float dp2, float 14 dp3 , float dp4, 15 float dp5, float dp6 , float dp7) 16/**********************************************************************/ 17{ 18 19 params[0] = p0; 20 params[1] = p1; 21 params[2] = p2; 22 params[3] = 23 p3; 24 params[4] = p4; 25 params[5] = p5; 26 params[6] = p6; 27 params[7] 28 = p7; 29 dparams[0] = dp0; 30 dparams[1] = dp1; 31 dparams[2] = dp2; 32 33 dparams[3] = dp3; 34 dparams[4] = dp4; 35 dparams[5] = dp5; 36 dparams[6] 37 = dp6; 38 dparams[7] = dp7; 39 index = 0; 40 besterr = 9999.99; 41 nextStep 42 = 1; 43 AnzahlElements = anzParams; 44} 45/**********************************************************************/ 46Twiddle::Twiddle( 47 int anzParams, float iparams[], float idparams[] ) // Constructor 48/**********************************************************************/ 49{ 50 index = 0; 51 besterr = 9999.99; 52 nextStep = 1; 53 AnzahlElements = anzParams; 54} 55/**********************************************************************/ 56float 57 Twiddle::next(float error, float &p0, float &p1, float &p2, float &p3, 58 float 59 &p4, float &p5, float &p6, float &p7) 60/**********************************************************************/ 61{ 62 63 static int skip = 0; 64 65 sum_average += abs(error); 66 cnt_average ++; 67 68 69 if (skip > 5 || // for collection some data 70 skip == 0 ) { //first Time 71 72 skip = 1; 73 if ( cnt_average > 0 ) { 74 average = sum_average / cnt_average; 75 76 sum_average = 0; 77 cnt_average = 0; 78 } 79 } 80 else { 81 82 skip ++; 83 return average; 84 } 85 86 if (( dparams[0] + dparams[1] 87 + dparams[2] + dparams[3] + ( dparams[4] + dparams[5] + dparams[6] + dparams[7])) 88 < 0.03 ) { 89 // Serial.println(" erledigt "); 90 p0 = params[0]; 91 92 p1 = params[1]; 93 p2 = params[2]; 94 p3 = params[3]; 95 p4 = params[4]; 96 97 p5 = params[5]; 98 p6 = params[6]; 99 p7 = params[7]; 100 101 // 102 try again 103 dparams[0] *= 4.0; 104 dparams[1] *= 4.0; 105 dparams[2] 106 *= 4.0; 107 dparams[3] *= 4.0; 108 dparams[4] *= 4.0; 109 dparams[5] *= 110 4.0; 111 dparams[6] *= 4.0; 112 dparams[7] *= 4.0; 113 besterr = 9999.99; 114 115 116 return average; // it is done 117 } 118 119 120 if ( nextStep == 3 ) { 121 122 nextStep = 1; 123 if (index == AnzahlElements - 1) { 124 index = 0; 125 126 } else { 127 index ++; 128 } 129 params[index] += dparams[index]; 130 131 } 132 133 logging(); // last step 134 135 calcCost(average); 136 137 p0 = params[0]; 138 139 p1 = params[1]; 140 p2 = params[2]; 141 p3 = params[3]; 142 p4 = params[4]; 143 144 p5 = params[5]; 145 p6 = params[6]; 146 p7 = params[7]; 147 return average; 148} 149//---------------------------------------------------------------- 150void 151 Twiddle::calcCost(float average) 152//---------------------------------------------------------------- 153// 154 Dieser Algorithmus sucht nun den Parameterraum intelligent ab und 155// variiert 156 die Schrittweite der Suche, je nachdem ob man in der Nähe 157// eines Maxima bzw. 158 Minima ist. 159{ 160 161 switch (nextStep) { 162 case 1: 163 if (average 164 < besterr) { 165 // aktuelle Kosten < besterr # There was some improvement 166 167 besterr = average; 168 //mit größerem Schritt probieren 169 dparams[index] 170 *= 1.1; 171 nextStep = 3; 172 } else // # There was no improvement 173 174 { 175 // # Go into the other direction 176 params[index] = params[index] 177 - (2 * dparams[index]); 178 nextStep = 2; 179 } 180 break; 181 182 183 case 2: 184 if (average < besterr) { 185 // aktuelle Kosten < besterr 186 # There was some improvement 187 besterr = average; 188 //mit größerem 189 Schritt probieren 190 dparams[index] *= 1.05; 191 nextStep = 1; 192 193 } else { 194 // As there was no improvement, the step size in either 195 196 // direction, the step size might simply be too big. 197 params[index] 198 += dparams[index]; 199 dparams[index] *= 0.95;//an sonsten kleineren Schritt 200 201 nextStep = 3; 202 } 203 break; 204 } 205} 206/*------------------------------------------------------------ 207 208 # Choose an initialization parameter vector 209 p = [0, 0, 0] 210 # Define potential 211 changes 212 dp = [1, 1, 1] 213 # Calculate the error 214 best_err = A(p) 215 216 threshold = 0.001 217 while sum(dp) > threshold: 218 for i in range(len(p)): 219 220 p[i] += dp[i] 221 err = A(p) 222 if err < best_err: # There 223 was some improvement 224 best_err = err 225 dp[i] *= 1.1 226 227 else: # There was no improvement 228 p[i] -= 2*dp[i] # Go into 229 the other direction 230 err = A(p) 231 if err < best_err: 232 # There was an improvement 233 best_err = err 234 dp[i] 235 *= 1.05 236 else # There was no improvement 237 p[i] 238 += dp[i] 239 # As there was no improvement, the step size in either 240 241 # direction, the step size might simply be too big. 242 dp[i] 243 *= 0.95 244 245 https://www.gomatlab.de/twiddle-algorithmus-zum-optimieren-von-parametern-t24517.html 246 247 % Maximierung der Kostenfunktion! 248 while sum(dparams) > 0.01 249 for i=1:length(params) 250 % alle Parameter durch gehen 251 params(i)=params(i)+dparams(i); 252 %Kostenfunktion 253 ausrechnen 254 cfzz(it) = calcCost(params(1),params(2)); 255 if cfzz(it) 256 > besterr 257 besterr = cfzz(it); 258 dparams(i)= dparams(i)*1.05; 259 260 else 261 % in andere Richtung suchen 262 params(i)=params(i)- 263 2*dparams(i); 264 cfzz(it) = calcCost(params(1),params(2)); 265 if 266 cfzz(it) > besterr %wenn aktuelle Kosten höher (=gut) 267 besterr 268 = cfzz(it); 269 dparams(i) = dparams(i)*1.05; %mit größerem Schritt 270 probieren 271 else 272 params(i)=params(i)+dparams(i); 273 274 dparams(i)=dparams(i)*0.95; % an sonsten kleineren Schritt 275 end 276 277 end 278 it = it+1; 279 end 280*/ 281 282//---------------------------------------------------------------- 283void 284 Twiddle::logging() 285//---------------------------------------------------------------- 286{ 287 288 289 Serial.print(" Step= "); 290 Serial.print(nextStep ); 291 Serial.print(" 292 Ind= "); 293 Serial.print(index ); 294 Serial.print(" av= "); 295 Serial.print(average 296 , 4 ); 297 Serial.print(" besterr "); 298 Serial.print(besterr , 4 ); 299 300 Serial.print(" P0 "); 301 Serial.print(params[0] , 4 ); 302 Serial.print(" 303 P1 "); 304 Serial.print(params[1] , 4); 305 Serial.print(" P2 "); 306 307 Serial.print(params[2] , 4 ); 308 Serial.print(" P3 "); 309 Serial.print(params[3] 310 , 4 ); 311 Serial.print(" P4 "); 312 Serial.print(params[4] , 2 ); 313 314 Serial.print(" P5 "); 315 Serial.print(params[5] , 2); 316 Serial.print(" 317 P6 "); 318 Serial.print(params[6] , 2 ); 319 Serial.print(" P7 "); 320 321 Serial.print(params[7] , 4 ); 322 Serial.println(" "); 323 324/* 325 326 // Serial.println("{Message|data|Hello World!}"); 327 Serial.print("{MESSAGE:" 328 ); 329 Serial.print("Log" ); 330 Serial.print("|data|" ); 331 Serial.print(average, 332 4 ); 333 Serial.print(", "); 334 Serial.print(besterr, 4 ); 335 Serial.print(", 336 "); 337 Serial.print(params[0], 2 ); 338 Serial.print(", "); 339 Serial.print(params[1], 340 2 ); 341 Serial.print(", "); 342 Serial.print(params[2], 2 ); 343 Serial.print(", 344 "); 345 Serial.print(params[3], 2); 346 Serial.print(", "); 347 Serial.print(params[4], 348 2); 349 Serial.print(", "); 350 Serial.print(params[5], 2); 351 Serial.print(", 352 "); 353 Serial.print(params[6], 2); 354 Serial.println("}"); 355*/ 356}
SBRobotDueDP6500_NewPWM_55.ino
c_cpp
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
PidControl.cpp
c_cpp
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
PidParameter.h
c_cpp
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#ifndef 7 PidParameter_h 8#define PidParameter_h 9#include "PidParameter.h" 10 11// 12 PidParameter Motor 13struct PidParameter { 14 float K = 5.0; 15 float 16 Kp = 9.3 ; 17 float Ki = 5.27; //6.01; 18 float Kd 19 = 0.13; //0.12 ; 20 float Ka = 0.1007; //0.1246; 21 float Kx 22 = 0.0; // 23}; 24// PidParameter Position 25struct PidParameterDi { 26 float 27 K = 1.0; 28 float Kp = 0.18; 29 float Ki = 0.0; 30 31 float Kd = 0.4 ; 32 float Ka = 0.0 ; 33 float Kx 34 = 0.0; 35}; 36// PidParameter Position 37struct PidParameterRot { //not yet used 38 39 float K = 1.0; 40 float Kp = 0.01 ; //0.0037; 41 float 42 Ki = 0.000; 43 float Kd = 0.00; 44 float Ka = 0.0 45 ; 46 float Kx = 0.0; 47}; 48#endif 49
DueTimer.cpp
c_cpp
1/* 2 DueTimer.cpp - Implementation of Timers defined on DueTimer.h 3 4 For instructions, go to https://github.com/ivanseidel/DueTimer 5 6 Created 7 by Ivan Seidel Gomes, March, 2013. 8 Modified by Philipp Klaus, June 2013. 9 10 Thanks to stimmer (from Arduino forum), for coding the "timer soul" (Register 11 stuff) 12 Released into the public domain. 13*/ 14 15#include <Arduino.h> 16#if 17 defined(_SAM3XA_) 18#include "DueTimer.h" 19 20const DueTimer::Timer DueTimer::Timers[NUM_TIMERS] 21 = { 22 {TC0, 0, TC0_IRQn}, 23 {TC0, 1, TC1_IRQn}, 24 {TC0, 2, TC2_IRQn}, 25 26 {TC1, 0, TC3_IRQn}, 27 {TC1, 1, TC4_IRQn}, 28 {TC1, 2, TC5_IRQn}, 29 {TC2, 30 0, TC6_IRQn}, 31 {TC2, 1, TC7_IRQn}, 32 {TC2, 2, TC8_IRQn}, 33}; 34 35// 36 Fix for compatibility with Servo library 37#ifdef USING_SERVO_LIB 38// Set callbacks 39 as used, allowing DueTimer::getAvailable() to work 40void (*DueTimer::callbacks[NUM_TIMERS])() 41 = { 42 (void (*)()) 1, // Timer 0 - Occupied 43 (void (*)()) 0, // Timer 1 44 45 (void (*)()) 1, // Timer 2 - Occupied 46 (void (*)()) 1, // Timer 3 - Occupied 47 48 (void (*)()) 1, // Timer 4 - Occupied 49 (void (*)()) 1, // Timer 5 - Occupied 50 51 (void (*)()) 0, // Timer 6 52 (void (*)()) 0, // Timer 7 53 (void (*)()) 0 54 // Timer 8 55}; 56#else 57void (*DueTimer::callbacks[NUM_TIMERS])() = {}; 58#endif 59float 60 DueTimer::_frequency[NUM_TIMERS] = { -1, -1, -1, -1, -1, -1, -1, -1, -1}; 61 62/* 63 64 Initializing all timers, so you can use them like this: Timer0.start(); 65*/ 66DueTimer 67 Timer(0); 68 69DueTimer Timer1(1); 70// Fix for compatibility with Servo library 71#ifndef 72 USING_SERVO_LIB 73DueTimer Timer0(0); 74DueTimer Timer2(2); 75DueTimer Timer3(3); 76DueTimer 77 Timer4(4); 78DueTimer Timer5(5); 79#endif 80DueTimer Timer6(6); 81DueTimer Timer7(7); 82DueTimer 83 Timer8(8); 84 85DueTimer::DueTimer(unsigned short _timer) : timer(_timer) { 86 87 /* 88 The constructor of the class DueTimer 89 */ 90} 91 92DueTimer DueTimer::getAvailable(void) 93 { 94 /* 95 Return the first timer with no callback set 96 */ 97 98 for 99 (int i = 0; i < NUM_TIMERS; i++) { 100 if (!callbacks[i]) 101 return DueTimer(i); 102 103 } 104 // Default, return Timer0; 105 return DueTimer(0); 106} 107 108DueTimer& 109 DueTimer::attachInterrupt(void (*isr)()) { 110 /* 111 Links the function passed 112 as argument to the timer of the object 113 */ 114 115 callbacks[timer] = isr; 116 117 118 return *this; 119} 120 121DueTimer& DueTimer::detachInterrupt(void) { 122 /* 123 124 Links the function passed as argument to the timer of the object 125 */ 126 127 128 stop(); // Stop the currently running timer 129 130 callbacks[timer] = NULL; 131 132 133 return *this; 134} 135 136DueTimer& DueTimer::start(float microseconds) { 137 138 /* 139 Start the timer 140 If a period is set, then sets the period and 141 start the timer 142 */ 143 144 if (microseconds > 0) 145 setPeriod(microseconds); 146 147 148 if (_frequency[timer] <= 0) 149 setFrequency(1); 150 151 NVIC_ClearPendingIRQ(Timers[timer].irq); 152 153 NVIC_EnableIRQ(Timers[timer].irq); 154 155 TC_Start(Timers[timer].tc, Timers[timer].channel); 156 157 158 return *this; 159} 160 161DueTimer& DueTimer::stop(void) { 162 /* 163 Stop 164 the timer 165 */ 166 167 NVIC_DisableIRQ(Timers[timer].irq); 168 169 TC_Stop(Timers[timer].tc, 170 Timers[timer].channel); 171 172 return *this; 173} 174 175uint8_t DueTimer::bestClock(float 176 frequency, uint32_t& retRC) { 177 /* 178 Pick the best Clock, thanks to Ogle 179 Basil Hall! 180 181 Timer Definition 182 TIMER_CLOCK1 MCK / 2 183 TIMER_CLOCK2 184 MCK / 8 185 TIMER_CLOCK3 MCK / 32 186 TIMER_CLOCK4 MCK /128 187 */ 188 189 const struct { 190 uint8_t flag; 191 uint8_t divisor; 192 } clockConfig[] 193 = { 194 { TC_CMR_TCCLKS_TIMER_CLOCK1, 2 }, 195 { TC_CMR_TCCLKS_TIMER_CLOCK2, 196 8 }, 197 { TC_CMR_TCCLKS_TIMER_CLOCK3, 32 }, 198 { TC_CMR_TCCLKS_TIMER_CLOCK4, 199 128 } 200 }; 201 float ticks; 202 float error; 203 int clkId = 3; 204 int bestClock 205 = 3; 206 float bestError = 9.999e99; 207 do 208 { 209 ticks = (float) VARIANT_MCK 210 / frequency / (float) clockConfig[clkId].divisor; 211 // error = abs(ticks - 212 round(ticks)); 213 error = clockConfig[clkId].divisor * abs(ticks - round(ticks)); 214 // Error comparison needs scaling 215 if (error < bestError) 216 { 217 bestClock 218 = clkId; 219 bestError = error; 220 } 221 } while (clkId-- > 0); 222 ticks 223 = (float) VARIANT_MCK / frequency / (float) clockConfig[bestClock].divisor; 224 225 retRC = (uint32_t) round(ticks); 226 return clockConfig[bestClock].flag; //Rolf 227 228 229} 230 231 232DueTimer& DueTimer::setFrequency(float frequency) { 233 /* 234 235 Set the timer frequency (in Hz) 236 */ 237 238 // Prevent negative frequencies 239 240 if (frequency <= 0) { 241 frequency = 1; 242 } 243 244 // Remember the frequency 245 see below how the exact frequency is reported instead 246 //_frequency[timer] 247 = frequency; 248 249 // Get current timer configuration 250 Timer t = Timers[timer]; 251 252 253 uint32_t rc = 0; 254 uint8_t clock; 255 256 // Tell the Power Management Controller 257 to disable 258 // the write protection of the (Timer/Counter) registers: 259 pmc_set_writeprotect(false); 260 261 262 // Enable clock for the timer 263 pmc_enable_periph_clk((uint32_t)t.irq); 264 265 266 // Find the best clock for the wanted frequency 267 clock = bestClock(frequency, 268 rc); 269 270 switch (clock) { 271 case TC_CMR_TCCLKS_TIMER_CLOCK1: 272 _frequency[timer] 273 = (float )VARIANT_MCK / 2.0 / (float )rc; 274 break; 275 case TC_CMR_TCCLKS_TIMER_CLOCK2: 276 277 _frequency[timer] = (float )VARIANT_MCK / 8.0 / (float )rc; 278 break; 279 280 case TC_CMR_TCCLKS_TIMER_CLOCK3: 281 _frequency[timer] = (float )VARIANT_MCK 282 / 32.0 / (float )rc; 283 break; 284 default: // TC_CMR_TCCLKS_TIMER_CLOCK4 285 286 _frequency[timer] = (float )VARIANT_MCK / 128.0 / (float )rc; 287 break; 288 289 } 290 291 // Set up the Timer in waveform mode which creates a PWM 292 // in 293 UP mode with automatic trigger on RC Compare 294 // and sets it up with the determined 295 internal clock as clock input. 296 TC_Configure(t.tc, t.channel, TC_CMR_WAVE | 297 TC_CMR_WAVSEL_UP_RC | clock); 298 // Reset counter and fire interrupt when RC value 299 is matched: 300 TC_SetRC(t.tc, t.channel, rc); 301 // Enable the RC Compare Interrupt... 302 303 t.tc->TC_CHANNEL[t.channel].TC_IER = TC_IER_CPCS; 304 // ... and disable all 305 others. 306 t.tc->TC_CHANNEL[t.channel].TC_IDR = ~TC_IER_CPCS; 307 308 return 309 *this; 310} 311 312DueTimer& DueTimer::setPeriod(float microseconds) { 313 /* 314 315 Set the period of the timer (in microseconds) 316 */ 317 318 // Convert period 319 in microseconds to frequency in Hz 320 float frequency = 1000000.0 / microseconds; 321 322 setFrequency(frequency); 323 return *this; 324} 325 326float DueTimer::getFrequency(void) 327 const { 328 /* 329 Get current time frequency 330 */ 331 332 return _frequency[timer]; 333} 334 335float 336 DueTimer::getPeriod(void) const { 337 /* 338 Get current time period 339 */ 340 341 342 return 1.0 / getFrequency() * 1000000; 343} 344 345 346/* 347 Implementation 348 of the timer callbacks defined in 349 arduino-1.5.2/hardware/arduino/sam/system/CMSIS/Device/ATMEL/sam3xa/include/sam3x8e.h 350*/ 351// 352 Fix for compatibility with Servo library 353#ifndef USING_SERVO_LIB 354void TC0_Handler(void) 355 { 356 TC_GetStatus(TC0, 0); 357 DueTimer::callbacks[0](); 358} 359#endif 360void 361 TC1_Handler(void) { 362 TC_GetStatus(TC0, 1); 363 DueTimer::callbacks[1](); 364} 365// 366 Fix for compatibility with Servo library 367#ifndef USING_SERVO_LIB 368void TC2_Handler(void) 369 { 370 TC_GetStatus(TC0, 2); 371 DueTimer::callbacks[2](); 372} 373void TC3_Handler(void) 374 { 375 TC_GetStatus(TC1, 0); 376 DueTimer::callbacks[3](); 377} 378void TC4_Handler(void) 379 { 380 TC_GetStatus(TC1, 1); 381 DueTimer::callbacks[4](); 382} 383void TC5_Handler(void) 384 { 385 TC_GetStatus(TC1, 2); 386 DueTimer::callbacks[5](); 387} 388#endif 389void 390 TC6_Handler(void) { 391 TC_GetStatus(TC2, 0); 392 DueTimer::callbacks[6](); 393} 394void 395 TC7_Handler(void) { 396 TC_GetStatus(TC2, 1); 397 DueTimer::callbacks[7](); 398} 399void 400 TC8_Handler(void) { 401 TC_GetStatus(TC2, 2); 402 DueTimer::callbacks[8](); 403} 404#endif 405 406
DuePWMmod.cpp
c_cpp
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
Battery.cpp
c_cpp
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 #include "Battery.h" 9#include <Arduino.h> 10 11/**********************************************************************/ 12Battery::Battery(int 13 PIN) 14/**********************************************************************/ 15{ 16 17 _VoltPin = PIN; 18 pinMode(_VoltPin, INPUT); 19} 20 21// ----------------------------------------------------------------------------- 22float 23 Battery::VoltageRead() 24// -------------------------------------------------------------- 25{ 26 27 Voltage = ( analogRead(_VoltPin)) * 0.018 ; 28 /*Serial.print("Voltage "); 29 30 Serial.print(Voltage); 31 Serial.println(VoltPin );*/ 32 return Voltage; 33} 34// 35 ----------------------------------------------------------------------------- 36bool 37 Battery::VoltageCheck() 38// -------------------------------------------------------------- 39{ 40 41 // Lipo Nennspannung von 3,7 Volt 42 // die Spannung der einzelnen Zellen nicht 43 unter 3,2 Volt sinkt. 44 // voltage divider: 45 // 21 Kohm 4,7 Kohm maximal 46 12 Volt 47 // Uinp = U * R1 / ( R1 + R2 ) = 0,183 * U 48 // 1024 (resolution) 49 50 // analogRead(VoltPin)) * 5.0VoltRef / 1024.0 / 0.183 = 0.027 51 // in case 52 of Resistor toleranz 0.0225 53 54 // Voltage = ( analogRead(VoltPin)) * 0.0225 55 ; 56 Voltage = VoltageRead(); 57 58 if (Voltage > 4.8 && Voltage < 6.4 ) { 59 60 return false; 61 } 62 return true; 63} 64
Battery.h
c_cpp
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
Motor.cpp
c_cpp
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 Stepper Motor: Unipolar/Bipolar, 200 Steps/Rev, 42×48mm, 4V, 1.2 A/Phase 8 9 https://www.pololu.com/product/1200/faqs 10 This NEMA 17-size hybrid stepping 11 motor can be used as a unipolar or bipolar stepper motor 12 and has a 1.8° step 13 angle (200 steps/revolution). Each phase draws 1.2 A at 4 V, 14 allowing for a 15 holding torque of 3.2 kg-cm (44 oz-in). 16 17 Wheel Durchmesser 88mm = > Distance 18 per Pulse Dpp = d * pi / 200 = 1,3816 mm 19 Distance per Revolution = 276,32 20 mm 21 Max 1000 Steps per Second = 5 Revolutions => 13816 mm Distance 22 23 Motion 24 Control Modules » Stepper Motor Drivers » MP6500 Stepper Motor Driver Carriers 25 26 https://www.pololu.com/product/2966https://www.pololu.com/product/2966 27 This 28 breakout board for the MPS MP6500 microstepping bipolar stepper motor driver has 29 a 30 pinout and interface that are very similar to that of our popular A4988 carriers, 31 32 so it can be used as a drop-in replacement for those boards in many applications. 33 34 The MP6500 offers up to 1/8-step microstepping, operates from 4.5 V to 35 V, 35 36 and can deliver up to approximately 1.5 A per phase continuously without a heat 37 sink 38 or forced air flow (up to 2.5 A peak). 39 40 MS1 MS2 Microstep 41 Resolution 42 Low Low Full step 43 High Low Half (1/2) step 44 Low 45 High Quarter (1/4) step 46 High High Eighth (1/8) step 47*/ 48 49#include 50 "Motor.h" 51#include "Config.h" 52/**********************************************************************/ 53Motor::Motor(DuePWMmod* 54 ipwm, PidControl * Pid, int iPinDir, int iPinStep, 55 int iPinMs1, 56 int iPinMs2, int iPinSleep, char iMotorSide ) 57/**********************************************************************/ 58{ 59 60 _Ms1 = iPinMs1; 61 _Ms2 = iPinMs2; 62 _PinStep = iPinStep; 63 _PinDir 64 = iPinDir; 65 _PinSleep = iPinSleep; 66 _MotorSide = iMotorSide; 67 68 // 69 The default SLEEP state prevents the driver from operating; 70 // this pin must 71 be high to enable the driver 72 pinMode(_PinSleep, OUTPUT); 73 pinMode(_PinDir, 74 OUTPUT); 75 pinMode(_Ms1, OUTPUT); 76 pinMode(_Ms2, OUTPUT); 77 78 ptr = 79 (int) this; 80 // WrapperMode = true; 81 ptrPid = Pid; 82 ptrpwm = ipwm; 83} 84/**********************************************************************/ 85Motor* 86 Motor::getInstance() 87/**********************************************************************/ 88{ 89 90 pMotor = this; 91 return pMotor; 92} 93/**********************************************************************/ 94void 95 Motor::init ( ) 96/**********************************************************************/ 97{ 98 99 100 Serial.print("Motor "); 101 Serial.print(ptr , HEX); 102 Serial.print(" Side 103 "); 104 Serial.print(_MotorSide); 105 Serial.print(" iPinStep "); 106 Serial.print(_PinStep); 107 108 Serial.print(" iPinSleep "); 109 Serial.print(_PinSleep); 110 Serial.println(" 111 init..."); 112 lastTime = millis(); 113 114 ptrPid->changePIDparams( params); 115} 116/**********************************************************************/ 117void 118 Motor::SleepMode( ) 119/**********************************************************************/ 120{ 121 122 digitalWrite(_PinSleep, LOW); 123 MotorMode = false; 124} 125/**********************************************************************/ 126void 127 Motor::RunMode( ) 128/**********************************************************************/ 129{ 130 131 digitalWrite(_PinSleep, HIGH); 132 MotorMode = true; 133} 134/**********************************************************************/ 135void 136 Motor::toggleMode( ) 137/**********************************************************************/ 138{ 139 140 if ( MotorMode == false ) RunMode( ); 141 else SleepMode(); 142} 143/**********************************************************************/ 144float 145 Motor::Calculate(float angle, float Setpoint) 146/**********************************************************************/ 147{ 148 149 Steps = ptrPid->calculate (angle, Setpoint ); 150 return Steps; 151} 152/**********************************************************************/ 153float 154 Motor::Run(float Steps) { 155 /**********************************************************************/ 156 157 if (!digitalRead(PinSleepSwitch)) { 158 RunMode( ); 159 if (Steps >= 0 ) 160 { 161 if (_MotorSide == rechterMotor) { 162 digitalWrite(_PinDir, LOW); 163 164 } 165 else { 166 digitalWrite(_PinDir, HIGH); 167 } 168 } 169 else 170 { 171 if (_MotorSide == rechterMotor) { 172 digitalWrite(_PinDir, 173 HIGH); 174 } 175 else { 176 digitalWrite(_PinDir, LOW); 177 } 178 179 } 180 181 if (_Divisor > 0) { 182 Steps = Steps * _Divisor; // convert 183 into Microsteps 184 } 185 186 Steps_tmp = Steps; 187 Steps = abs(Steps_tmp); 188 189 190 if ( Steps < 2.0) Steps = 2.0; // Microsteps 191 return Steps; 192 } else 193 SleepMode( ); 194 195} 196/**********************************************************************/ 197void 198 Motor::MsFull ( ) { 199 digitalWrite(_Ms1, LOW); 200 digitalWrite(_Ms2, LOW); 201 202 _Divisor = 1; 203} 204void Motor::MsHalf ( ) { 205 digitalWrite(_Ms1, LOW); 206 207 digitalWrite(_Ms2, HIGH); 208 _Divisor = 2; 209} 210void Motor::MsQuater ( ) 211 { 212 digitalWrite(_Ms1, HIGH); 213 digitalWrite(_Ms2, LOW); 214 _Divisor = 4; 215} 216void 217 Motor::MsMicrostep ( ) { 218 digitalWrite(_Ms1, HIGH); 219 digitalWrite(_Ms2, 220 HIGH); 221 _Divisor = 8; 222 // Serial.println("MsMicrostep"); 223} 224
Config.h
c_cpp
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#ifndef 7 Config_h 8#define Config_h 9#include "Arduino.h" 10#include "Filter.h" 11 12 13/* 14 PIN Belegung 15 D2 Interrupt 0 for MPU6050 16 D4 LiquidCrystal DB4 17 D5 18 LiquidCrystal DB5 19 D9 LiquidCrystal rs grau 20 D8 LiquidCrystal enable 21 lila 22 D10 LiquidCrystal DB6 gelb > 23 D11 LiquidCrystal DB7 orange > 24 25 D18 TX1 BlueThooth 26 D19 RX1 BlueThooth 27 D36 InterruptStartButton 28 29 D42 Test Pin 30 D44 Test Pin 31 32 D36 Motor A Direction 33 D6 Motor 34 A Step 35 D40 Motor B Direction 36 D7 Motor B Step 37 38 D28 PinMs1MotorA 39 40 D30 PinMs2MotorA 41 D46 PinMs1MotorB 42 D48 PinMs2MotorB 43 44 D22 45 Sleep MotorA 46 D24 Sleep MotorB 47 D53 Sleep Switch Input 48 49 A6 Spannung 50 VoltPin 51 A7 Tuning Poti 10Kohm 52 53 LiquidCrystal(rs, enable, d4, d5, 54 d6, d7) 55 LiquidCrystal lcd(9, 8, 4, 5, 10, 11); 56 10K resistor: 57 ends 58 to +5V and ground 59 wiper (3) to LCD VO pin 60*/ 61 62#define MpuInterruptPin 63 2 // use pin on Arduino for MPU Interrupt 64#define LED_PIN 13 65#define 66 MpuIntPin 27 //the interrupt is used to determine when new data is available. 67 68 69 70const 71 int PinMs1MotorA = 28; 72const int PinMs2MotorA = 30; 73const int PinMs1MotorB 74 = 46; 75const int PinMs2MotorB = 48; 76const int PinDirMotorA = 36; 77const 78 int PinDirMotorB = 40; 79const int PinStepMotorA = 6; 80const int PinStepMotorB 81 = 7; 82const int PinSleepSwitch = 53; 83const int PinSleepA = 22; 84const 85 int PinSleepB = 24; 86const int VoltPin = A6; // Voltage VIN 87const 88 int TuningPin = A7; //Potentiometer 89 90struct JStickData { 91 int Xvalue, 92 Yvalue, Up=1, Down=1, Left=1, Right=1, JButton=1; 93}; 94 95struct MpuYawPitchRoll 96 { 97 float yaw, pitch, roll; 98}; 99 100 101#endif 102
Downloadable files
SB Robot
SB Robot
SB Robot
SB Robot
Comments
Only logged in users can leave comments
bobfound
2 years ago
Rolf, why did you change from a Mega to a Due? I know the Due is faster, more memory...but was it needed?
Anonymous user
2 years ago
Hello my friend, how are you? Your job is fantastic. Could you send me the step-by-step? I would like to do this work for a science fair. Can you help me? Please.
Anonymous user
2 years ago
Nice project! Did you ever try to make it turn left or right?
RolfK
2 years ago
Hello, now the robot is moving in all directions. As usual it was a bit more elaborate as expected. Have a look at the new video. Many greetings from Germany. Rolf
RolfK
2 years ago
Hello , That's next. It's prepared. Viele Grüße Rolf
Anonymous user
2 years ago
Where is Filter.h that appears in config.h? I found the others and coverted XXX_ino.c to XXX.ino and most of main file compiles except for that error.
Anonymous user
2 years ago
I've been struggling trying to get this code to compile. There is an Include <duepwmmod.h> that I can't find anywhere on the Internet. The program name ends with xxx_ino.c. What is that about? Is it an Arduino "ino" file or a C file, or both? I've been removing the C as extension and using .ino but not sure if this works. This version on this site is different from two other versions but there is no version history, just that the file was created in 2019. This version here seems to have more "stuff", like Megunalink codes, but it still has the <duepwmmod.h> library which cannot be found. My robot is BUILT...I just need some help with the coding and Rolf's work is the best so far.
Anonymous user
2 years ago
Where is Filter.h that appears in config.h? I found the others and coverted XXX_ino.c to XXX.ino and most of main file compiles except for that error.
Anonymous user
2 years ago
I've been struggling trying to get this code to compile. There is an Include <duepwmmod.h> that I can't find anywhere on the Internet. The program name ends with xxx_ino.c. What is that about? Is it an Arduino "ino" file or a C file, or both? I've been removing the C as extension and using .ino but not sure if this works. This version on this site is different from two other versions but there is no version history, just that the file was created in 2019. This version here seems to have more "stuff", like Megunalink codes, but it still has the <duepwmmod.h> library which cannot be found. My robot is BUILT...I just need some help with the coding and Rolf's work is the best so far.
bobfound
2 years ago
Rolf, why did you change from a Mega to a Due? I know the Due is faster, more memory...but was it needed?
Anonymous user
3 years ago
Cool. Any chance this can run on an Arduino Uno as well? What would it take to modify?
Anonymous user
5 years ago
Hello my friend, how are you? Your job is fantastic. Could you send me the step-by-step? I would like to do this work for a science fair. Can you help me? Please.
Anonymous user
6 years ago
Nice project! Did you ever try to make it turn left or right?
RolfK
2 years ago
Hello, now the robot is moving in all directions. As usual it was a bit more elaborate as expected. Have a look at the new video. Many greetings from Germany. Rolf
RolfK
2 years ago
Hello , That's next. It's prepared. Viele Grüße Rolf
RolfK
2 Followers
•3 Projects
21
16
DaveX
10 months ago
How does the Kx term work any differently than a plain Ki term? It looks like they'd combine to be: `Kboth = Kx +Ki/1000` (or `Kboth = Kp*Kx +Ki/1000` depending whether the code or the written formulas are the used), and work exactly like `Kboth * integrated_error`.