Моя программа подвеса не компилируется

Question

Моя программа подвеса не компилируется

accelerometer

мой код:

// Получить значения рысканья, тангажа и крена
#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

Обсуждение

Пожалуйста, покажите нам свой полный код. Вы определили ypr заранее?, @chrisl

а также любые сообщения об ошибках из консоли отладки, @MichaelT

1 ответ

@*Jeff A* · Answer 1 · 2020-01-05 19:24-00:00

Я знаю, что это скорее комментарий, но я пока не могу этого сделать.

Это для кода с wire.h в верхней части кода.

Я загрузил ваш код в свою IDE Arduino, и мои номера строк ссылаются на него

Строки 22–32 закомментированы.
Символ // в конце строки 40 может вызвать проблемы.
Если строки 41–43 такие же, как у вас в пустая установка () то же самое, тогда у вас есть программа, делающая что-то снова и снова, что уже было настроено.
в строке 145 у вас есть один */, который должен был найти компилятор.
Строки с 136 по 178 не находятся внутри {} и не выполняются. Они также очень похожи на то, что находится внутри вашего void loop()
В строке 180 используется тот же void calculate_IMU_error() {, что и в строке 80. Какой из них выполняется?