Hi, I am current working on a project which I am gonna built a 2-D self stabilization platform with 2 servos. At the beginning, I just used the value directly from what I have got from gyroscope to control the servos. Then I found out that the platform is shaky, I did a little bit research on this, resulting in PID controller probably is a good alternative for me. I have attached my code to this post. Anyone here is familiar with the PID programming, could you please take a look at this code and help me to figure out how to include the PID to it? It would be greatly appreciated.
#include <Servo.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo myservo;
Servo myservoy;
int val;
int valy;
int prevVal;
int prevValy;
void setup()
{
Wire.begin();
Serial.begin(38400);
Serial.println('Initialize MPU');
mpu.initialize();
Serial.println(mpu.testConnection() ? 'Connected' : 'Connection failed');
myservo.attach(7);
myservoy.attach(8);
}
void loop()
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
valy = map(ay, -17000, 17000, 0, 179);
val = map(ax, -17000, 17000, 0, 179);
if (val != prevVal)
{
//val=val-90;
myservo.write(val);
prevVal = val;
Serial.print("x : ");
Serial.println(val);
}
if (valy != prevValy)
{
//val=val-90;
myservoy.write(valy);
prevValy = valy;
Serial.print("y : ");
Serial.println(valy);
}
}