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.