Skip to contentSkip to main navigation Skip to footer

RC машинка. Мотор и сервопривод

Задача


  • Перевести магазинную радиоуправляемую машинку на управление с Arduino по радио 433 МГц
    • Руль – сервомашинка
    • Привод колёс – коллекторный мотор
  • В рамках данного примера брать сигнал управления с руля и педалей для компьютера
    • Потенциометр – руль
    • Потенциометр – газ
    • Потенциометр – тормоз (задний ход)

Базовые уроки


Подключение


Базовая схема передатчика и приёмника:

Библиотеки


  • Gyver433 – библиотека для радиомодулей
  • Servo – встроенная библиотека для управления сервоприводом

Программа


Логика работы следующая: передатчик измеряет показания потенциометров, преобразует в нужный диапазон для управления и отправляет по радио 10 раз в секунду. Приёмник принимает и раздаёт сигналы на мотор и сервопривод.

Библиотека Gyver433 позволяет передавать любые данные, поэтому для удобства и наглядности сделаем структуру. Для скорости используем int, значения в нашем случае не будут превышать -255.. 255. Для поворота руля серво хватит byte, так как диапазон поворота серво составляет 0.. 180 градусов. Структура выглядит так:

// формат пакета для отправки
struct Data {
  int16_t speed;  // -255.. 255
  uint8_t steer;  // 0..180
};

Передатчик


Зададим константами пины подключения, для удобства дальнейшей работы:

#define RADIO_PIN 2     // пин радио
#define THROT_PIN A0    // газ
#define BRAKE_PIN A1    // тормоз
#define STEER_PIN A2    // руль

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

// пределы сигналов с датчиков
#define STEER_MIN 1023  // руль
#define STEER_MAX 0
#define THROT_MIN 330   // газ
#define THROT_MAX 0
#define BRAKE_MIN 190   // тормоз
#define BRAKE_MAX 1023

Эти значения нужно будет измерить и исправить для своего железа.

У сервы и мотора тоже есть пределы, предлагаю ввести их на стороне передатчика, чтобы для настройки прошивать только его и не разбирать машинку:

// пределы поворота серво
#define SERVO_MIN (90-40)
#define SERVO_MAX (90+40)

// пределы газа и тормоза
#define MOTOR_MAX 250
#define MOTOR_MIN 100

Данные значения опять же настраиваются под своё железо и предпочтения.

Затем подключаем библиотеку и настраиваем радио. Я буду использовать FAST режим и скорость 3000, “зелёные” радиомодули отлично работают в таком режиме.

#define G433_SPEED 3000
#define G433_FAST
#include <Gyver433.h>
Gyver433_TX<RADIO_PIN> tx;  // указали пин

В блоке setup() запустим Serial для вывода сигналов с потенциометров. После настройки Serial можно будет отключить (закомментировать строку):

void setup() {
  Serial.begin(9600);
}

В блоке loop() последовательно выполняем следующие действия:

Читаем аналоговый сигнал со всех трёх элементов управления:

// читаем сигналы с управления
int steer = analogRead(STEER_PIN);
int throt = analogRead(THROT_PIN);
int brake = analogRead(BRAKE_PIN);

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

// для отладки
Serial.print(steer);
Serial.print(',');
Serial.print(throt);
Serial.print(',');
Serial.println(brake);

Далее приводим эти значения к указанным диапазонам для серво и мотора, и на всякий случай ограничиваем:

// приводим
steer = map(steer, STEER_MIN, STEER_MAX, SERVO_MIN, SERVO_MAX);
throt = map(throt, THROT_MIN, THROT_MAX, 0, MOTOR_MAX);
brake = map(brake, BRAKE_MIN, BRAKE_MAX, 0, MOTOR_MIN);

// ограничиваем
steer = constrain(steer, SERVO_MIN, SERVO_MAX);
throt = constrain(throt, 0, MOTOR_MAX);
brake = constrain(brake, 0, MOTOR_MIN);

Осталось создать экземпляр структуры, заполнить его данными и отправить по радио. С педалями газ-тормоз поступим следующим образом: вычтем сигнал тормоза (заднего хода) из сигнала “газа”. Это позволит допускать одновременное нажатие педалей, и работать оно будет вполне реалистично: дали газу, чуть поджали тормоз – машинка стала ехать медленнее =)

// готовим к отправке
Data data;
data.steer = steer;

// газ минус тормоз
// Допускает одновременное нажатие
data.speed = throt - brake;

// отправляем
tx.sendData(data);

Слишком часто отправлять нет смысла, поэтому введём задержку на 10 мс (отправка 100 раз в секунду). Если вы будете модифицировать данный проект и задержка будет мешать – отправку всегда можно сделать по таймеру на millis() (см. уроки).

// 100 раз в секунду
delay(10);

Приёмник


В приёмной части тоже задаём пины:

#define RADIO_PIN 2   // пин радио
#define SERVO_PIN 5   // пин серво
#define MOTOR_A 3     // пин мотора
#define MOTOR_B 11    // пин мотора

Также я ввёл такую настройку, как минимальная скорость мотора. Например мы знаем, что машинка начинает ехать при значении сигнала ШИМ больше 45. В дальнейшей программе диапазон управления будет отмасштабирован таким образом, чтобы при небольшом “нажатии на педаль” значение ШИМ росло с 45 (или другого значения):

// мин. скорость мотора (0-255)
// для большей резкости управления
#define MIN_DUTY 45

Далее подключаем библиотеку радио и настраиваем на такую же скорость, как у приёмника. Также у приёмника есть буфер, мы будем передавать 3 байта данных (int + byte), размер буфера ставим 3:

#define G433_SPEED 3000
#include <Gyver433.h>
Gyver433_RX<RADIO_PIN, 3> rx; // размер буфера 3 байта

Подключаем библиотеку серво и создаём себе одну серву:

#include <Servo.h>
Servo servo;

Для мотора я написал отдельный класс, который использует два ШИМ пина для равномерного управления. Он просто размещается в программе:

Класс Motor
// мини "библиотека" мотора
class Motor {
  public:
    Motor(const uint8_t pinA, const uint8_t pinB, const uint8_t minD = 0) :
      _pinA(pinA), _pinB(pinB), _minD(minD) {
      pinMode(_pinA, OUTPUT);
      pinMode(_pinB, OUTPUT);
    }

    void run(int speed) {
      speed = constrain(speed, -255, 255);
      speed = (long)speed * (255 - _minD) / 255;

      if (speed > 0) {
        digitalWrite(_pinA, 1);
        analogWrite(_pinB, 255 - (speed + _minD));
      } else if (speed < 0) {
        digitalWrite(_pinB, 1);
        analogWrite(_pinA, 255 - (-speed + _minD));
      } else {
        digitalWrite(_pinA, 0);
        digitalWrite(_pinB, 0);
      }
    }

  private:
    const uint8_t _pinA, _pinB, _minD;
};

По сути, это мини библиотека. Создаём себе мотор с указанием пинов и минимального сигнала ШИМ:

Motor motor(MOTOR_A, MOTOR_B, MIN_DUTY);

В блоке setup() включаем Serial (если нужен для отладки, в рабочем проекте можно выключить), подключаем серво и разгоняем частоту ШИМ как в этом уроке:

void setup() {
  Serial.begin(9600);

  // подрубаем серво
  servo.attach(SERVO_PIN);
  servo.write(90);

  // разгоняем ШИМ чтобы не гудело
  // Пины D3 и D11 - 8 кГц
  TCCR2B = 0b00000010;  // x8
  TCCR2A = 0b00000011;  // fast pwm
}

В блоке loop() нам нужно опрашивать радио и раздавать сигналы управления на “железо”. Но начнём мы с другого: представьте, что вы ехали на полном газу, и тут вдруг пропал сигнал с радио. Машинка продолжит движение с той же скоростью. Чтобы обезопасить себя от этого, введём таймаут: если сигнала с радио не было дольше определённого количества времени – останавливаем мотор. На RC сленге это называется failsafe:

// таймаут приёма данных
static uint32_t tmr;
// если сигнала с радио нет 200 мс
// выключаем мотор
if (millis() - tmr > 200) {
  motor.run(0);
  tmr = millis();
}

Далее нужно принять сигнал с радио (подробнее см. пример в библиотеке Gyver433). Если данные успешно приняты – сбрасываем таймаут и раздаём сигналы управления:

// приём по радио
if (rx.tick()) {
  Data data;

  // если чтение без ошибок
  if (rx.readData(data)) {
    tmr = millis(); // сброс таймаута
    servo.write(data.steer);
    motor.run(data.speed);
    /*
      // для отладки
      Serial.print(data.steer);
      Serial.print(',');
      Serial.println(data.speed);
    */
  }
}

Также можно вывести отладочную информацию в порт, проверить, работает ли радио. В рабочей программе отладку можно закомментировать.

Полный код программы


Передатчик
#define RADIO_PIN 2     // пин радио
#define THROT_PIN A0    // газ
#define BRAKE_PIN A1    // тормоз
#define STEER_PIN A2    // руль

// пределы сигналов с датчиков
#define STEER_MIN 1023  // руль
#define STEER_MAX 0
#define THROT_MIN 330   // газ
#define THROT_MAX 0
#define BRAKE_MIN 190   // тормоз
#define BRAKE_MAX 1023

// пределы поворота серво
#define SERVO_MIN (90-40)
#define SERVO_MAX (90+40)

// пределы газа и тормоза
#define MOTOR_MAX 250
#define MOTOR_MIN 100

//
#define G433_SPEED 3000
#define G433_FAST
#include <Gyver433.h>
Gyver433_TX<RADIO_PIN> tx;  // указали пин

// формат пакета для отправки
struct Data {
  int16_t speed;  // -255.. 255
  uint8_t steer;  // 0..180
};

void setup() {
  Serial.begin(9600);
}

void loop() {
  // читаем сигналы с управления
  int steer = analogRead(STEER_PIN);
  int throt = analogRead(THROT_PIN);
  int brake = analogRead(BRAKE_PIN);

  // для отладки
  Serial.print(steer);
  Serial.print(',');
  Serial.print(throt);
  Serial.print(',');
  Serial.println(brake);

  // приводим
  steer = map(steer, STEER_MIN, STEER_MAX, SERVO_MIN, SERVO_MAX);
  throt = map(throt, THROT_MIN, THROT_MAX, 0, MOTOR_MAX);
  brake = map(brake, BRAKE_MIN, BRAKE_MAX, 0, MOTOR_MIN);

  // ограничиваем
  steer = constrain(steer, SERVO_MIN, SERVO_MAX);
  throt = constrain(throt, 0, MOTOR_MAX);
  brake = constrain(brake, 0, MOTOR_MIN);

  // готовим к отправке
  Data data;
  data.steer = steer;

  // газ минус тормоз
  // Допускает одновременное нажатие
  data.speed = throt - brake;

  // отправляем
  tx.sendData(data);

  // 100 раз в секунду
  delay(10);
}
Приёмник
#define RADIO_PIN 2   // пин радио
#define SERVO_PIN 5   // пин серво
#define MOTOR_A 3     // пин мотора
#define MOTOR_B 11    // пин мотора

// мин. скорость мотора (0-255)
// для большей резкости управления
#define MIN_DUTY 45

//
#define G433_SPEED 3000
#include <Gyver433.h>
Gyver433_RX<RADIO_PIN, 3> rx; // размер буфера 3 байта

#include <Servo.h>
Servo servo;

// формат пакета для приёма
struct Data {
  int16_t speed;  // -255.. 255
  uint8_t steer;  // 0..180
};

// мини "библиотека" мотора
class Motor {
  public:
    Motor(const uint8_t pinA, const uint8_t pinB, const uint8_t minD = 0) :
      _pinA(pinA), _pinB(pinB), _minD(minD) {
      pinMode(_pinA, OUTPUT);
      pinMode(_pinB, OUTPUT);
    }

    void run(int speed) {
      speed = constrain(speed, -255, 255);
      speed = (long)speed * (255 - _minD) / 255;

      if (speed > 0) {
        digitalWrite(_pinA, 1);
        analogWrite(_pinB, 255 - (speed + _minD));
      } else if (speed < 0) {
        digitalWrite(_pinB, 1);
        analogWrite(_pinA, 255 - (-speed + _minD));
      } else {
        digitalWrite(_pinA, 0);
        digitalWrite(_pinB, 0);
      }
    }

  private:
    const uint8_t _pinA, _pinB, _minD;
};

Motor motor(MOTOR_A, MOTOR_B, MIN_DUTY);

void setup() {
  Serial.begin(9600);

  // подрубаем серво
  servo.attach(SERVO_PIN);
  servo.write(90);

  // разгоняем ШИМ чтобы не гудело
  // Пины D3 и D11 - 8 кГц
  TCCR2B = 0b00000010;  // x8
  TCCR2A = 0b00000011;  // fast pwm
}

void loop() {
  // таймаут приёма данных
  static uint32_t tmr;
  // если сигнала с радио нет 200 мс
  // выключаем мотор
  if (millis() - tmr > 200) {
    motor.run(0);
    tmr = millis();
  }

  // приём по радио
  if (rx.tick()) {
    Data data;

    // если чтение без ошибок
    if (rx.readData(data)) {
      tmr = millis(); // сброс таймаута
      servo.write(data.steer);
      motor.run(data.speed);
      /*
        // для отладки
        Serial.print(data.steer);
        Serial.print(',');
        Serial.println(data.speed);
      */
    }
  }
}

Возможные доработки


Можно оставить только одну “педаль” – газ. В среднем положении потенциометра – стоп, вперёд и назад – соответственно вперёд и назад. Сделать это очень просто: убираем из кода всё что связано с педалью brake, а педаль газа масштабируем как

throt = map(throt, THROT_MIN, THROT_MAX, -MOTOR_MIN, MOTOR_MAX);
throt = constrain(throt, -MOTOR_MIN, MOTOR_MAX);

И отправляем по радио как

data.speed = throt;

И всё!

Видео


Полезный пример?

Похожие примеры
6 Комментариев
  • Почему у меня не работает? Передатчик передает, но приемник нивкакую не хочет принимать, делал все по инструкции, вообще никаких шевелений со стороны приемника. Ардуинки местами менял, приемник-передатчик менял на новые, результата НОЛЬ

  • Никак не могу заменить серво на 2 реле. Вот хоть так, хоть так.. Серво нагрузку не выдерживает и ломается, нужно стопорить правые или левые колеса и тогда поворот. Я перевел это все на джойстик. Вперед назад стоп работает четко. Серво на реле не выходит.
    Может еще создать один класс мотор?
    Тогда бы такая схема сработала:
    If(speed1 > 200)
    {
    digitalWrite(RELE_A, 1);
    digitalWrite(RELE_B, 0);
    }
    else if(speed1 < 200)
    {
    digitalWrite(RELE_A, 0);
    digitalWrite(RELE_B, 1);
    }else{
    digitalWrite(RELE_A, 0);
    digitalWrite(RELE_A, 0);
    }
    А проблема в приеме-передаче. Я нигде не могу найти расшифровку этих команд
    servo.write(data.steer);
    motor.run(data.speed);
    Почему в передаче все просто:

    Data data;
    data.steer = steer;
    data.speed = throt;
    tx.sendData(data);

    А в приеме этих же данных я не могу подставить ничего, кроме servo.write ?
    Я изучаю ардуино сам и спросить не у кого.

    • Спасибо. Сам разобрался. Думаю не правильно, но все работает четко и едет и поволрачивает как танк . Сделал еще класс, нигде не учился, аж самому не верится но работает, зуб даю.

      //приемник моторы 4 два слева два справа
      //джойстик вперед стоп назад
      //джойстик вправо влево
      //когда поворот реле меняют полюса
      //у двух моторов и поворот

      #define RADIO_PIN 2 // пин радио
      #define MOTOR_A 3 // пин мотора
      #define MOTOR_B 11 // пин мотора
      #define MOTOR_AA 9 //реле1
      #define MOTOR_BA 10 //реле2
      #define MIN_DUTY 20
      #define MIN_DUTYA 20

      #define G433_SPEED 4000 // (3000)
      #include
      Gyver433_RX rx; // размер буфера (3) 4 байта

      // формат пакета для приёма
      struct Data {
      int16_t speed; // -255.. 255
      int16_t speed1; // -255.. 255
      };

      // мини “библиотека” мотора
      class Motor {
      public:
      Motor(const uint8_t pinA, const uint8_t pinB, const uint8_t minD = 0) :
      _pinA(pinA), _pinB(pinB), _minD(minD) {
      pinMode(_pinA, OUTPUT);
      pinMode(_pinB, OUTPUT);
      }

      void run(int speed) {
      speed = constrain(speed, -255, 255);
      speed = (long)speed * (255 – _minD) / 255;

      if (speed > 0) {
      digitalWrite(_pinA, 1);
      analogWrite(_pinB, 255 – (speed + _minD));
      } else if (speed 100) {
      digitalWrite(_pinAA, 1);

      } else if (speed1 200) {
      motor.run(0);
      tmr = millis();
      }

      // приём по радио
      if (rx.tick()) {
      Data data;

      // если чтение без ошибок
      if (rx.readData(data)) {
      tmr = millis(); // сброс таймаута
      motors.run(data.speed1);
      motor.run(data.speed);

      // для отладки
      Serial.print(data.speed1);
      Serial.print(‘,’);
      Serial.println(data.speed);

      }
      }
      }

      #define RADIO_PIN 2 // пин радио
      #define THROT_PIN A0 // вперед стоп назад
      #define KEKS_PIN A2 // вправо влево

      // пределы сигналов с датчиков
      #define THROT_MIN 1023 //
      #define THROT_MAX 0
      #define KEKS_MIN 0 // поворот 2 реле
      #define KEKS_MAX 1023

      // пределы газа и тормоза
      #define MOTOR_MAX 250
      #define MOTOR_MIN 250
      #define MOTORS_MIN 250 //реле1
      #define MOTORS_MAX 250 //реле2

      //
      #define G433_SPEED 4000 // 3000
      #define G433_FAST
      #include
      Gyver433_TX tx; // указали пин

      // формат пакета для отправки
      struct Data {
      int16_t speed; // -255.. 255
      int16_t speed1; // -255.. 255
      };

      void setup() {
      Serial.begin(9600);
      }

      void loop() {
      // читаем сигналы
      int throt = analogRead(THROT_PIN); //джойстик
      int keks = analogRead(KEKS_PIN); //джойстик

      // для отладки
      Serial.print(keks);
      Serial.print(‘,’);
      Serial.println(throt);

      // приводим

      throt = map(throt, THROT_MIN, THROT_MAX, -MOTOR_MIN, MOTOR_MAX);
      keks = map(keks, KEKS_MIN, KEKS_MAX, MOTORS_MAX, -MOTORS_MIN);

      // готовим к отправке
      Data data;
      data.speed1 = keks;
      data.speed = throt;

      // отправляем
      tx.sendData(data);

      Serial.print(data.speed1);
      Serial.print(‘,’);
      Serial.println(data.speed);

      // 100 раз в секунду
      delay(10);
      }

      • что то там не то
        еще раз
        //приемник моторы 4 два слева два справа
        //джойстик вперед стоп назад
        //джойстик вправо влево
        //когда поворот реле меняют полюса
        //у двух моторов и поворот

        #define RADIO_PIN 2 // пин радио
        #define MOTOR_A 3 // пин мотора
        #define MOTOR_B 11 // пин мотора
        #define MOTOR_AA 9 //реле1
        #define MOTOR_BA 10 //реле2
        #define MIN_DUTY 20
        #define MIN_DUTYA 20

        #define G433_SPEED 4000 // (3000)
        #include
        Gyver433_RX rx; // размер буфера (3) 4 байта

        // формат пакета для приёма
        struct Data {
        int16_t speed; // -255.. 255
        int16_t speed1; // -255.. 255
        };

        // мини “библиотека” мотора
        class Motor {
        public:
        Motor(const uint8_t pinA, const uint8_t pinB, const uint8_t minD = 0) :
        _pinA(pinA), _pinB(pinB), _minD(minD) {
        pinMode(_pinA, OUTPUT);
        pinMode(_pinB, OUTPUT);
        }

        void run(int speed) {
        speed = constrain(speed, -255, 255);
        speed = (long)speed * (255 – _minD) / 255;

        if (speed > 0) {
        digitalWrite(_pinA, 1);
        analogWrite(_pinB, 255 – (speed + _minD));
        } else if (speed 100) {
        digitalWrite(_pinAA, 1);

        } else if (speed1 200) {
        motor.run(0);
        tmr = millis();
        }

        // приём по радио
        if (rx.tick()) {
        Data data;

        // если чтение без ошибок
        if (rx.readData(data)) {
        tmr = millis(); // сброс таймаута
        motors.run(data.speed1);
        motor.run(data.speed);

        // для отладки
        Serial.print(data.speed1);
        Serial.print(‘,’);
        Serial.println(data.speed);

        }
        }
        }

        #define RADIO_PIN 2 // пин радио
        #define THROT_PIN A0 // вперед стоп назад
        #define KEKS_PIN A2 // вправо влево

        // пределы сигналов с датчиков
        #define THROT_MIN 1023 //
        #define THROT_MAX 0
        #define KEKS_MIN 0 // поворот 2 реле
        #define KEKS_MAX 1023

        // пределы газа и тормоза
        #define MOTOR_MAX 250
        #define MOTOR_MIN 250
        #define MOTORS_MIN 250 //реле1
        #define MOTORS_MAX 250 //реле2

        //
        #define G433_SPEED 4000 // 3000
        #define G433_FAST
        #include
        Gyver433_TX tx; // указали пин

        // формат пакета для отправки
        struct Data {
        int16_t speed; // -255.. 255
        int16_t speed1; // -255.. 255
        };

        void setup() {
        Serial.begin(9600);
        }

        void loop() {
        // читаем сигналы
        int throt = analogRead(THROT_PIN); //джойстик
        int keks = analogRead(KEKS_PIN); //джойстик

        // для отладки
        Serial.print(keks);
        Serial.print(‘,’);
        Serial.println(throt);

        // приводим

        throt = map(throt, THROT_MIN, THROT_MAX, -MOTOR_MIN, MOTOR_MAX);
        keks = map(keks, KEKS_MIN, KEKS_MAX, MOTORS_MAX, -MOTORS_MIN);

        // готовим к отправке
        Data data;
        data.speed1 = keks;
        data.speed = throt;

        // отправляем
        tx.sendData(data);

        Serial.print(data.speed1);
        Serial.print(‘,’);
        Serial.println(data.speed);

        // 100 раз в секунду
        delay(10);
        }

Оставить комментарий

Ваш адрес email не будет опубликован.