# The control algorithm of the balance car (PID, LQR, MPC) and the arduino program navigation sticker

Small experiment for the position test of the motor of the balance trolley

1. Encoder pulse count

PID control algorithm

Balance car PID parameter adjustment experiment

position ring

2. Encoder count conversion angle

Dynamic modeling of the car as a whole

Judging the dynamic characteristics of the system by eigenvalues

Lomborg Observer

## Small experiment for the position test of the motor of the balance trolley

### 1. Encoder pulse count

```const byte LeftMotorInterruptP = 22; //Left motor encoder interrupt pin
const byte LeftMotorCountP = 23; //Left motor encoder count pin
const byte RightMotorInterruptP = 18; //Right motor encoder interrupt pin
const byte RightMotorCountP = 19; //Right motor encoder count pin

const byte MotorDriverEn = 5; // motor driver enable
const byte LeftMotorP1 = 15; //Left motor control io port 1
const byte LeftMotorP2 = 13; //Left motor control io port 2
const byte RightMotorP1 = 16; //Right motor control io port 1
const byte RightMotorP2 = 17; //Right motor control io port 2

volatile long LeftMotorCounter = 0; //left motor interrupt count position
volatile long RightMotorCounter = 0; //Right motor interrupt count position

portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; //Declare a variable of type portMUX_TYPE, which is used to process the synchronization between the main code and the interrupt
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;

//left motor interrupt function
void LeftMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux_1);
delayMicroseconds(10); //Delay 20ms as debounce, if it is a very stable interrupt, you can add no or very little debounce time
if(digitalRead(LeftMotorInterruptP) == LOW) //Because it is a pull-up trigger, read the high and low levels of the pin after the debounce time, if it is still low, it means a stable interrupt has occurred
{
LeftMotorCounter++;
else
LeftMotorCounter--;
Serial.print("Left motor position:");Serial.println(LeftMotorCounter);
}
portEXIT_CRITICAL_ISR(&mux_1);
}
//Right motor interrupt function
void RightMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux);
delayMicroseconds(10); //Delay 2ms as debounce, if it is a very stable interrupt, you can add no or very little debounce time
if(digitalRead(RightMotorInterruptP) == LOW) //Because it is a pull-down trigger, read the high and low levels of the pin after the debounce time, if it is still low, it means a stable interrupt has occurred
{
RightMotorCounter--;
else
RightMotorCounter++;
Serial.print("Right motor position:");Serial.println(RightMotorCounter);
}
portEXIT_CRITICAL_ISR(&mux);
}

void setup(){
Serial.begin(115200);
Serial.println("Interrupt Test Experiment");

//off motor enable
pinMode(MotorDriverEn,OUTPUT);
digitalWrite(MotorDriverEn,LOW);

pinMode(LeftMotorP1,OUTPUT);
pinMode(LeftMotorP2,OUTPUT);
pinMode(RightMotorP1,OUTPUT);
pinMode(RightMotorP2,OUTPUT);
digitalWrite(LeftMotorP1,LOW);
digitalWrite(LeftMotorP2,LOW);
digitalWrite(RightMotorP1,LOW);
digitalWrite(RightMotorP2,LOW);

pinMode(LeftMotorInterruptP, INPUT_PULLUP); //First set the pin to pull-up input mode
pinMode(RightMotorInterruptP, INPUT_PULLUP); //First set the pin to pull-up input mode
pinMode(LeftMotorCountP,INPUT);
pinMode(RightMotorCountP,INPUT);

attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}

void loop(){

}```

## PID control algorithm

### 2. Encoder count conversion angle

```const int CountperCircuit = 390; //390 pulses are one circle
const double pi = 3.141592653;        //pi

const byte LeftMotorInterruptP = 22; //Left motor encoder interrupt pin
const byte LeftMotorCountP = 23; //Left motor encoder count pin
const byte RightMotorInterruptP = 18; //Right motor encoder interrupt pin
const byte RightMotorCountP = 19; //Right motor encoder count pin

const byte MotorDriverEn = 5; // motor driver enable
const byte LeftMotorP1 = 15; //Left motor control io port 1
const byte LeftMotorP2 = 13; //Left motor control io port 2
const byte RightMotorP1 = 16; //Right motor control io port 1
const byte RightMotorP2 = 17; //Right motor control io port 2

volatile long LeftMotorCounter = 0; //left motor interrupt count position
volatile long RightMotorCounter = 0; //Right motor interrupt count position
volatile double LeftMotordeg = 0.0; //left motor interrupt count position
volatile double RightMotordeg = 0.0; //Right motor interrupt count position

portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED; //Declare a variable of type portMUX_TYPE, which is used to process the synchronization between the main code and the interrupt
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;

//left motor interrupt function
void LeftMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux_1);
delayMicroseconds(10); //Delay 20ms as debounce, if it is a very stable interrupt, you can add no or very little debounce time
if(digitalRead(LeftMotorInterruptP) == LOW) //Because it is a pull-up trigger, read the high and low levels of the pin after the debounce time, if it is still low, it means a stable interrupt has occurred
{
LeftMotorCounter++;
else
LeftMotorCounter--;
LeftMotordeg = double(LeftMotorCounter)/double(CountperCircuit)*360.0;
Serial.print("Left motor position:");Serial.print(LeftMotordeg);Serial.println("'C");
}
portEXIT_CRITICAL_ISR(&mux_1);
}
//Right motor interrupt function
void RightMotorInterruptF() {
portENTER_CRITICAL_ISR(&mux);
delayMicroseconds(10); //Delay 2ms as debounce, if it is a very stable interrupt, you can add no or very little debounce time
if(digitalRead(RightMotorInterruptP) == LOW) //Because it is a pull-down trigger, read the high and low levels of the pin after the debounce time, if it is still low, it means a stable interrupt has occurred
{
RightMotorCounter--;
else
RightMotorCounter++;
RightMotordeg = double(RightMotorCounter)/double(CountperCircuit)*360.0;
Serial.print("Right motor position:");Serial.print(RightMotordeg);Serial.println("'C");
}
portEXIT_CRITICAL_ISR(&mux);
}

void setup(){
Serial.begin(115200);
Serial.println("Interrupt Test Experiment");

//off motor enable
pinMode(MotorDriverEn,OUTPUT);
digitalWrite(MotorDriverEn,LOW);

pinMode(LeftMotorP1,OUTPUT);
pinMode(LeftMotorP2,OUTPUT);
pinMode(RightMotorP1,OUTPUT);
pinMode(RightMotorP2,OUTPUT);
digitalWrite(LeftMotorP1,LOW);
digitalWrite(LeftMotorP2,LOW);
digitalWrite(RightMotorP1,LOW);
digitalWrite(RightMotorP2,LOW);

pinMode(LeftMotorInterruptP, INPUT_PULLUP); //First set the pin to pull-up input mode
pinMode(RightMotorInterruptP, INPUT_PULLUP); //First set the pin to pull-up input mode
pinMode(LeftMotorCountP,INPUT);
pinMode(RightMotorCountP,INPUT);

attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}

void loop(){

}```

# Dynamic modeling of the car as a whole

After measurement, my balance car

Mass: m=0.748kg

The height of the center of mass is about 40mm: L=0.04m

Use 9.8 for the acceleration of gravity: g=9.8m/s^2

Then the dynamic model is:

## Judging the dynamic characteristics of the system by eigenvalues

Calculate eigenvalues

It can be seen that the two eigenvalues ​​are not both less than 0. That is to say, the system is unstable.

This is the same as our experience, if the balance cart has no input, it will tip over.

Then we look at controllability :

That is, the rank of the Co matrix. If the Co matrix is ​​full rank, it means that the system is controllable.

It can be seen that the Co matrix is ​​full rank, which means that our system is controllable.

Then we can design the controller to control.

We design a linear controller

At this point, our controller is designed, and the input u is the same as the last line in the above figure.

# Lomborg Observer

Derive

After finding L, we arrive at the expression for the observer.

Then our observer can be written as a specific expression: