Can I please have help getting my PID code to work. I'm not sure where it's going wrong. The angle value isn't changing, and therefore error is not changing. This makes the motor_output keep increasing instead of changing to what it needs to be, this means that the servo motor won't move, which is constrained to a position of 30 to 150 degrees. I am using an IMU MPU6050 three axis gyroscope and accelerometer. I can't change the code to much, and I can't use a different sensor because of constraints, mostly because this is the first time I've done something like this and am new to C++ and Arduino.
Here is my code below:
Hey guys, sorry, like I said I'm new to this. Here it is formatted properly.
#include <Servo.h>
#include "MPU6050.h"
#define GYROSCALE 0.00763358778626
#define ACCELSCALE 0.00006103515625
MPU6050 accelgyro;
//Add the includes for the sensor libraries
//Setup the gains for your control loop here
#define KP 0.001
#define KI 0
#define KD 0
//Setup the variables for your control
Servo fin_servo; //create servo object to control a servo
float set_point = 0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float f_gx, f_gy, f_gz;
float ang_vel = 0;
int16_t bias;
float error;
float motor_output;
float angle_value;
long sum;
float f_angle;
float integral = 0;
float derivative;
float error_previous = 0;
//Setup the time recording variables, these are unsigned long as they can only be positive integers and can be quite large values.
unsigned long DT, end_time, start_time;
void setup() {
// put your setup code here, to run once:
//Setup serial port and baud rate, ensure baudrate in serial monitor matches up!
Serial.begin(38400);
fin_servo.write(75);
// get or set setpoint value
set_point = 75; // desired angle in degrees, change this to what is required.
//attach your servo motor to pin 8, check if your hardware uses pin 8 for the servo motor
signal.
fin_servo.attach(8);
//calibrate the sensor
for(int i = 0; i<500; i++){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sum = sum + gx;
delay(10);
}
bias = sum/500;
}
void loop() {
// put your main code here, to run repeatedly:
//get starttime
start_time = millis();
//get sensor value, this needs to be returned in angvelocity (this should be calibrated as
well)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ang_vel = (gx - bias) * GYROSCALE;
//integrate the angular vel to get angle.
angle_value = angle_value + ang_vel * DT/1000.0;
//calc error
error = set_point - angle_value;
// print error to serial to check
Serial.print("Error is: ");
Serial.println(error);
//calc integral error
integral = integral + error*DT/1000.0;
//calc derivative error
derivative = (error - error_previous)/DT/1000.0;
//calc motor output
motor_output = KP*error + KI*integral + KD*derivative;
//check if motor output is within its limits of the servo motor.
if (150 >= motor_output && motor_output >= 30) {Serial.println("Motor output is within the
limits of the servo motor");
fin_servo.write(motor_output);
}
else {Serial.println("Motor output is outside of the limits of the servo motor");
}
//printout serial motor_output for debug
Serial.println(motor_output);
//set error_previous = error for derivative error calc next time around
error_previous = error;
//get the endtime
end_time = millis();
DT = end_time - start_time; //calc loop time, this should be less that 50ms but greater than 10ms
//print DT to serial to check
Serial.println(DT);
}
Please post your sketch, using code tags when you do. This prevents parts of it being interpreted as HTML coding and makes it easier to copy for examination
In my experience the easiest way to tidy up the code and add the code tags is as follows
Start by tidying up your code by using Tools/Auto Format in the IDE to make it easier to read. Then use Edit/Copy for Forum and paste what was copied in a new reply. Code tags will have been added to the code to make it easy to read in the forum thus making it easier to provide help.
#include <Servo.h>
#include "MPU6050.h"
#define GYROSCALE 0.00763358778626
#define ACCELSCALE 0.00006103515625
MPU6050 accelgyro;
//Add the includes for the sensor libraries
//Setup the gains for your control loop here
#define KP 0.001
#define KI 0
#define KD 0
//Setup the variables for your control
Servo fin_servo; //create servo object to control a servo
float set_point = 0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float f_gx, f_gy, f_gz;
float ang_vel = 0;
float error;
float motor_output;
float angle_value;
int16_t bias;
long sum;
float f_angle;
float integral = 0;
float derivative;
float error_previous = 0;
//Setup the time recording variables, these are unsigned long as they can only be positive integers and can be quite large values.
unsigned long DT, end_time, start_time;
void setup() {
// put your setup code here, to run once:
//Setup serial port and baud rate, ensure baudrate in serial monitor matches up!
Serial.begin(38400);
fin_servo.write(75);
// get or set setpoint value
set_point = 75; // desired angle in degrees, change this to what is required.
//attach your servo motor to pin 8, check if your hardware uses pin 8 for the servo motor signal.
fin_servo.attach(8);
//calibrate the sensor
for (int i = 0; i < 500; i++) {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sum = sum + gx;
delay(10);
}
bias = sum / 500;
}
void loop() {
// put your main code here, to run repeatedly:
//get starttime
start_time = millis();
//get sensor value, this needs to be returned in angvelocity (this should be calibrated as well)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
ang_vel = (gx - bias) * GYROSCALE;
//integrate the angular vel to get angle.
angle_value = angle_value + ang_vel * DT / 1000.0;
//calc error
error = set_point - angle_value;
// print error to serial to check
Serial.print("Error is: ");
Serial.println(error);
//calc integral error
integral = integral + error * DT / 1000.0;
//calc derivative error
derivative = (error - error_previous) / DT / 1000.0;
//calc motor output
motor_output = KPerror + KIintegral + KD * derivative;
//check if motor output is within its limits of the servo motor.
if (150 >= motor_output && motor_output >= 30) {
Serial.println("Motor output is within the limits of the servo motor");
fin_servo.write(motor_output);
}
else {
Serial.println("Motor output is outside of the limits of the servo motor");
}
//printout serial motor_output for debug
Serial.println(motor_output);
//set error_previous = error for derivative error calc next time around
error_previous = error;
//get the endtime
end_time = millis();
DT = end_time - start_time; //calc loop time, this should be less that 50ms but greater than 10ms
//print DT to serial to check
Serial.println(DT);
}
It does not compile ...
sketch.ino: In function 'void loop()':
sketch.ino:80:18: error: 'KPerror' was not declared in this scope
motor_output = KPerror + KIintegral + KD * derivative;
^~~~~~~
sketch.ino:80:18: note: suggested alternative: 'error'
motor_output = KPerror + KIintegral + KD * derivative;
^~~~~~~
error
sketch.ino:80:28: error: 'KIintegral' was not declared in this scope
motor_output = KPerror + KIintegral + KD * derivative;
^~~~~~~~~~
sketch.ino:80:28: note: suggested alternative: 'integral'
motor_output = KPerror + KIintegral + KD * derivative;
^~~~~~~~~~
integral
Error during build: exit status 1
I copied your sketch to Wokwi for formatting (plus removal of some blank lines) and testing:
However, you are using a servo not a stepper motor. Does it really make sense to change the servo position based on an angle value calculated from integrating the angular velocity of a gyro? If I do not miss something there is no reference to the real starting angle, the software always starts with zero.
The line " fin_servo.attach(8);" must be called before the line " fin_servo.write(75);" in setup()!
Thanks. I am trying to base the movement of the servo motor of moving the device, and that's why I'm basing motor output off the angle value calculated of a gyro value. I didn't see that last error though, and I do know that your order is the correct order for it to work. Thanks for pointing out my mistake.
I did, however, look at the datasheet of the gyroscope I am using, the IMU MPU6050, and it says the angle is outputted in degrees per second. This just needs to be converted from analogue output to something readable by humans, which is what my conversions do.
On the second point, I recently just tested it and found that I haven't been getting the values from the gyroscope, or at least, there is an error concerning those values in my code. I had set it to 0.001 because I was seeing how it responded to different values. Because I do not seem to be receiving the values for the gyroscope, or if I am, they just aren't having effects on my calculations, the KP, KI, and KD values aren't influencing things too much, only that if I set KI to a non-zero value, the error just keeps on increasing without seeming to slow down or stop.
I have some code that successfully gets the values from the gyroscope, so I'll look at this against that. If I find anything I did wrong, I'll post it.
Thanks
Starting up sitting still, it could be biased by +/-20°/sec and rapidly integrate away from 0.
How fast do you expect the system spin in real life? If the fin was at one extreme, would it spin at 250°/sec? 500°/sec? More?
One way to get reasonable PID Kp starting values is to look at them as a units conversion and do the math -- If you had 75° of error, how much output fin deflection would you want the system to produce? 60° of servo? then Kp should be on the order of 60°/75° = 0.8°ServoDefection/°AngleError.
Integration of a gyro could rapidly go to much more than 75° of error What would you want the system to do if your integration showed more than than 180° or 360° or more turns of error? Unwind all the way back? Or slow down and turn to the closest angle?
Since you don't have a magnetometer to measure actual angle, maybe it would be best to try to get the gyro rate down to 0 before you deal with controlling the integration of the gyro.
Some of this might seem different, as the particular library I'm using is from GitHub, and is designed by Jeff Rowberg, who seems to be from MIT. I used particular library this on a recommendation. I am fairly certain, but not 100% sure, that it requires this startup code to run the MPU6050.
The final line automatically sets the gyroscope range to +/-250 degrees, and to change the range you just increase the 0 to 1, 2, ...
As for bias, I used a for loop to calculate the mean of the first 500 values as bias, then subtracted this from all further values to remove potential bias, which gets the values fairly close to zero. I know this isn't the best method, but it is fairly fast, and when I used slower methods, or used this method multiple times, the Arduino just wouldn't run the program as it was too slow. I also won't be moving the actual device to fast, and I'll be keeping it within a range of movement from 30 to 150 degrees. Here's the for loop I mentioned earlier:
for (int i = 0; i < 500; i++) {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
sum = sum + gx;
delay(10);
}
bias = sum / 500;
I also multiplied the Gyroscope values by the Gyroscale value to convert the values from raw to degrees per second.
Thanks again
