Мобильная версияВ состоянии покоя у меня есть разумное значение, но когда я наклоняю своего робота, гироскоп все еще дает меня разумные значения, тогда как значение ускорения всегда находится в диапазоне [-1;1].
- Остановка робота:
aX: 0,06 - aY: 0.00 - АЯ: 1,06 - gX: -0,11 - gY: 0,00 - gZ: -0,03 - Крен: -0,04 - Шаг: -0,00 - Рыскание: -0,01 - Наклон вправо:
aX: 0,39 - aY: 0,92 - aZ: 1,12 -gX: 4,36 - gY: 0,73 - gZ: -1,35 -
Крен: 8,29 - Шаг: 1,39 - Рыскание: -2,62
Я надеюсь получить некоторые разъяснения.
Ниже кода:
#include
#include
#include
static double previousTime = 0;
struct accel_data{
int16_t tmp_x = 0;
int16_t tmp_y = 0;
int16_t tmp_z = 0;
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
};
struct gyros_data {
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
int16_t tmp_x = 0;
int16_t tmp_y = 0;
int16_t tmp_z = 0;
};
struct accel_gyros{
float gx = 0;
float gy = 0;
float gz = 0;
float ax = 0;
float ay = 0;
float az = 0;
float roll = 0;
float pitch = 0;
float yaw = 0;
};
class MPU6050_obj{
public:
accel_data a_data;
gyros_data g_data;
accel_gyros data;
void calibration(const motor_setup&, accel_data& , gyros_data&, MPU6050&);
void data_processing(const accel_data& ,const gyros_data&, accel_gyros&);
};
MPU6050_obj myMPU;
MPU6050 mpu_lib;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu_lib.initialize();
myMPU.calibration(App_motor, myMPU.a_data, myMPU.g_data, mpu_lib);
}
void loop() {
if(irrecv.decode(&results)){
if(static_cast(results.value) == button::unknown){
Serial.println("Ignoring erroneous signal");
irrecv.resume();
return;
}
switch (button_prs) {
case button:
//mpumyMPU.data_processing(myMPU.a_data, myMPU.g_data, myMPU.data);
break;
case button:
//something
break;
default:
break;
}
irrecv.resume();
}
}
void MPU6050_obj::calibration(const motor_setup& App_motor, accel_data& a_data, gyros_data& g_data, MPU6050& mpu_lib){ //There are better ways to implement this fcn
int16_t accel_x, accel_y, accel_z, gyros_x, gyros_y, gyros_z;
int n = 250, i = 0;
// Set gyroscope sensitivity to ±250 dps (131)
mpu_lib.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //I'm using ±250dps and ±2G because of my robot behaviour (small turns, flat ground mostly, no rapid accelerations)
// Set accelerometer sensitivity to ±2 G (16384)
mpu_lib.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
//reading_test();
if(mpu_lib.testConnection() && App_motor.get_pin() == LOW){ //add this piece of code beacuse cal has to happen in standstill condition
for (i = 0; i < n ; i++){
mpu_lib.getAcceleration(&accel_x, &accel_y, &accel_z);
mpu_lib.getRotation(&gyros_x, &gyros_y, &gyros_z);
a_data.tmp_x += accel_x;
a_data.tmp_y += accel_y;
a_data.tmp_z += accel_z;
g_data.tmp_x += gyros_x;
g_data.tmp_y += gyros_y;
g_data.tmp_z += gyros_z;
delay(10);
}
a_data.ax = a_data.tmp_x/n;
a_data.ay = a_data.tmp_y/n;
a_data.az = a_data.tmp_z/n;
g_data.gx = g_data.tmp_x/n;
g_data.gy = g_data.tmp_y/n;
g_data.gz = g_data.tmp_z/n;
}
else Serial.print("MPU non identified");
}
void MPU6050_obj::data_processing(const accel_data& a_data ,const gyros_data& g_data, accel_gyros& data){
int s_a = 16384, s_g = 131; //accelerometer/gyroscope sensitivity
float alpha = 0.98;//weight factor
float a_roll = 0, a_pitch = 0, g_roll = 0, g_pitch = 0, yaw = 0;
unsigned long currentTime = micros();
float dt = (currentTime - previousTime) / 1000000.0; // Calculate dt in seconds
int16_t accel_x, accel_y, accel_z, gyros_x, gyros_y, gyros_z;
previousTime = currentTime; //PreviousTime is defined as static
//Start measurements
mpu_lib.getAcceleration(&accel_x, &accel_y, &accel_z);
mpu_lib.getRotation(&gyros_x, &gyros_y, &gyros_z);
//Subtracting the bias (calculated in calibration function) and dividing by sensitivy in order to convert raw values to dps and gravitation force (G)
data.ax = (static_cast(accel_x) - static_cast(a_data.ax))/s_a;
data.ay = (static_cast(accel_y) - static_cast(a_data.ay))/s_a;
data.az = (static_cast(accel_z) - static_cast(a_data.az))/s_a;
data.gx = ((static_cast(gyros_x) - static_cast(g_data.gx))/s_g) * (PI / 180.0); //rad/s
data.gy = ((static_cast(gyros_y) - static_cast(g_data.gy))/s_g) * (PI / 180.0); //rad/s
data.gz = ((static_cast(gyros_z) - static_cast(g_data.gz))/s_g) * (PI / 180.0);
//Calculating roll and pitch from accel readings
a_roll = atan2(data.ay, sqrt(pow(data.ax, 2) + pow(data.az, 2)));
a_pitch = atan2(-data.ax, sqrt(pow(data.ay, 2) + pow(data.az, 2)));
//Integrate gyroscope data for angular velocity
yaw += data.gz * dt;
g_roll += data.gx * dt;
g_pitch += data.gy * dt;
// Complementary filter
data.roll = alpha * g_roll + (1 - alpha) * a_roll;
data.pitch = alpha * g_pitch + (1 - alpha) * a_pitch;
data.yaw = yaw;
}
Подробнее здесь: https://stackoverflow.com/questions/791 ... sue-with-a
