Ball and Beam experiment - LQR control

Hi all. I have a problem with programming in Arduino. Help please, this is my diploma work. With the help of kind people I was able to realize this work with PID controller. Now we need to implement it with the LQR controller. The system is called “Ball and Beam”. If you want, I can explain the principle of operation. Here’s my code in Arduino for PID controller:

#include<Servo.h>
#include<PID_v1.h>

const int servoPin = 11;                                               //Servo Pin
 
float Kp = 5.5;                                                    //Initial Proportional Gain
float Ki = 0.1;                                                      //Initial Integral Gain
float Kd = 0.5;                                                    //Intitial Derivative Gain
double Setpoint, Input, Output, ServoOutput;                                       

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);           //Initialize PID object, which is in the class PID.
                                                                     
Servo myServo;                                                       //Initialize Servo.
const int echoPin = 4;
const int trigPin = 7;

void setup() {

  Serial.begin(9600);      //Begin Serial 
   pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myServo.attach(servoPin);                                          //Attach Servo

  Input = readPosition();                                            //Calls function readPosition() and sets the balls
                                                                     //  position as the input to the PID algorithm
                                                                      
  myPID.SetMode(AUTOMATIC);                                          //Set PID object myPID to AUTOMATIC 
  myPID.SetOutputLimits(-80,80);                                     //Set Output limits to -80 and 80 degrees. 
}

void loop()
{
 
  Setpoint = 10;
  Input = readPosition();                                            
 
  myPID.Compute();                                                   //computes Output in range of -80 to 80 degrees
  ServoOutput=102+Output;                                            // 150 degrees is my horizontal 
  myServo.write(ServoOutput);                                        //Writes value of Output to servo  
}

float readPosition() {
  delay(30);                                                            
  
long duration, cm;
unsigned long now = millis();
 digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);

  cm = duration/(29*2);
  
  if(cm > 31)     // 35 cm is the maximum position for the ball
  {cm=31;}
  
  Serial.println(cm);
  
  return cm;                                          //Returns distance value.
}

If you can help in debt will not stay.

Now we need to implement it with the LQR controller.

You want us to guess what a liquor controller is?

Bing told me that LQR is Linear Quadratic Regulator. So did Google. It would have been nice if the OP had also provided this information but perhaps the OP only wants responses from those who already know what LQR means - a poor plan.

The code posted by the OP is for a PID controller, which he had help writing.

Now he wants us to write an LQR controller for him, which is related to PID but more sophisticated.

jremington:
The code posted by the OP is for a PID controller, which he had help writing.

Now he wants us to write an LQR controller for him, which is related to PID but more sophisticated.

Here's some code for him I found on github written in C++ It shouldn't be to difficult to implement on an arduino uno

I was interested in what an LQR actually is... quite interesting!
Z

I have use matlab to find gains for inverted pendulum, but now I am not getting how to transfer or use that gains in arduino.