#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"
#include <avr/interrupt.h>
#include <avr/io.h>
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
Servo motoreSX;
Servo motoreDX;
float velSX;
float velDX;
float P=0;
float I=0;
float D=0;
float PID;
float kP=25;
float kI=0;
float kD=0;
float MaxP=30;
float MinP=-30;
float MaxI=40;
float MinI=-40;
float MaxD=15; //15
float MinD=-15;
int roll=0;
int rollOld=0;
float val_Consegna=0;
//interrupt timer, call PID_callback() every 5 ms
ISR(TIMER2_OVF_vect) {
count++; //Increments the interrupt counter
if(count > 4){
PID_callback();
count = 0; //Resets the interrupt counter
}
TCNT2 = 130; //Reset Timer to 130 out of 255
TIFR2 = 0x00; //Timer2 INT Flag Reg: Clear Timer Overflow Flag
};
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
Serial.begin(9600);
// initialize device
accelgyro.initialize();
/* verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
*/
motoreDX.attach(8);
motoreDX.writeMicroseconds(800);
motoreSX.attach(7);
motoreSX.writeMicroseconds(800);
delay(5000);
for(int x=800; x<=1200; x++){
motoreDX.writeMicroseconds(x);
motoreSX.writeMicroseconds(x);
}
velSX=velDX=1200;
TCCR2B = 0x00; //Disbale Timer2 while we set it up
TCNT2 = 130; //Reset Timer Count to 130 out of 255
TIFR2 = 0x00; //Timer2 INT Flag Reg: Clear Timer Overflow Flag
TIMSK2 = 0x01; //Timer2 INT Reg: Timer2 Overflow Interrupt Enable
TCCR2A = 0x00; //Timer2 Control Reg A: Normal port operation, Wave Gen Mode normal
TCCR2B = 0x05; //Timer2 Control Reg B: Timer Prescaler set to 128
}
//Implementatio of PID
float Pid(float val_Consegna){
if(kP!=0){
P = roll / kP;
if(P>MaxP) P=MaxP;
if(P<MinP) P=MinP;
}
if(kI!=0){
I = I + ((roll/2) * kI);
if(I>MaxI) I=MaxI;
if(I<MinI) I=MinI;
}
if(kD!=0){
D = (roll - rollOld) * kD;
if(D>MaxD) D=MaxD;
if(D<MinD) D=MinD;
}
float PID = P + I + D;
rollOld = roll;
return PID;
}
//calback for PID and update motor state
void PID_callback(){
velDX=1200-PID;
velSX=1200+PID;
//Guardie, fa rimanere la velocità nei limiti di sicurezza
if(velDX>1400) velDX=1400;
if(velSX>1400) velSX=1400;
if(velDX<1100) velDX=1100;
if(velSX<1100) velSX=1100;
motoreSX.writeMicroseconds(int(velSX));
motoreDX.writeMicroseconds(int(velDX));
PID=Pid(0);
}
void loop() {
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
roll=map(constrain(ay, -16000, 16000), -16000, 16000, -1000, 1000);
/* Serial.print(P);
Serial.print("\t");
Serial.print(I);
Serial.print("\t");
Serial.print(D);
Serial.print("\t");
Serial.print(interval);
Serial.print("\t");
Serial.print(roll);
Serial.print("\t");Serial.print(velSX); Serial.print("\t");Serial.println(velDX);
*/
Serial.print(velSX);
Serial.print(" ");
Serial.print(velDX);
Serial.print("\n");
//delay(500);
}
Questo è il mio programma che implementa il PID, non ci salto fuori, mi è difficilissimo regolare il tutto, magari compio qualche errore grossolano che non vedo...
Il valore ay corrisponde al valore del giroscopio, se lo inclino è stabile quindi dovrebbe essre quello.
Grazie!