Моя программа подвеса не компилируется
мой код:
// Получить значения рысканья, тангажа и крена
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
// Значения Yaw, Pitch, Roll - Радианы в градусы
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
// Пропустить 300 показаний (процесс самокалибровки)
if (j <= 300) {
correct = ypr[0]; // Рыскание начинается со случайного значения, поэтому мы фиксируем последнее значение после 300 показаний
j++;
}
// После 300 чтений
else {
ypr[0] = ypr[0] - correct; // Установите рыскание на 0 градусов - вычтите последнее случайное значение рыскания из текущего значения, чтобы сделать рыскание равным 0 градусов
// Сопоставьте значения датчика MPU6050 от -90 до 90 со значениями, подходящими для сервоуправления от 0 до 180
int servo0Value = map(ypr[0], -90, 90, 0, 180);
int servo1Value = map(ypr[1], -90, 90, 0, 180);
int servo2Value = map(ypr[2], -90, 90, 180, 0);
// Управление сервоприводами в соответствии с ориентацией MPU6050
servo0.write(servo0Value);
servo1.write(servo1Value);
servo2.write(servo2Value);
}
#endif
// Значения Yaw, Pitch, Roll - Радианы в градусы
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
// Пропустить 300 показаний (процесс самокалибровки)
if (j <= 300) {
correct = ypr[0]; // Рыскание начинается со случайного значения, поэтому мы фиксируем последнее значение после 300 показаний
j++;
}
// После 300 чтений
else {
ypr[0] = ypr[0] - correct; // Установите рыскание на 0 градусов - вычтите последнее случайное значение рыскания из текущего значения, чтобы сделать рыскание равным 0 градусов
// Сопоставьте значения датчика MPU6050 от -90 до 90 со значениями, подходящими для сервоуправления от 0 до 180
int servo0Value = map(ypr[0], -90, 90, 0, 180);
int servo1Value = map(ypr[1], -90, 90, 0, 180);
int servo2Value = map(ypr[2], -90, 90, 180, 0);
// Управление сервоприводами в соответствии с ориентацией MPU6050
servo0.write(servo0Value);
servo1.write(servo1Value);
servo2.write(servo2Value);
}
он говорит, что ypr не называет тип
/*
Учебное пособие по акселерометру и гироскопу для Arduino и MPU6050
Деян, https://howtomechatronics.com
*/
#include <Wire.h>
const int MPU = 0x68; // I2C-адрес MPU6050
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup() {
Serial.begin(19200);
Wire.begin(); // Инициализация связи
Wire.beginTransmission(MPU); // Начать связь с MPU6050 // MPU=0x68
Wire.write(0x6B); // Обращение к регистру 6B
Wire.write(0x00); // Сделать сброс - поместить 0 в регистр 6B
Wire.endTransmission(true); //заканчиваем передачу
/*
// Настройка чувствительности акселерометра — полный диапазон шкалы (по умолчанию +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); // Обращение к регистру ACCEL_CONFIG (1С hex)
Wire.write(0x10); // Установите биты регистра как 00010000 (+/- 8g полный диапазон шкалы)
Wire.endTransmission(true);
// Настройка чувствительности гироскопа — полный диапазон шкалы (по умолчанию +/- 250 град/с)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Обращение к регистру GYRO_CONFIG (1B hex)
Wire.write(0x10); // Установите биты регистра как 00010000 (полная шкала 1000 град/с)
Wire.endTransmission(true);
delay(20);
*/
// Вызовите эту функцию, если вам нужно получить значения ошибок IMU для вашего модуля
calculate_IMU_error();
delay(20);
}
void loop() {
// === Чтение данных ускорителя === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Начать с регистра 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); //Чтение всего 6 регистров, значение каждой оси хранится в 2 регистрах
//Для диапазона +-2g нам нужно разделить необработанные значения на 16384, согласно таблице данных
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // значение по оси Z
// Расчет крена и тангажа по данным акселерометра
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) Подробнее см. в пользовательской функции calculate_IMU_error().
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
// === Чтение данных гироскопа === //
previousTime = currentTime; // Предыдущее время сохраняется перед чтением фактического времени
currentTime = millis(); // Текущее время фактическое время чтения
elapsedTime = (currentTime - previousTime) / 1000; // Делим на 1000, чтобы получить секунды
Wire.beginTransmission(MPU);
Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); //Чтение всего 4 регистра, значение каждой оси хранится в 2 регистрах
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 град/с мы должны сначала разделить необработанное значение на 131,0, согласно техническому описанию
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
// Исправьте выходные данные с вычисленными значениями ошибки
GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
GyroY = GyroY - 2; // GyroErrorY ~(2)
GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// В настоящее время необработанные значения выражены в градусах в секунду, град/с, поэтому нам нужно умножить на сендоны (с), чтобы получить угол в градусах
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // град/с * с = град
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Дополнительный фильтр - объединить значения акселерометра и угла гироскопа
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
// Печатаем значения на последовательном мониторе
Serial.print(roll);
Serial.print("/");
Serial.print(pitch);
Serial.print("/");
Serial.println(yaw);
}
void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе настройки, чтобы рассчитать погрешность данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанных на последовательном мониторе.
// Обратите внимание, что мы должны разместить IMU горизонтально, чтобы получить правильные значения, чтобы мы могли затем получить правильные значения
// Чтение значений акселерометра 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Суммируем все показания
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
// Делим сумму на 200, чтобы получить значение ошибки
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Чтение значений гироскопа 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Суммируем все показания
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
// Делим сумму на 200, чтобы получить значение ошибки
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Печатаем значения ошибок в последовательном мониторе
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}
Wire.beginTransmission(MPU);
Wire.write(0x1C); // Обращение к регистру ACCEL_CONFIG (1С hex)
Wire.write(0x10); // Установите биты регистра как 00010000 (+/- 8g полный диапазон шкалы)
Wire.endTransmission(true);
// Настройка чувствительности гироскопа — полный диапазон шкалы (по умолчанию +/- 250 град/с)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Обращение к регистру GYRO_CONFIG (1B hex)
Wire.write(0x10); // Установите биты регистра как 00010000 (полная шкала 1000 град/с)
Wire.endTransmission(true);
*/
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Начать с регистра 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); //Чтение всего 6 регистров, значение каждой оси хранится в 2 регистрах
//Для диапазона +-2g нам нужно разделить необработанные значения на 16384, согласно таблице данных
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси X
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Значение по оси Y
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // значение по оси Z
accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) Подробнее см. в пользовательской функции calculate_IMU_error().
accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
previousTime = currentTime; // Предыдущее время сохраняется перед чтением фактического времени
currentTime = millis(); // Текущее время фактическое время чтения
elapsedTime = (currentTime - previousTime) / 1000; // Делим на 1000, чтобы получить секунды
Wire.beginTransmission(MPU);
Wire.write(0x43); // Адрес первого регистра данных гироскопа 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); //Чтение всего 4 регистра, значение каждой оси хранится в 2 регистрах
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // Для диапазона 250 град/с мы должны сначала разделить необработанное значение на 131,0, согласно техническому описанию
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
GyroY = GyroY - 2; // GyroErrorY ~(2)
GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// В настоящее время необработанные значения выражены в градусах в секунду, град/с, поэтому нам нужно умножить на сендоны (с), чтобы получить угол в градусах
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // град/с * с = град
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
void calculate_IMU_error() {
// Мы можем вызвать эту функцию в разделе настройки, чтобы рассчитать погрешность данных акселерометра и гироскопа. Отсюда мы получим значения ошибок, используемые в приведенных выше уравнениях, напечатанных на последовательном мониторе.
// Обратите внимание, что мы должны разместить IMU горизонтально, чтобы получить правильные значения, чтобы мы могли затем получить правильные значения
// Чтение значений акселерометра 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
// Суммируем все показания
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
// Делим сумму на 200, чтобы получить значение ошибки
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Чтение значений гироскопа 200 раз
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
// Суммируем все показания
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
// Делим сумму на 200, чтобы получить значение ошибки
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Печатаем значения ошибок в последовательном мониторе
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}
Мы просто печатаем значения на последовательном мониторе, и как только мы их узнаем, мы можем реализовать их в коде, как показано выше, для расчета как крена, так и тангажа, а также для 3 выходных сигналов гироскопа.
Выводы MPU6050 Pitch Roll и Yaw
Наконец, с помощью функции Serial.print мы можем распечатать значения Roll, Pitch и Yaw на последовательном мониторе и посмотреть, правильно ли работает датчик.
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
if (j <= 300) {
correct = ypr[0];
j++;
}
else {
ypr[0] = ypr[0] - correct;
int servo0Value = map(ypr[0], -90, 90, 0, 180);
int servo1Value = map(ypr[1], -90, 90, 0, 180);
int servo2Value = map(ypr[2], -90, 90, 180, 0);
servo0.write(servo0Value);
servo1.write(servo1Value);
servo2.write(servo2Value);
}
#endif
ypr[0] = ypr[0] * 180 / M_PI;
ypr[1] = ypr[1] * 180 / M_PI;
ypr[2] = ypr[2] * 180 / M_PI;
if (j <= 300) {
correct = ypr[0];
j++;
}
else {
ypr[0] = ypr[0] - correct;
int servo0Value = map(ypr[0], -90, 90, 0, 180);
int servo1Value = map(ypr[1], -90, 90, 0, 180);
int servo2Value = map(ypr[2], -90, 90, 180, 0);
servo0.write(servo0Value);
servo1.write(servo1Value);
servo2.write(servo2Value);
}
@Dabber, 👍0
Обсуждение1 ответ
▲ 1
Я знаю, что это скорее комментарий, но я пока не могу этого сделать.
Это для кода с wire.h в верхней части кода.
Я загрузил ваш код в свою IDE Arduino, и мои номера строк ссылаются на него
Строки 22–32 закомментированы.
Символ // в конце строки 40 может вызвать проблемы.
Если строки 41–43 такие же, как у вас в пустая установка () то же самое, тогда у вас есть программа, делающая что-то снова и снова, что уже было настроено.
в строке 145 у вас есть один */, который должен был найти компилятор.
Строки с 136 по 178 не находятся внутри {} и не выполняются. Они также очень похожи на то, что находится внутри вашего void loop()
В строке 180 используется тот же
void calculate_IMU_error() {, что и в строке 80. Какой из них выполняется?
,
@Jeff A
Смотрите также:
- В чем разница между акселерометром, гироскопом и датчиком магнитометра?
- OVF в последовательном мониторе вместо данных
- Как связаться с датчиком через порты RX/TX Arduino?
- Линейное ускорение от MPU 6050
- Как правильно определить крен, тангаж, перемещение?
- Снять гравитацию с акселерометра MPU-6050
- Правильный способ получить значения крена, тангажа и перемещения
- Arduino Nano IOT LSM6DS3 получить угол гироскопа в градусах
Пожалуйста, покажите нам свой полный код. Вы определили
yprзаранее?, @chrislа также любые сообщения об ошибках из консоли отладки, @MichaelT