Hi guys my name is Juraj and I am working with arduino since January.
I am working on project whichi consist of acclerometer and gyroscope MPU6050. I come to one point which i cant move from.
My code read quite good the rotation of my MPU in degreece(stable position is 180 degreece) but everytime when I tilt it on side X or Y more than 270 or less than 90 degrece, it starts measure completelly inmossible things can anybody help me please?
my code :
#include <Servo.h>
#include <Wire.h>
#include "Kalman.h"
#define COM_CONST1 0.99
#define COM_CONST2 0.1
#define LSBsensitivity 131
Kalman kalmanX;
Kalman kalmanY;
Servo xservo;
uint8_t IMUAddress = 0x68; // MPU6050 Address
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
int mapX;
double pitch; //accXangle
double roll; //accYangle
double gyroXangle = 9;
double gyroYangle = 180;
double compAngleX = 90;
double compAngleY = 90;
double kalAngleX;
double kalAngleY;
uint32_t timer;
// ---------- VOID SETUP START -------------- /
void setup() {
Serial.begin(115200);
// xservo.attach(9);
// xservo.write(90);
Wire.begin();
i2cWrite(0x6B,0x00); // Disable sleep mode
if(i2cRead(0x75,1)[0] != IMUAddress) { // Read "WHO_AM_I" register
Serial.print(F("MPU-6050 with address 0x"));
Serial.print(IMUAddress,HEX);
Serial.println(F(" is not connected"));
while(1);
}
uint8_t* data = i2cRead(0x3B, 6);
accX = (data[0] << 8) | data[1];
accY = (data[2] << 8) | data[3];
accZ = (data[4] << 8) | data[5];
rollpitch();
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
/* Update all the values */
readValues();
KalmanValue();
Serial.print(kalAngleX);
Serial.print("\t");
Serial.print(kalAngleY);
Serial.print("\t");
Serial.print("\r\n");
delay(20); // delay to allow servos to move (ms)
}
void moveServo()
{
mapX = map(kalAngleX, 80, 280, 0, 179); //calculate limitation of servo mechanical
Serial.print("\t");
xservo.write(mapX); // Send signal to servo
Serial.print(mapX);
Serial.print("\n");
}
void KalmanValue()
{
rollpitch();
double dt = (double)(micros()-timer)/1000000;
timer = micros();
double gyroXrate = ( (double) gyroX / LSBsensitivity ); // LSB Sensitivity 131 LSB/s Full scale range = +- 250 degree/s (datasheet)
double gyroYrate =-( (double) gyroY / LSBsensitivity );
if (( kalAngleX > 360) || (kalAngleX < -0)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
}
else kalAngleX = kalmanX.getAngle(pitch, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 270)
gyroYrate = -gyroYrate;
kalAngleY = kalmanY.getAngle(roll, gyroYrate, dt); // Calculate the angle using a Kalman filter
}
#define RESTRICT_PITCH
void rollpitch()
{
#ifdef RESTRICT_PITCH
roll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; //accYangle
pitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; //accXangle
#else
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
}
// -- FUNCTIONS START --
void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data[nbytes];
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data[i] = Wire.read();
return data;
}
void readValues()
{
uint8_t* data = i2cRead(0x3B,14);
accX = ((data[0] << 8) | data[1]);
accY = ((data[2] << 8) | data[3]);
accZ = ((data[4] << 8) | data[5]);
tempRaw = ((data[6] << 8) | data[7]);
gyroX = ((data[8] << 8) | data[9]);
gyroY = ((data[10] << 8) | data[11]);
gyroZ = ((data[12] << 8) | data[13]);
return ;
}
Thank you all so much for your helo. I am thankfull for all kinds of help. If you have some source where can I learn more about Kalman filter please send me.
Next in the future I woult like make balancing robot but there comes another problem and that is PID control. I wrote one PID algorytm but everytime when I start this code the values is increasing. Is it correct? Thank you for help!!!!!!!