Hi.
So I'm trying to learn PID controllers for another project and to do so I decided to make a simple 'auto-stabilising' platform.
The idea being a gyro/accel (MPU-6050) measures the angle the system is at, runs the PID controller based on a target setpoint (i.e 90 degrees) and then writes a new position to the servo. Thats the plan anyway.
My code results in huge errors and the output (servo position) maxes out almost immediately. I'm not sure if im using the PID() function wrong, or if theres some other stupid mistake in there.
NB. I know that a PID might be overkill for this sort of thing but as I mentioned above, this is a small part of a much larger project. Eventually I am going to need two servo's which respond to serial inputs to move themselves to a desired angle, and then use the MPU or similar in a control loop to ensure they are at the correct angles. I have build most of this, except for the control system, hence why I am doing this
//Include Libs
#include <Wire.h>
#include <Servo.h>
#include <MPU6050_tockn.h>
#include <PID_v1.h>
//Define Constants
const int servo_pin = 3;
const double Kp = 2, Ki = 5, Kd = 1; //PID controller values
double setpoint = 90; //Target Position
//Define Variables
double servo_pos, y_angle; //Servo Position and Measured Y angle
//Create Servo, MPU and PID objects
Servo servo_1;
MPU6050 MPU_1(Wire, 0.1, 0.9); //0.1 & 0.9 are percentages of Accel and Gyro angle used
PID PID_1(&y_angle, &servo_pos, &setpoint, Kp, Ki, Kd, DIRECT);
void setup() {
Serial.begin(115200); //For Debugging and Displaying results
servo_1.attach(servo_pin); //Attach Servo
servo_pos = setpoint;
servo_1.write(servo_pos); //Turn Servo to Desired Angle to start
Wire.begin(); //Begin I2C comms.
MPU_1.begin(); //Start Comms with MPU-6050
MPU_1.calcGyroOffsets(true); //Run Gyro Calibration
PID_1.SetMode(AUTOMATIC); //Start PID
}
void loop() {
MPU_1.update(); //Get Data From MPU
y_angle = MPU_1.getAngleY() + 90; //Calculate angle as raw angle + 90
PID_1.Compute(); //Compute PID
servo_1.write(servo_pos); //Write PID Output to Servo
Serial.print("Input Angle: ");Serial.print(setpoint);
Serial.print("\tMeasured Angle: ");Serial.print(y_angle);
Serial.print("\tServo Position: ");Serial.println(servo_pos);
delay(5);
}