Hi,
I programmed a PID for a MPU6050 and a servo. Obviously I am experiencing some problem with the Output. I summarize it this way:
Input Output
From 0 to 90 large value
From 90 to 180 0
Any guess?
Here is my code:
/********************************************************
- PID Basic Example
********************************************************/
#include <PID_v1.h>
//Hereafter the code for the IMU+servo
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo myservo;
int val, val2;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
void setup()
{
//initialize the variables we're linked to
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
val = map(ay, -17000, 17000, 0, 255);
Input = val;
Setpoint = 90;
//turn the PID on
myPID.SetMode(AUTOMATIC);
//Hereafter the code for the IMU+servo
Wire.begin();
Serial.begin(38400);
Serial.println("Initialize MPU");
mpu.initialize();
Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
myservo.attach(9);
Serial.begin(9600); //initialize serial monitor
}
void loop()
{
//Hereafter the code for the IMU+servo
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
val = map(ay, -17000, 17000, 0, 255);
Input = val;
myPID.Compute();
//analogWrite(9,Output);
val2 = map(Output, 0, 255, 0, 179);
myservo.write(val2);
//Print some info for debug
Serial.print("Setpoint: ");
Serial.print(Setpoint);
Serial.print(" Intput: ");
Serial.print(val);
Serial.print(" Output: ");
Serial.println(Output);
delay(100);
}
