Подергивающийся серво MG995 при дистанционном управлении через RemoteXY

Доброго времени суток! Решил собрать небольшого бота, сейчас возникают небольшие проблемы с сервоприводами MG995. Всё устройство управляется с телефона посредством программы RemoteXY и радиомодуля HC-06. В интерфейсе присутствует джойстик для управления положением камеры. Ось X управляет горизонтальным серво, ось Y вертикальным. Работает стабильно, но сервушки немного подергиваются. Пробовал и пришедшее на плату значение делить без остатка на 10, после умножать на 10, и резистор на 10кОм ставил, не помогает. Если у Вас есть идеи, как стабилизировать сервоприводы, то буду признателен и премного благодарен. Код ниже

uint8_t RemoteXY_CONF[] =   // 53 bytes
  { 255,6,0,17,0,46,0,16,156,1,5,4,3,68,24,24,58,24,31,67,
  5,16,16,44,11,97,26,17,3,4,3,13,10,37,58,24,5,1,35,67,
  25,25,58,24,31,4,128,16,37,44,10,2,26 };
// структура определяет все переменные и события вашего интерфейса управления 
struct {
    // input variables
  int8_t drive_x; // oт -100 до 100  
  int8_t drive_y; // oт -100 до 100  
  uint8_t selector; // =0 если переключатель в положении A, =1 если в положении B, =2 если в положении C, ... 
  int8_t camera_x; // oт -100 до 100  
  int8_t camera_y; // oт -100 до 100  
  int8_t slider; // =0..100 slider position
    // output variables
  char info[17];  // =строка UTF8 оканчивающаяся нулем 
    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 
} RemoteXY;
void setup() 
{
  RemoteXY_Init (); 
  kamera_x.attach(8);
  kamera_y.attach(7);
}
void loop() 
{ 
  RemoteXY_Handler ();
  kamera_x.write(map(RemoteXY.camera_x, -100, 100, 45, 135));
  kamera_y.write(map(RemoteXY.camera_y, -100, 100, 135, 45));
}
Serial.println(map(RemoteXY.camera_x, -100, 100, 45, 135));

А код полностью выложить никак нельзя? Секретный?

1 лайк

Спасибо, сам что-то не догадался. Но это не помогло, угол поворота не колеблется, значения в сериале стабильные и соответствуют действительности.

Спойлер
// определение режима соединения и подключение библиотеки RemoteXY 
#define REMOTEXY_MODE__SOFTSERIAL
#include <SoftwareSerial.h>
#include <RemoteXY.h>
#include <MQ135.h>
#include <DHT.h>
#include <SoftwareSerial.h>
#include <Servo.h>
// настройки соединения 
#define REMOTEXY_SERIAL_RX 10
#define REMOTEXY_SERIAL_TX 11
#define REMOTEXY_SERIAL_SPEED 9600
#define PIN_MQ135 A0 // MQ135 Analog Input Pin
#define DHTPIN 4 // DHT Digital Input Pin
#define DHTTYPE DHT11 // DHT11 or DHT22, depends on your sensor
#define PIN_ENA_1 13 // Вывод управления скоростью вращения мотора №1
#define PIN_ENB_1 12 // Вывод управления скоростью вращения мотора №2
#define PIN_IN1_1 50 // Вывод управления направлением вращения мотора №1
#define PIN_IN2_1 51 // Вывод управления направлением вращения мотора №1
#define PIN_IN3_1 48 // Вывод управления направлением вращения мотора №2
#define PIN_IN4_1 49 // Вывод управления направлением вращения мотора №2
#define PIN_ENA_2 2 // Вывод управления скоростью вращения мотора №3
#define PIN_ENB_2 3 // Вывод управления скоростью вращения мотора №4
#define PIN_IN1_2 24 // Вывод управления направлением вращения мотора №3
#define PIN_IN2_2 25 // Вывод управления направлением вращения мотора №3
#define PIN_IN3_2 26 // Вывод управления направлением вращения мотора №4
#define PIN_IN4_2 27 // Вывод управления направлением вращения мотора №4
#define PIN_ENA_3 9 // Вывод управления степенью выдвижения зонда
#define PIN_IN1_3 30 // Вывод управления направлением вращения мотора №5
#define PIN_IN2_3 31 // Вывод управления направлением вращения мотора №5
#define PIN_IN3_3 32 // Вывод управления светом
#define PIN_IN4_3 33 // Вывод управления светом
MQ135 mq135_sensor(PIN_MQ135);
DHT dht(DHTPIN, DHTTYPE);
Servo kamera_x;
Servo kamera_y;
float temperature, humidity, rzero, correctedRZero, resistance, ppm, correctedPPM; // floats for data from sensors
uint8_t powerR, powerL, xVal, yVal; // SPI values
uint8_t powerN = 100;
int z = 0;
uint8_t seconds;
String j; //str that we'll convert to char and send to phone
// конфигурация интерфейса  
#pragma pack(push, 1)
#pragma pack(push, 1)
uint8_t RemoteXY_CONF[] =   // 53 bytes
  { 255,6,0,17,0,46,0,16,156,1,5,4,3,68,24,24,58,24,31,67,
  5,16,16,44,11,97,26,17,3,4,3,13,10,37,58,24,5,1,35,67,
  25,25,58,24,31,4,128,16,37,44,10,2,26 };
// структура определяет все переменные и события вашего интерфейса управления 
struct {
    // input variables
  int8_t drive_x; // oт -100 до 100  
  int8_t drive_y; // oт -100 до 100  
  uint8_t selector; // =0 если переключатель в положении A, =1 если в положении B, =2 если в положении C, ... 
  int8_t camera_x; // oт -100 до 100  
  int8_t camera_y; // oт -100 до 100  
  int8_t slider; // =0..100 slider position
    // output variables
  char info[17];  // =строка UTF8 оканчивающаяся нулем 
    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 
} RemoteXY;
#pragma pack(pop)
void setup() 
{
  RemoteXY_Init (); 
  Serial.begin(9600);
  dht.begin();
  // Установка всех управляющих пинов в режим выхода
  pinMode(PIN_ENA_1, OUTPUT);
  pinMode(PIN_ENB_1, OUTPUT);
  pinMode(PIN_IN1_1, OUTPUT);
  pinMode(PIN_IN2_1, OUTPUT);
  pinMode(PIN_IN3_1, OUTPUT);
  pinMode(PIN_IN4_1, OUTPUT);
  pinMode(PIN_ENA_2, OUTPUT);
  pinMode(PIN_ENB_2, OUTPUT);
  pinMode(PIN_IN1_2, OUTPUT);
  pinMode(PIN_IN2_2, OUTPUT);
  pinMode(PIN_IN3_2, OUTPUT);
  pinMode(PIN_IN4_2, OUTPUT);
  pinMode(PIN_ENA_3, OUTPUT);
  pinMode(PIN_IN1_3, OUTPUT);
  pinMode(PIN_IN2_3, OUTPUT);
  pinMode(PIN_IN3_3, OUTPUT);
  pinMode(PIN_IN4_3, OUTPUT);
  // Команда остановки двум моторам
  digitalWrite(PIN_IN1_1, LOW);
  digitalWrite(PIN_IN2_1, LOW);
  digitalWrite(PIN_IN3_1, LOW);
  digitalWrite(PIN_IN4_1, LOW);
  digitalWrite(PIN_IN1_2, LOW);
  digitalWrite(PIN_IN2_2, LOW);
  digitalWrite(PIN_IN3_2, LOW);
  digitalWrite(PIN_IN4_2, LOW);
  digitalWrite(PIN_IN1_3, LOW);
  digitalWrite(PIN_IN2_3, LOW);
  digitalWrite(PIN_IN3_3, LOW);
  digitalWrite(PIN_IN4_3, LOW);
  analogWrite(PIN_ENA_3, powerN);
  kamera_x.attach(8);
  kamera_y.attach(7);
}
void loop() 
{ 
  humidity = dht.readHumidity();
  temperature = dht.readTemperature();
  if (isnan(humidity) || isnan(temperature)) {
    Serial.println(F("Failed to read from DHT sensor!"));
    return;
  }
  rzero = mq135_sensor.getRZero();
  correctedRZero = mq135_sensor.getCorrectedRZero(temperature, humidity);
  resistance = mq135_sensor.getResistance();
  ppm = mq135_sensor.getPPM();
  correctedPPM = mq135_sensor.getCorrectedPPM(temperature, humidity);
  /*Serial.print("MQ135 RZero: ");
  Serial.print(rzero);
  Serial.print("\t Corrected RZero: ");
  Serial.print(correctedRZero);
  Serial.print("\t Resistance: ");
  Serial.print(resistance);
  Serial.print("\t PPM: ");
  Serial.print(ppm);
  Serial.print("ppm");
  Serial.print("\t Corrected PPM: ");
  Serial.print(correctedPPM);
  Serial.print("ppm");
  Serial.print("\t Temperature: ");
  Serial.print(temperature);
  Serial.print("°C");
  Serial.print("\t Humidity: ");
  Serial.print(humidity);
  Serial.print("%");*/
  RemoteXY_Handler ();
  kamera_x.write(map(RemoteXY.camera_x, -100, 100, 45, 135));
  kamera_y.write(map(RemoteXY.camera_y, -100, 100, 135, 45));
  seconds = map(RemoteXY.slider, 0, 100, 0, 10000); //в будущем возможна замена
  if (z < seconds) {
    digitalWrite(PIN_IN1_3, HIGH);
    digitalWrite(PIN_IN2_3, LOW);
    for (int i = seconds - z; i < seconds; i++) {
      delayMicroseconds(999);
    }
    digitalWrite(PIN_IN1_3, LOW);
    z = seconds;
  } else if (z > seconds) {
    digitalWrite(PIN_IN1_3, LOW);
    digitalWrite(PIN_IN2_3, HIGH);
    for (int i = z - seconds; i < z; i++) {
      delayMicroseconds(999);
    }
    digitalWrite(PIN_IN2_3, LOW);
    z = seconds;
  }
  switch (RemoteXY.drive_y) {
    case -100 ... -1:
      yVal = map(-RemoteXY.drive_y, 0, 100, 0, 255);
        digitalWrite(PIN_IN1_1, LOW);
        digitalWrite(PIN_IN2_1, HIGH);
        digitalWrite(PIN_IN3_1, LOW);
        digitalWrite(PIN_IN4_1, HIGH);
        digitalWrite(PIN_IN1_2, LOW);
        digitalWrite(PIN_IN2_2, HIGH);
        digitalWrite(PIN_IN3_2, LOW);
        digitalWrite(PIN_IN4_2, HIGH);
      break;
    case 1 ... 100:
      yVal = map(RemoteXY.drive_y, 0, 100, 0, 255);
      digitalWrite(PIN_IN1_1, HIGH);
      digitalWrite(PIN_IN2_1, LOW);
      digitalWrite(PIN_IN3_1, HIGH);
      digitalWrite(PIN_IN4_1, LOW);
      digitalWrite(PIN_IN1_2, HIGH);
      digitalWrite(PIN_IN2_2, LOW);
      digitalWrite(PIN_IN3_2, HIGH);
      digitalWrite(PIN_IN4_2, LOW);
      break;
    case 0:
      yVal = 0;
      digitalWrite(PIN_IN1_1, LOW);
      digitalWrite(PIN_IN2_1, LOW);
      digitalWrite(PIN_IN3_1, LOW);
      digitalWrite(PIN_IN4_1, LOW);
      digitalWrite(PIN_IN1_2, LOW);
      digitalWrite(PIN_IN2_2, LOW);
      digitalWrite(PIN_IN3_2, LOW);
      digitalWrite(PIN_IN4_2, LOW);
      break;
  }
  switch (RemoteXY.drive_x) {
    case -100 ... -1:
      xVal = map(-RemoteXY.drive_x, 0, 100, 0, 255);
      powerR = yVal + xVal;
      if (powerR > 255) powerR = 255;
      analogWrite(PIN_ENB_1, powerR);
      analogWrite(PIN_ENB_2, powerR);
      powerL = yVal - xVal;
      if (powerL < 0) powerL = 0;
      analogWrite(PIN_ENA_1, powerL);
      analogWrite(PIN_ENA_2, powerL);
      break;
    case 1 ... 100:
      xVal = map(RemoteXY.drive_x, 0, 100, 0, 255);
      powerR = yVal - xVal;
      if (powerR < 0) powerR = 0;
      analogWrite(PIN_ENB_1, powerR);
      analogWrite(PIN_ENB_2, powerR);
      powerL = yVal + xVal;
      if (powerL > 255) powerL = 255;
      analogWrite(PIN_ENA_1, powerL);
      analogWrite(PIN_ENA_2, powerL);
      break;
    case 0:
      analogWrite(PIN_ENA_1, yVal);
      analogWrite(PIN_ENB_1, yVal);
      analogWrite(PIN_ENA_2, yVal);
      analogWrite(PIN_ENB_2, yVal);
      break;
  }
  switch (RemoteXY.selector) {
    case 0:
      if (temperature > 25 and humidity < 16) {
        String result = "Жуки!";
        result.toCharArray(RemoteXY.info, 17);
      } else if (temperature > 25 and humidity > 15) {
        String result = "Плесень!";
        result.toCharArray(RemoteXY.info, 17);
      } else {
        String result = "Норма";
        result.toCharArray(RemoteXY.info, 17);
      }
      break;
    case 1:
      dtostrf(temperature, 4, 2, RemoteXY.info);
      break;
    case 2:
      dtostrf(humidity, 5, 2, RemoteXY.info);
      break;
    case 3:
      dtostrf(correctedPPM, 6, 2, RemoteXY.info);
      break;    
  }
}

Скорее всего беда с прерываниями - это основная причина дёрганий серв с библиотекой Servo.h
Осциллограф покажет - идут ли импульсы на серву равные или болтает их.

Если нет осциллографа, то можно попробовать загрузить скетч на основе простейшего примера для серв, где кроме них нет ничего и они выставлены на фиксированный угол. Если дрожание есть и в этом случае, то дело скорее всего не в прерываниях.

Это так, примеры работают стабильно
Теперь я абсолютно не понимаю, в чем проблема. Преобразуется значение нормально, прерываний нет. Всякие условия для выполнения задания (типа значение сейчас должно быть на 10, 15, 25 больше, чем при прошлой итерации) не помогают

RemoteXY внутри себя может придерживать прерывания.

Совсем идеально было бы перестроить один из таймеров, и задавать длительность импульса управления сервой через analogWrite. Но тогда с моторами нужно как-то это всё увязать.
Или использовать внешний модуль управления сервомеханизмами.

Вы используете SoftwareSerial, а значит блокируете прерывания на ощутимое время. Если можно, перейдите на Хард сериал.

Помогло!! Победа!! Спасибо Вам большое, все утро с этим разбирался

@Dead_Cat20
что именно помогло, напишите.
Это поможет другим участникам, что будут читать тему после вас.

По совету пользователя Upper: SoftwareSerial блокирует прерывания ШИМ, что и вызвало подергивания сервоприводов. Решение: использовать HardwareSerial, в таком случае рекомендую Arduino Mega, тк у нее есть дополнительные 3 пары TX-RX, не блокирующие загрузку скетча на плату

2 лайка