Arduino-ερώτηση σχετικά με γυροσκόπιο

[alexvs]

Καλησπέρα, έχω φτιάξει ένα όχημα δύο αξόνων του οποίο ο σκοπός είναι να ισορροπεί. Χρησιμοποιώ δύο servo κινητήρες συνεχής περιστροφής (FS90R) και ένα γυροσκόπιο MPU6050. Στο κώδικα που έχω γράψει, υπολογίζω την γωνία μέσω του γυροσκοπίου και με την χρήση ενός PID ελεγκτή προσπαθώ να ισορροπήσω το όχημα.Όμως αντιμετωπίζω ένα πρόβλημα, μετά απο ένα σημείο ενώ ισορροπεί κατα κάποιο τρόπο(δεν ισορροπεί πάντα 100%) σταματά να ανταποκρίνεται στο MPU και το όχημα απλά πέφτει. Έχει κανείς κάποια ιδέα γιατί μπορεί να συμβαίνει αυτό; 

[Επισκέπτης]

Αμα δεν δούμε τον κώδικα σου πώς να καταλάβουμε?

[NewProject]

Βάλε debugger.

[alexvs]

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <Wire.h>
#include <Servo.h>

#define sampleTime  0.005
#define targetAngle -0.02
Servo right_eng;
Servo left_eng;

MPU6050 mpu;

int accY, accZ, gyroX;
int gyroRate;
float accAngle, gyroAngle, currentAngle, prevAngle=0, currentAngle1, error, prevError=0, errorSum=0;

float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;

float PID, pwmLeft, pwmRight, previous_error;
float pid_p=0;
float pid_i=0;
float pid_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=500;//3.55 //500
double ki=300;//0.005 300
double kd=500;//2.05 500
///////////////////////////////////////////////

double throttle=1500; //initial value of throttle to the motors

void setup() {
 Wire.begin();//Start I2C as master
 Serial.begin(115200);  
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x6B);                                                    //Send the requested starting register
  Wire.write(0x00);                                                    //Set the requested starting register
  Wire.endTransmission();                                             
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1C);                                                    //Send the requested starting register
  Wire.write(0x10);                                                    //Set the requested starting register
  Wire.endTransmission();                                             
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0x08);                                                    //Set the requested starting register
  Wire.endTransmission(); 
  
  
  right_eng.attach(9);   //attatch the right motor to pin 3
  left_eng.attach(10);  //attatch the left motor to pin 5
 // pinMode(12, OUTPUT); //led
  time = millis(); //Start counting time in milliseconds
 
  delay(7000); 
  left_eng.writeMicroseconds(900); 
  right_eng.writeMicroseconds(2100);
  
}

void loop() {
// put your main code here, to run repeatedly:
timePrev = time;  // the previous time is stored before the actual time read
time = millis();  // actual time read
elapsedTime = (time - timePrev) / 1000; 

  // read acceleration and gyroscope values
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();  
  gyroX = mpu.getRotationX();

   

  // calculate the angle of inclination
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate*sampleTime;  
 
 currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
 Serial.println(currentAngle);

 error = currentAngle - targetAngle;
 errorSum = error;
 previous_error = error;

 pid_p = kp*errorSum;

 pid_i= ki*(errorSum)*sampleTime;
 
pid_d = kd*((error-previous_error)/elapsedTime);

PID = pid_p + pid_i + pid_d;

pwmLeft  = throttle - PID;
pwmRight = throttle + PID;

left_eng.writeMicroseconds(pwmLeft);
right_eng.writeMicroseconds(pwmRight);

}