1) Mechanics
2) Electronics
3) Motor control
4) Control algorithms
4.1 Position control - " Lageregelung
4.1.1 Tuning the position controller
4.2 Speed control - " Geschwindigkeitsregelung
4.2.1 Tuning the speed controller
4.3 Position control - " Positionsregelung

Published February 7, 2022 © MIT

Self-Balancing Robot

Build a self balancing robot using our Arduino compatible XMC4700 microcontroller.

IntermediateFull instructions provided5 hours1,487

Things used in this project

Hardware components

Infineon KIT XMC47 RELAX LITE V1

Adafruit Motor/Stepper/Servo Shield for Arduino v2 Kit - v2.3

Motor shield

Adafruit 9-DOF Absolute Orientation IMU Fusion Breakout - BNO055

Orientation sensor

NEMA 17 Stepper SM-42BYG011-25

3s/2400mAh lipo battery

RC-car wheels

Software apps and online services

Arduino IDE

Hand tools and fabrication machines

Soldering iron (generic)

3D Printer (generic)

Story

We built this self stabalizing robot as a base setup to show how robots will interact with their sourrunding based on different sensor types.

In this example you can see the robot equipped with a 24GHz radar and an ultrasonic sensor:

The XMC4700 offers several unused communication interfaces to add different kind of sensors to your project.

Interested in building your own robot?Find instructions how to build it HERE.

1) Mechanics

Start your build with the "Top PlateXMC4700" or "Top Plate XMC4700 electric wire". The electric wire version offers a surrounding notch for an electroluminescent wire.

Top Plate XMC4700 electric wire

This part fits a DCDC voltage converter (WE 173010578) from 12V battery voltage to 5V for our controller and a possible electroluminescence inverter.Further it keeps your cables in place and holds the robots main switch.

Top Plate XMC4700 electric wire assembled

Screw the XMC4700 Relax Kit on top of it and stack the Motorshield on top of the microcontroller.The Motorshield offers an open area which is great to solder the IMU(inertial measurement unit) sensor right on top.

Controller, sensor and powerboard

Continue with printing and assembly of the lower part - the chassis.It holds the two motors as well as the optional electroluminescence inverter.

chassis

chassis mounted

Mount the top plate to the chassis using M3 spacers that matches your batterie height.

To connect wheels to the motors we used M5 spacers which we drilled out and added a thread to fix the wheel axle to the motor axle.

wheel axle

To attach the wheels to the wheel axle we printed another adaptor:

wheel adaptor

That's it - these are all parts you need to assemble your self-stabilized robot

Full assembly

2) Electronics

This section gives you a short intro into the used electronic components. For a more detailed look check the supplier webpages.

Stepper Motors (Nema17) Steppers are important whenever you need to set an exact position. For such kind of robots that's not a must have but we used them to drive the robot back to its "home position" which is the starting point after you pushed it away.

Stepper Motor Nema17

Adafruit Motor/Stepper/Servo Shield for Arduino v2 This is the power stage to drive the motors. It's using TB6612 MOSFET drivers with 1.2A current capability per channel and comes with a handy Arduino library to drive stepper motors.

Adafruit Motor Control Shield

Elwire We use an electroluminescence (EL) wire to show the mode of our robot. It is a wire that you feed with AC voltage of about 100V and 2500Hz. This will let it glow all over its surface.

EL wire

ELInverter To create the AC voltage for our EL wire an EL inverter is needed: Input 5V DC -> Output 100V AC

EL inverter

IMUBNO055 This sensor will help us to know the position of our robot and if it is about to fall to one side. For this we use: the Absolute Orientation (Euler Vector, 100Hz) Three axis orientation data based on a 360° sphere and the Angular Velocity Vector (100Hz) Three axis of 'rotation speed' in rad/s.

BNO055 breakout board

XMC4700RelaxKit the main controller and therefore the brain of our robot. It's featuring the Arduino Uno pinout and is enabled to be used in the Arduino IDE:

XMC 4700 Relax Kit lite

3) Motor control

You can run the motors in four modes: Microstep (1/8), Interleave, Single and Double

Microstep (1/8) performs a smooth handover from one coil of the sepper to the other.This is the smoothest but also the slowest mode:

Microstepping

Interleave is only introducing a half step (1:2) during the handover from one coil to the other. This mode is faster than microstepping but not that smooth.

Double and Singlestep(1:1) are fulfilling a full step. While "single" mode is using only one coil as active one, "double" is using two active coils and has therefore more torque. These are the fastest modes but are also a bit rough at low speed.

The different speed ranges can be better seen in this graph:

To make full use of the stepper's capabilities in the different modes we use all of them by handing over from one to another:

Unfortunately this is a nightmare for any following control algorithm as for a different change of the control signal (x-axis) you see a different increase of speed value (y-axis).Thats why we linearized the motor control to one get a linear relation of control signal to speed increase:

In Arduino we could use this mapping function to do so:

if(Regelausgang_Lage >= -160 && Regelausgang_Lage <= 160){
        Schrittart=1; // Microschritt (beide Richtungen)
        Speed = map (Regelausgang_Lage, -160,160, -400,400);
      }
      else if (Regelausgang_Lage >=161 && Regelausgang_Lage <= 650)  {
        Schrittart=2; //Interleave (positiv)
        Speed = map (Regelausgang_Lage, 161,650, 90,400);
      }
        else if ( Regelausgang_Lage >= -650 && Regelausgang_Lage <=-161) {
        Schrittart=2; //Interleave (negativ)
        Speed = map (Regelausgang_Lage, -161,-650, -90,-400); 
       }
      if (Regelausgang_Lage >=651)  {
        Schrittart=3; //Double  (positiv)
        Speed = map (Regelausgang_Lage, 651,1300, 200,400);
       }                    
       if ( Regelausgang_Lage <= -651) {
        Schrittart=3; //Double (negativ)
        Speed = map (Regelausgang_Lage, -651,-1300, -200,-400); 
       }                    
    }

4) Control algorithms

A cascade control is used to control the system. This means we use four subordinate control loops. This topology offers the advantage of simplifying a complex system using several subsystems. Each subsystem is regulated with a specific controller. Disturbances that act on an internal subsystem are adjusted before they can influence the external control loops. Since each subsystem requires a certain amount of time to implement manipulated variables, the higher-level control systems must be slower by a factor of 4. The system can be divided into four subsystems:

Lageregelung adjusts the angle of the robot and prevents the robot from falling over. We use a PID controller manipulating the wheelspeed for this.
Geschwindigkeitsregelung controls the speed of the robot. We use another PID controller manipulating the angle of the robot for this.
Positionsregelung controls the position of the robot in relation to its starting point. We use a P controller to manipulate the speed of the robot.
Orientierungsregelung controls the direction in which the robot moves. We use another P controller for this which acts on the wheelspeeds.

As it''s time critical to run these control algorithms they are triggered by timer interrupts

// Interrupt Setup
//------------------------------------------------------------------------------------------------
// Setup Postition Interrupt
//------------------------------------------------------------------------------------------------
  XMC_CCU4_SLICE_COMPARE_CONFIG_t pwm_config = {0};
            pwm_config.passive_level = XMC_CCU4_SLICE_OUTPUT_PASSIVE_LEVEL_HIGH;
            pwm_config.prescaler_initval = XMC_CCU4_SLICE_PRESCALER_32768;
            XMC_CCU4_Init(CCU41, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU41_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU41, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU41_CC43, 704); // Adjust last Value or Prescaler
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU41_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU41_0_IRQn); 
        XMC_CCU4_EnableShadowTransfer(CCU41, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        XMC_CCU4_SLICE_StartTimer(CCU41_CC43);

//------------------------------------------------------------------------------------------------
// Setup Geschwindigkeit Interrupt
//------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU42, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU42_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU42, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU42_CC43, 176); // Adjust last Value or Prescaler 176->25Hz 220->20Hz
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU42_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU42_0_IRQn); 
        XMC_CCU4_EnableShadowTransfer(CCU42, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        XMC_CCU4_SLICE_StartTimer(CCU42_CC43);
        
//------------------------------------------------------------------------------------------------
// Setup Sensor Interrupt
//------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU43, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);
            XMC_CCU4_SLICE_CompareInit(CCU43_CC43, &pwm_config);
            XMC_CCU4_EnableClock(CCU43, 3);
            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU43_CC43, 43); // Adjust last Value or Prescaler 
 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);
/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  /* Configure NVIC */
  /* Set priority */
  NVIC_SetPriority(CCU43_0_IRQn, 10U);
  /* Enable IRQ */
  NVIC_EnableIRQ(CCU43_0_IRQn); 
       XMC_CCU4_EnableShadowTransfer(CCU43, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
       XMC_CCU4_SLICE_StartTimer(CCU43_CC43

4.1 Position control - " Lageregelung"

Remember - we adjust the angle of the robot and prevent the robot from falling over. In order to set a certain angle, the motor axis is moved in such a way that it is either below the center of gravity (robot is standing) or a moment equilibrium is established from accelerating moments (robot moves). The position control loop is called via a timer interrupt with 100 Hz. It can be described as following:

PID controller:

P - the proportional share is multiplying our angle offset with a constant factor "Kp"You can imagine it as a lenght of an lever pushing against the offset

I - the integral part term "Ki" increases action in relation not only to the error but also the time for which it has persisted. We simply add the current offset to an "integrator" value.

D- the derivative term "Kd" does not consider the error, but the rate of change of error. Luckily our sensor is already measuring this by measuring the Angular Velocity Vector. The D part is acting like a damper to the system.

//------------------------------------------------------------------------------------------------
// PID Lageregler 
//------------------------------------------------------------------------------------------------
    Winkelabweichung  = (double(euler.z())-sollWinkel);  
    integrator_Lage   = constrain (integrator_Lage + Winkelabweichung,-integrator_LageMAX, integrator_LageMAX); // I Anteil der Lageregelung wird begrenzt
    Regelausgang_Lage = constrain (KP_Lage*Winkelabweichung\
                                 + KI_Lage * integrator_Lage\
                                 + KD_Lage*double(gyroscope.x()),-1300, 1300);                                  // Begrenzung auf Schrittmotorkennlinie

4.1.1 Tuning the position controller

We tune the controller by adjusting the parameters Kp, Ki, Kd of the PID control algorithm. The method used is called "Ziegler–Nichols tuning method".Keep Ki and Kd at 0 while increasing Kp until the system reached the limit of stability and thus gets in a harmonic oscillation. The value found in out case this way is referred to as Kp_krit=200 and showed a period of 300ms.

harminic oscillation

Use the following table to calculate Kd = Kp*Tv and Ki = Kp/Tn

Ziegler–Nichols table

Note: Ki would be 800 in our case but as we add 100 times per second we bring it down to 8.

4.2 Speed control - " Geschwindigkeitsregelung "

The speed controller controls the speed of the robot. We use another PID controller manipulating the angle of the robot for this. The position control loop is called via a timer interrupt with 25Hz. It can be described as following:

Speed control

The wheelspeed of the robot is given by our "Lageregelung" control loop. We additionaly smooth this value with a low pass filter.Then we also calculate the acceleration by taking the actual and last speed value as well as the time difference between measuring them of 40ms (25Hz).

PID controller:

PID speed control

void get_sollsollWinkel() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Tiefpassfilter
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
      Regelausgang_Lage_5 = Regelausgang_Lage_4;
      Regelausgang_Lage_4 = Regelausgang_Lage_3;
      Regelausgang_Lage_3 = Regelausgang_Lage_2;
      Regelausgang_Lage_2 = Regelausgang_Lage_1;
      Regelausgang_Lage_1 = Regelausgang_Lage;

      Regelausgang_Lage_filter = (Regelausgang_Lage_1 \
                                + Regelausgang_Lage_2 \
                                + Regelausgang_Lage_3 \
                                + Regelausgang_Lage_4 \
                                + Regelausgang_Lage_5)/5;   
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Bilden der Geschwindigkeitsableitung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
    
      d_Geschwindigkeit = (Regelausgang_Lage_filter - Regelausgang_Lage_filter_alt)\
                          /Ta_Geschwindigkeitsregler;
      Regelausgang_Lage_filter_alt = Regelausgang_Lage_filter;
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Geschwindigkeitsregler -> Ausgang sollWinkel
//--------------------------------------------------------------------------------------------------------------------------------------------------------------

     Geschwindigkeitsabweichung = sollGeschwindigkeit - Regelausgang_Lage_filter;

     if (abs(Positionsabweichung) <= 15 ) {  //Regelparameter bei geringer Positionsabweichung
           KP_geschw = 0.0045;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0009;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.000;                // Differentianteil des Geschwindigkeitsreglers  
     }     
     else{                                   //Regelparameter bei größerer Positionsabweichung
           KP_geschw = 0.0090;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0007;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.0002;               // Differentianteil des Geschwindigkeitsreglers  
     }
           
           integrator_geschw = constrain(integrator_geschw + Geschwindigkeitsabweichung,\
                               -sollWinkelMAX/KI_geschw, sollWinkelMAX/KI_geschw);   //I Anteil wird begrenzt
           sollWinkel        = constrain(KP_geschw * Geschwindigkeitsabweichung \   
                                       + KI_geschw * integrator_geschw \                           
                                       + KD_geschw * d_Geschwindigkeit,\                                  
                                       -sollWinkelMAX, sollWinkelMAX);               //Winkelbegrenzung
  }

4.2.1 Tuning the speed controller

Tuning of the speed controller is the same as for the position controller.With PID we still see oscillations for low speed offsets. Switching to an PI controller for low speed offsets solved this issue. In this we decreased Kp and increased Ki a bit. This is a bit of trying out ;)

4.3 Position control - " Positionsregelung"

This controller controls the position of the robot in regards to its initial position.The position is known by counting rotation steps of the steppers. A simple P controller acting on the speed of the robot is suficcient for this:

We tried different values for Kp. At Kp=4 the best controller performance was shown.

void get_sollGeschwindigkeit() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Positionsregler -> Ausgang sollGeschwindigkeit
//--------------------------------------------------------------------------------------------------------------------------------------------------------------      
 Positionsabweichung  = (sollPosition - pos_aktuell);
 
   //+++Ausweichen+++    
       if (Distanz_X1 <= 15 && Distanz_X1 > 0) {      //Einschaltschwelle "Hand folgen"
        KP_dist_X1 = 75;
        KP_pos = 0;
        EL_threshold = 1;                             //EL-Wire leuchtet
       }                                              
   //+++Positionsregler+++                            
       else if (Distanz_X1 > 30 || Distanz_X1 == 0) { //Ausschaltschwelle "Hand folgen"
         KP_dist_X1 = 0;
         KP_pos = 4;
         EL_threshold = constrain(abs(Positionsabweichung),0,300)* -0.15 + 50;
                                                     //EL-Wire blinkt(2-10Hz)              
       }
       else {/*Hysterese*/}

        sollGeschwindigkeit  =  constrain (KP_pos*Positionsabweichung  \
                                         + KP_dist_X1*(-15+Distanz_X1),\
                                -GeschwindigkeitMAX, GeschwindigkeitMAX);      

}

Schematics

Code

//Interrupt Funktionen
extern "C" {
  #include "ExternInterrupt.h"
}


#include <AccelStepper.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

#include <Wire.h>

#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Initialisieren des Sensors
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
Adafruit_BNO055 bno = Adafruit_BNO055();

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Initialisieren der Schrittmotoren
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
int Schrittart = 0;

Adafruit_MotorShield AFMS(0x60); // Default address, no jumpers

// Connect two steppers with 200 steps per revolution (1.8 degree)
Adafruit_StepperMotor *myStepper1 = AFMS.getStepper(200, 1);
Adafruit_StepperMotor *myStepper2 = AFMS.getStepper(200, 2);


// you can change these to DOUBLE or INTERLEAVE or MICROSTEP!
// wrappers for the first motor!
void forwardstep1() {  
  if (Schrittart == 1) {
    myStepper1->onestep(FORWARD,MICROSTEP );
  }
  else if(Schrittart == 2) {
    myStepper1->onestep(FORWARD, INTERLEAVE);
  } 
  else {
    myStepper1->onestep(FORWARD, DOUBLE);
  } 
}

void backwardstep1() {  
 if (Schrittart == 1) {
    myStepper1->onestep(BACKWARD,MICROSTEP );
  }
  else if(Schrittart == 2) {
    myStepper1->onestep(BACKWARD, INTERLEAVE);
  } 
  else {
    myStepper1->onestep(BACKWARD, DOUBLE);
  } 
}
// wrappers for the second motor!
void forwardstep2() {  
  if (Schrittart == 1) {
    myStepper2->onestep(BACKWARD,MICROSTEP );
  }
  else if(Schrittart == 2) {
    myStepper2->onestep(BACKWARD, INTERLEAVE);
  } 
  else {
    myStepper2->onestep(BACKWARD, DOUBLE);
  } 
}
void backwardstep2() {  
   if (Schrittart == 1) {
    myStepper2->onestep(FORWARD,MICROSTEP );
  }
  else if(Schrittart == 2) {
    myStepper2->onestep(FORWARD, INTERLEAVE);
  } 
  else {
    myStepper2->onestep(FORWARD, DOUBLE);
  } 
}

// Now we'll wrap the 2 steppers in an AccelStepper object
AccelStepper stepper1(forwardstep1, backwardstep1);
AccelStepper stepper2(forwardstep2, backwardstep2);



//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter fr die Lageregelung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
const int KP_Lage = 120;                 // Proportionalanteil des PID Reglers
const int KI_Lage = 8;                   // Integralanteil des PID Reglers
const int KD_Lage = 5;                   // Differentialanteil des PID Reglers
float sollWinkel = 4.25;                    // Sollwinkel
float integrator_LageMAX = 1300/KI_Lage; // Maximalwert fr den I Anteil des PID Reglers
float integrator_Lage = 0;               // Anfangswert der Integration
float Winkelabweichung;                  // aktuelle Winkelabweichung

int Regelausgang_Lage;                   // Regelausgang der Lageregelung
int Speed;                               // Geschwindigkeit der Motoren 

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter  fr die Geschwindigeitsregler
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
int sollGeschwindigkeit = 0;             // Sollgeschwindigkeit
float Geschwindigkeitsabweichung;        // aktuelle Geschwindigkeitsabweichung
const int GeschwindigkeitMAX = 600;      // Maximale Sollgeschwindigkeit

int Regelausgang_Lage_5 = 0;             // Messwert 5 des Tiefpassfilters
int Regelausgang_Lage_4 = 0;             // Messwert 4 des Tiefpassfilters
int Regelausgang_Lage_3 = 0;             // Messwert 3 des Tiefpassfilters
int Regelausgang_Lage_2 = 0;             // Messwert 2 des Tiefpassfilters
int Regelausgang_Lage_1 = 0;             // Messwert 1 des Tiefpassfilters
int Regelausgang_Lage_filter = 0;        // aktuelle Istgeschwindigkeit (nach Tiefpass)
int Regelausgang_Lage_filter_alt = 0;    // vorherige Istgeschwindigkeit (nach Tiefpass)
  
int d_Geschwindigkeit;                   // aktuelle Beschleunigung
float Ta_Geschwindigkeitsregler = 0.040; // Abtastzeit der Geschwindigkeitsregelung 

float KP_geschw;                         // Proportionalanteil des Geschwindigeitsreglers
float KI_geschw;                         // Integralanteil des Geschwindigeitsreglers
float KD_geschw;                         // Differentialanteil des Geschwindigeitsreglers

float integrator_geschw = 0;             // Anfangswert der Integration Geschwindigkeit

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter  fr die Positionsregelung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
int sollPosition = 0;                  // Sollposition
int sollPosition_max = 200;            // maximale Sollposition
int Positionsabweichung;               // aktuelle Positionsabweichung 
int Positionsaenderung = 0;            // Positionsnderung
const int sollWinkelMAX = 6;           // Maximaler Sollwinkel

int KP_pos = 4;                        // Proportionalanteil des P Reglers
int KP_dist_X1 = 0;                    // Proportionalanteil des P Reglers 
                                       // bei "Hand folgen"

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter  fr die Orientierungsregelung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
float sollOrientierung;                  // sollOrientierung
float Orientierungsabweichung;           // aktuelle Orientierungsabweichung
float Regelausgang_Orientierung;         // Regelausgang der Orientierungsregelung
float KP_orient = 0.1;                   // Proportionalanteil des P Reglers

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter  fr die Positionsberwachung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
float pos_aktuell = 0;                  // aktuelle Position
float pos_microstep = 0;                // aktuelle nderung "microstep"
float pos_microstep_done = 0;           // Depot "microstep" 
float pos_interleave= 0;                // aktuelle nderung "interleave"Seria
float pos_interleave_done = 0;          // Depot "interleave"
float pos_double = 0;                   // aktuelle nderung "double"
float pos_double_done = 0;              // Depot "double"
int Schrittart_alt = 0;                 // bisherige Schrittart

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Parameter fr die Serielle Kommunikation
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
String inString = "";                    // Input String fr Serial

//X1
String inStringX1 = "";                  // Input String fr Serial3
int neg;                                 // negative Zahl?
int Distanz_X1 = 0;                      // Distanz von Ultraschallsensor an X1
int v_X1 = 0;                            // Geschwindigkeit von Ultraschallsensor an X1

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Blinken der El-Schnur
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
const int EL_wire = 5;                   // MOSFET an GPIO 5
int EL_threshold = 100;                  // Leuchten(1) oder Blinken(>1)
int EL_wire_ON_OFF = 0;                  // Zustand der EL-Schnur
int n = 0;                               // Betimmt die Frequenz des Blinkes
  
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Eingabe sollPosition_max
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
const int buttonPin = 26;                // BUTTON1
int buttonState = 0;                     // Status des Tasters

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// SETUP
//--------------------------------------------------------------------------------------------------------------------------------------------------------------

void setup () {
 
   pinMode(EL_wire, OUTPUT);
   pinMode(7, OUTPUT);
   
   pinMode(LED_BUILTIN, OUTPUT);
   pinMode(buttonPin, INPUT);

  
// Serial Communication   
   Serial.begin (115200);
//   Serial1.begin (115200);                      // Kommunikation zu Wlan-Shield
//   Serial2.begin (115200);                    // Kommunikation zu Radar Sensor an X2
//   Serial3.begin (9600);                      // Kommunikation zu Ultraschall Sensor an X1

 Serial.print("start Setup:");

//Sensor
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }
  delay(1000);
   bno.setExtCrystalUse(true);

// Stepper
   AFMS.begin(); // Start the shield
   //(void)XMC_I2C_CH_SetBaudrate(XMC_I2C1_CH1, 400000u); // Change the i2c clock to 400KHz
  (void)XMC_I2C_CH_SetBaudrate(XMC_I2C1_CH1, 200000u); // Change the i2c clock to 400KHz
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Interrupt Setup
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Setup Postition Interrupt
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
  XMC_CCU4_SLICE_COMPARE_CONFIG_t pwm_config = {0};
            pwm_config.passive_level = XMC_CCU4_SLICE_OUTPUT_PASSIVE_LEVEL_HIGH;
            pwm_config.prescaler_initval = XMC_CCU4_SLICE_PRESCALER_32768;

            XMC_CCU4_Init(CCU41, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);

            XMC_CCU4_SLICE_CompareInit(CCU41_CC43, &pwm_config);

            XMC_CCU4_EnableClock(CCU41, 3);

            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU41_CC43, 704); // Adjust last Value or Prescaler

 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);

/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU41_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  
  /* Configure NVIC */

  /* Set priority */
  NVIC_SetPriority(CCU41_0_IRQn, 10U);

  /* Enable IRQ */
  NVIC_EnableIRQ(CCU41_0_IRQn); 
     
        XMC_CCU4_EnableShadowTransfer(CCU41, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        
        XMC_CCU4_SLICE_StartTimer(CCU41_CC43);

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Setup Geschwindigkeit Interrupt
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU42, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);

            XMC_CCU4_SLICE_CompareInit(CCU42_CC43, &pwm_config);

            XMC_CCU4_EnableClock(CCU42, 3);

            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU42_CC43, 176); // Adjust last Value or Prescaler 176->25Hz 220->20Hz

 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);

/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU42_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  
  /* Configure NVIC */

  /* Set priority */
  NVIC_SetPriority(CCU42_0_IRQn, 10U);

  /* Enable IRQ */
  NVIC_EnableIRQ(CCU42_0_IRQn); 
     
        XMC_CCU4_EnableShadowTransfer(CCU42, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        
        XMC_CCU4_SLICE_StartTimer(CCU42_CC43);
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Setup Sensor Interrupt
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
XMC_CCU4_Init(CCU43, XMC_CCU4_SLICE_MCMS_ACTION_TRANSFER_PR_CR);

            XMC_CCU4_SLICE_CompareInit(CCU43_CC43, &pwm_config);

            XMC_CCU4_EnableClock(CCU43, 3);

            XMC_CCU4_SLICE_SetTimerPeriodMatch(CCU43_CC43, 43); // Adjust last Value or Prescaler 44

 /* Enable compare match and period match events */
  XMC_CCU4_SLICE_EnableEvent(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH);

/* Connect period match event to SR0 */
  XMC_CCU4_SLICE_SetInterruptNode(CCU43_CC43, XMC_CCU4_SLICE_IRQ_ID_PERIOD_MATCH, XMC_CCU4_SLICE_SR_ID_0);
  
  /* Configure NVIC */

  /* Set priority */
  NVIC_SetPriority(CCU43_0_IRQn, 10U);

  /* Enable IRQ */
  NVIC_EnableIRQ(CCU43_0_IRQn); 
     
        XMC_CCU4_EnableShadowTransfer(CCU43, (CCU4_GCSS_S0SE_Msk << (4 * 3)));
        
        XMC_CCU4_SLICE_StartTimer(CCU43_CC43);

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Orientierung zu beginn
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  sollOrientierung = double(euler.x());

Serial.println("setup done");
}


 
void getSpeed() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Die Sensordaten werden ausgelesen 
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
//delayMicroseconds(5);  
   imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
   imu::Vector<3> gyroscope = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

  if(double(euler.z())==0 && millis()>5000){
    digitalWrite(7,HIGH);
    Serial.println("fail");
  }
  
//delayMicroseconds(5);
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Roboter Umgefallen?
//-------------------------------------------------------------------------------------------------------------------------------------------------------------- 
  if(abs(euler.z())>40){ 
    Regelausgang_Lage = 0;
    integrator_Lage = 0;
    integrator_geschw = 0;
    Positionsabweichung = 0;
    pos_aktuell = 0;
    pos_microstep = 0;
    pos_microstep_done = 0;
    pos_interleave= 0;
    pos_interleave_done = 0;
    pos_double = 0;
    pos_double_done = 0;
    Schrittart_alt = 0;
    sollOrientierung = double(euler.x());
     Serial.println("Roboter umgefallen");
     delay(1000);
  }
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Orientierungsregler 
//-------------------------------------------------------------------------------------------------------------------------------------------------------------- 
  else {
    Orientierungsabweichung = (double(euler.x())-sollOrientierung);
    if (Orientierungsabweichung > 180){
      Orientierungsabweichung -=360;
    }
    Regelausgang_Orientierung = constrain(1-abs(Orientierungsabweichung)*KP_orient,0.5,1);

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// PID Lageregler 
//--------------------------------------------------------------------------------------------------------------------------------------------------------------    
    Winkelabweichung  = (double(euler.z())-sollWinkel);  
    integrator_Lage   = constrain (integrator_Lage + Winkelabweichung,-integrator_LageMAX, integrator_LageMAX); // I Anteil der Lageregelung wird begrenzt
    Regelausgang_Lage = constrain (KP_Lage*Winkelabweichung\
                                 + KI_Lage * integrator_Lage\
                                 + KD_Lage*double(gyroscope.x()),-1300, 1300);                                  // Begrenzung auf Schrittmotorkennlinie
   
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Gliederung Regelausgang_Lage in drei Bereiche: 0-160(micro=1) / 161-650(interleave=1) / 651-1300(double=3) und mapping auf Speed-Werte
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
      if(Regelausgang_Lage >= -160 && Regelausgang_Lage <= 160){
        Schrittart=1; // Microschritt (beide Richtungen)
        Speed = map (Regelausgang_Lage, -160,160, -400,400);
      }
      else if (Regelausgang_Lage >=161 && Regelausgang_Lage <= 650)  {
        Schrittart=2; //Interleave (positiv)
        Speed = map (Regelausgang_Lage, 161,650, 90,400);
      }
        else if ( Regelausgang_Lage >= -650 && Regelausgang_Lage <=-161) {
        Schrittart=2; //Interleave (negativ)
        Speed = map (Regelausgang_Lage, -161,-650, -90,-400); 
       }
      if (Regelausgang_Lage >=651)  {
        Schrittart=3; //Double  (positiv)
        Speed = map (Regelausgang_Lage, 651,1300, 200,400);
       }                    
       if ( Regelausgang_Lage <= -651) {
        Schrittart=3; //Double (negativ)
        Speed = map (Regelausgang_Lage, -651,-1300, -200,-400); 
       }                    
    }  
      
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// EL-Wire
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
     if ( EL_threshold > 1) {                         //Blinken
       n++;                                           //Counter 
       if (n/EL_threshold ) {                         //Umschaltschwelle erreicht?
        EL_wire_ON_OFF = abs(EL_wire_ON_OFF)-1;       //Zustand wechseln
        digitalWrite(EL_wire, abs(EL_wire_ON_OFF));   //Zustand schalten
        n = 0;                                        //Counter zurcksetzten
       }   
     }
     else {                                           //Leuchten
      digitalWrite(EL_wire, HIGH);                    //Zustand schalten
     }
}

void get_sollsollWinkel() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Tiefpassfilter
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
      Regelausgang_Lage_5 = Regelausgang_Lage_4;
      Regelausgang_Lage_4 = Regelausgang_Lage_3;
      Regelausgang_Lage_3 = Regelausgang_Lage_2;
      Regelausgang_Lage_2 = Regelausgang_Lage_1;
      Regelausgang_Lage_1 = Regelausgang_Lage;

      Regelausgang_Lage_filter = (Regelausgang_Lage_1 \
                                + Regelausgang_Lage_2 \
                                + Regelausgang_Lage_3 \
                                + Regelausgang_Lage_4 \
                                + Regelausgang_Lage_5)/5;   
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Bilden der Geschwindigkeitsableitung
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
    
      d_Geschwindigkeit = (Regelausgang_Lage_filter - Regelausgang_Lage_filter_alt)\
                          /Ta_Geschwindigkeitsregler;
      Regelausgang_Lage_filter_alt = Regelausgang_Lage_filter;
        
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Geschwindigkeitsregler -> Ausgang sollWinkel
//--------------------------------------------------------------------------------------------------------------------------------------------------------------

     Geschwindigkeitsabweichung = sollGeschwindigkeit - Regelausgang_Lage_filter;

     if (abs(Positionsabweichung) <= 15 ) {  //Regelparameter bei geringer Positionsabweichung
           KP_geschw = 0.0045;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0009;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.000;                // Differentianteil des Geschwindigkeitsreglers  
     }     
     else{                                   //Regelparameter bei grerer Positionsabweichung
           KP_geschw = 0.0090;               // Proportionalanteil des Geschwindigkeitsreglers
           KI_geschw = 0.0007;               // Integralanteil des Geschwindigkeitsreglers  
           KD_geschw = 0.0002;               // Differentianteil des Geschwindigkeitsreglers  
     }
           
           integrator_geschw = constrain(integrator_geschw + Geschwindigkeitsabweichung,\
                               -sollWinkelMAX/KI_geschw, sollWinkelMAX/KI_geschw);   //I Anteil wird begrenzt
           sollWinkel        = constrain(KP_geschw * Geschwindigkeitsabweichung \   
                                       + KI_geschw * integrator_geschw \                           
                                       + KD_geschw * d_Geschwindigkeit,\                                  
                                       -sollWinkelMAX, sollWinkelMAX);               //Winkelbegrenzung
  }         

void get_sollGeschwindigkeit() {
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Positionsregler -> Ausgang sollGeschwindigkeit
//--------------------------------------------------------------------------------------------------------------------------------------------------------------      
 Positionsabweichung  = (sollPosition - pos_aktuell);
 
   //+++Ausweichen+++    
       if (Distanz_X1 <= 15 && Distanz_X1 > 0) {      //Einschaltschwelle "Hand folgen"
        KP_dist_X1 = 75;
        KP_pos = 0;
        EL_threshold = 1;                             //EL-Wire leuchtet
       }                                              
   //+++Positionsregler+++                            
       else if (Distanz_X1 > 30 || Distanz_X1 == 0) { //Ausschaltschwelle "Hand folgen"
         KP_dist_X1 = 0;
         KP_pos = 4;
         EL_threshold = constrain(abs(Positionsabweichung),0,300)* -0.15 + 50;
                                                     //EL-Wire blinkt(2-10Hz)              
       }
       else {/*Hysterese*/}

        sollGeschwindigkeit  =  constrain (KP_pos*Positionsabweichung  \
                                         + KP_dist_X1*(-15+Distanz_X1),\
                                -GeschwindigkeitMAX, GeschwindigkeitMAX);      

}


void loop () {
////--------------------------------------------------------------------------------------------------------------------------------------------------------------
//// Eingabe Sensor X1 (Ultraschall)
////--------------------------------------------------------------------------------------------------------------------------------------------------------------
//while (Serial3.available() > 0) { 
//    int inCharX1 = Serial3.read();
//      if (inCharX1 == '-') {
//         neg=1;
//      }
//      if (isDigit(inCharX1)) {
//        inStringX1 += (char)inCharX1;
//      }
//      if (inCharX1 == ',') {
//        Distanz_X1 = inStringX1.toInt();
//        inStringX1 = "";
//      }
//      if (inCharX1 == '\n') {
//        v_X1 = inStringX1.toInt();
//        if (neg){
//          v_X1 = -v_X1;
//        }
//        inStringX1 = "";
//        neg=0;
//      } 
//  }
    
////--------------------------------------------------------------------------------------------------------------------------------------------------------------
//// Eingabe Sensor X2 (Radar)
////--------------------------------------------------------------------------------------------------------------------------------------------------------------
//  while(Serial2.available() >0){
//  String velocity = Serial2.readStringUntil('\n');
//  int velocity_int = *velocity.c_str();
//      if(velocity_int > 128) {
//        velocity_int -= 256;
//      }
//         if (velocity_int > 50){
//          sollPosition = sollPosition_max;
//         }
//         if (velocity_int < -50){
//          sollPosition = 0;
//         }
//  }

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Eingabe sollPosition_max
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
   buttonState = digitalRead(buttonPin); 
     if (buttonState == LOW) {
         digitalWrite(LED_BUILTIN, HIGH);
         sollPosition_max = pos_aktuell;
     }
     else{
         digitalWrite(LED_BUILTIN, LOW);
     }

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Eingabe Serial
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
  while (Serial.available() > 0) { 
    int inChar = Serial.read();

    if (inChar == '-') {
       neg=1;
    }
    
    if (isDigit(inChar)) {
      // convert the incoming byte to a char
      // and add it to the string:
      inString += (char)inChar;
    }
    // if you get a newline, print the string,
    // then the string's value:
    if (inChar == '\n') {
      Positionsaenderung = inString.toInt();
      if (neg){
        Positionsaenderung = -Positionsaenderung;
      }
      inString = "";
      neg=0;
    }
  }

if (Positionsaenderung != 0){
  sollPosition += Positionsaenderung;
  Positionsaenderung = 0;
  delay(1);
}

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Interruptgesteuerte Regler
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
  if (enableSensor) {
        getSpeed();
        enableSensor=false;
  }
  if (enableGeschwindigkeit) {
        get_sollsollWinkel();  
        enableGeschwindigkeit=false; 
  }
  if (enablePosition) {
        get_sollGeschwindigkeit();
        enablePosition=false;
       
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Ausgabe WiFi
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
//             Serial1.print("t:");
//             Serial1.print(millis());                      Serial1.print("  ");
             Serial.print("Istwinkel:");            
             Serial.println(-Winkelabweichung+sollWinkel);  //Serial.print("  ");
//             Serial1.print("Istposition:");             
//             Serial1.print(pos_aktuell);                   Serial1.print("  "); 
//             Serial1.print("Istgeschwindigkeit:");             
//             Serial1.println(Regelausgang_Lage_filter);    Serial1.print("  ");                   
  }
        Serial.print("loop:");
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Die Motoren werden angesteuert.
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
  if (Orientierungsabweichung * Speed > 0) {   
     stepper1.setSpeed(Speed*Regelausgang_Orientierung);
     stepper2.setSpeed(Speed);
  }
  else if (Orientierungsabweichung * Speed < 0) {   
     stepper1.setSpeed(Speed);
     stepper2.setSpeed(Speed*Regelausgang_Orientierung);
  }
  else {
     stepper1.setSpeed(Speed);
     stepper2.setSpeed(Speed);    
  }
      if (Regelausgang_Lage == 0) { 
     myStepper1->release();;
     myStepper2->release();;
      }
      else {
           stepper1.runSpeed();
           stepper2.runSpeed(); 
      }

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// Feststellen der Istposition
//--------------------------------------------------------------------------------------------------------------------------------------------------------------
    if (Schrittart!=Schrittart_alt) {                           //Wechsel der Schrittart
      pos_microstep_done = pos_microstep_done+pos_microstep;    //Addieren zum Depot microstep
      pos_interleave_done = pos_interleave_done+pos_interleave; //Addieren zum Depot interleave
      pos_double_done = pos_double_done+pos_double;             //Addieren zum Depot double
      stepper1.setCurrentPosition(0);                           //Zurcksetzten des internen Zhlers
      pos_microstep = 0;                                        //Clear Zhler microstep
      pos_interleave = 0;                                       //Clear Zhler interleave
      pos_double = 0;                                           //Clear Zhler double
      Schrittart_alt=Schrittart;
    } 
    switch (Schrittart){
        case 1:
          pos_microstep = stepper1.currentPosition()/8;         //i_Schrittart = 1/8
          break;
        case 2:
          pos_interleave = stepper1.currentPosition()/2;        //i_Schrittart = 1/2
          break;  
        case 3:
          pos_double = stepper1.currentPosition();  ;           //i_Schrittart = 1
          break;
    }
      pos_aktuell = pos_microstep_done +  pos_microstep  \
                  + pos_interleave_done + pos_interleave \
                  + pos_double_done   +   pos_double;
       

}

 int enableSensor;          /* Declaration of the variable */
 int enableGeschwindigkeit; /* Declaration of the variable */
 int enablePosition;        /* Declaration of the variable */

void CCU41_0_IRQHandler(void) // Interrupt Positionsregelung
  {
             enablePosition = true; 
             Serial.println("position");
  }
  
void CCU42_0_IRQHandler(void) // Interrupt Geschwindigkeitsregelung
  {
             enableGeschwindigkeit = true; 
             Serial.println("Geschwindigkeit");
  }
    
void CCU43_0_IRQHandler(void) // Interrupt Lageregelung und Orientierungsregelung
  {
             enableSensor = true;
             Serial.println("orientierung");
  }

Self-Balancing Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

1) Mechanics

2) Electronics

3) Motor control

4) Control algorithms

4.1 Position control - " Lageregelung"

4.1.1 Tuning the position controller

4.2 Speed control - " Geschwindigkeitsregelung "

4.2.1 Tuning the speed controller

4.3 Position control - " Positionsregelung"

Custom parts and enclosures

Top Plate XMC4700 electric wire

Top Plate XMC4700

Chassis Nema 17

Chassis Nema 17 electric wire

Wheel Adapter

Schematics

Schematics

Code

Main code

Interrupt Handler

Credits

Infineon Team

Comments

Embed the widget on your own site

Self-Balancing Robot

Self-Balancing Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

1) Mechanics

2) Electronics

3) Motor control

4) Control algorithms

4.1 Position control - " Lageregelung"

4.1.1 Tuning the position controller

4.2 Speed control - " Geschwindigkeitsregelung "

4.2.1 Tuning the speed controller

4.3 Position control - " Positionsregelung"

Custom parts and enclosures

Top Plate XMC4700 electric wire

Top Plate XMC4700

Chassis Nema 17

Chassis Nema 17 electric wire

Wheel Adapter

Schematics

Schematics

Code

Main code

Interrupt Handler

Credits

Infineon Team

Comments

Related channels and tags