Здравствуйте! Возникла проблема с дёрганием двух сервоприводов при записи данных на SD-карту.
Оборудование:
- Arduino Uno;
- Модуль MPU6050;
- SD-кардридер (5V версия);
- 2 сервопривода (MG90S);
- Сервоприводы подключены к отдельному источнику питания.
Описание проблемы:
При одновременной работе PID-регулятора (обработка данных с MPU6050) и записи данных на SD-карту сервоприводы начинают дёргаться. Без записи на SD-карту система работает стабильно. Пытался решить проблему:
- Подключил сервы к отдельному блоку питания;
- Увеличил скорость выполнения кода (оптимизировал циклы).
ВОЗМОЖНО ПРОБЛЕМА В ЛОГИКЕ СРАБАТЫВАНИЯ ПРЕРЫВАНИЯ
Помогите, пожалуйста, исправить, уже не знаю, что делать((
#include <SPI.h>
#include <SD.h>
#include "GyverPID.h"
GyverPID pidX, pidY;
#include "I2Cdev.h"
#include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Servo.h>
// Датчики, буфер, флаг готовности mpu
MPU6050 mpu;
volatile bool mpuFlag = false; // флаг прерывания готовности
uint8_t fifoBuffer[64]; // буфер
uint16_t fifoCount;
Quaternion q; // [w, x, y, z]
VectorFloat gravity; // [x, y, z]
float ypr[3];
bool Program_State = 0; // Статус программы
// Сервопрводы
const byte servoPin1 = 5, servoPin2 = 9;
Servo servo1, servo2;
// SD карта
#define pinSelect 4
int lastAngleX = 90;
int lastAngleY = 89;
float yaw, pitch, roll; // Так-же для расчетов
unsigned long lastLogTime = 0;
// Переменные расчета
//float yaw = ypr[0] * 180 / M_PI;
//float pitch = ypr[1] * 180 / M_PI;
//float roll = ypr[2] * 180 / M_PI;
void setup() {
pidX.setDirection(NORMAL);
// Коэффициенты регулятора
pidX.Kp = 3.9018;
pidX.Ki = 2.1041;
pidX.Kd = 1.748;
pidX.setLimits(-90, 90);
pidX.setDt(25);
pidY.setDirection(NORMAL);
pidY.Kp = 3.9018;
pidY.Ki = 2.1041;
pidY.Kd = 1.748;
pidY.setLimits(-90, 90);
pidY.setDt(25);
// Сервомашинки
servo1.attach(servoPin1);
servo2.attach(servoPin2);
servo1.write(90); // Устанавливаем в среднее положение
servo2.write(89); // Устанавливаем в среднее положение, с корректировкой
// Настройка и проверки
Serial.begin(115200);
Wire.begin();
// инициализация DMP и прерывания
mpu.initialize();
mpu.dmpInitialize();
//Сигнал о начале калибровки
CalibrateSignal1();
// Каллибровка датчика
CalibrateMPU6050();
mpu.setDMPEnabled(true);
attachInterrupt(0, dmpReady, RISING);
// Инициализация SD карты
if (!SD.begin(pinSelect)) {
while (1);
}
// Создание файла с заголовком
File dataFile = SD.open("FlyData.csv", FILE_WRITE);
if (dataFile) {
dataFile.println("Time(ms),Program_State,Yaw(deg),Pitch(deg),Roll(deg),Servo1_Angle,Servo2_Angle");
dataFile.close();
}
delay(500);
//Сигнал о завершении каллибровки
CalibrateSignal2();
}
void loop() {
FifoCheck();
switch (Program_State){
case 0:
if (checkLaunch()){
Program_State = 1;
break;
} else {
MpuData1();
Serial.print(0);
logData0();
}
break;
case 1:
if (mpuFlag && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
AnglesAndControll();
mpuFlag = false;
logData1();
Serial.print(1);
}
break;
}
}
// Проверка на запуск
bool checkLaunch() {
static uint32_t tmr;
if (millis() - tmr >= 25) {
tmr = millis();
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (abs(gz) > 15) return true; // Используем абсолютное значение
}
return false;
}
void FifoCheck(){
fifoCount = mpu.getFIFOCount();
if (fifoCount >= 1024) {
mpu.resetFIFO();
}
}
void logData0(){
unsigned long currentTime = millis();
if (currentTime - lastLogTime >= 20) {
lastLogTime = currentTime;
File dataFile = SD.open("FlyData.csv", FILE_WRITE);
String dataString = String(currentTime) + ",";
dataString += String(Program_State) + "," + String(yaw);
dataString += "," + String(pitch) + "," + String(roll);
dataFile.println(dataString);
dataFile.close();
}
}
void logData1(){
unsigned long currentTime = millis();
if (currentTime - lastLogTime >= 20) {
lastLogTime = currentTime;
File dataFile = SD.open("FlyData.csv", FILE_WRITE);
String dataString = String(currentTime) + ",";
dataString += String(Program_State) + "," + String(yaw);
dataString += "," + String(pitch) + "," + String(roll) + "," + String(lastAngleX) + "," + String(lastAngleY);
dataFile.println(dataString);
dataFile.close();
}
}
void AnglesAndControll(){
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yaw = ypr[0] * 180 / M_PI;
pitch = ypr[1] * 180 / M_PI;
roll = ypr[2] * 180 / M_PI;
//-------------------------------------------------------------XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
pidX.setpoint = 0;
pidX.input = pitch;
pidX.getResult();
int angleX = map(pidX.output, -90, 90, 59, 119);
servo1.write(angleX); // servo2
lastAngleX = angleX;
//-------------------------------------------------------------YYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYYY
pidY.setpoint = 0;
pidY.input = roll;
pidY.getResult();
int angleY = map(pidY.output, -90, 90, 60, 120);
servo2.write(angleY); // servo3
lastAngleY = angleY;
mpuFlag = false;
}
void CalibrateMPU6050() {
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
}
void dmpReady() { // Прерывания
mpuFlag = true;
}
void CalibrateSignal1() {
servo1.write(100);
servo2.write(100);
}
void CalibrateSignal2() {
servo1.write(90);
servo2.write(89);
}
void MpuData1(){
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
}```