Интернет-лаборатория роботов ZiZiBOT.RU

Проектирование и разработки в области робототехники и автоматизации технологических процессов. Производство готовых роботов и конструкторов для творчества. Консультации и обучение по электронике и программированию.

г. Юрга,
ул.Ленинградская 38/83

+7 923-503-6074

Программное обеспечение робота (Arduino ESP32) на шаговых моторах с памятью команд - траектории движения

  1. Описание сборки механики робота
  2. Описание сборки электроники робота
  3. Программное обеспечение робота
  4. Архив с программой (Arduino) для робота
  5. Ссылка на ролик Youtube по данному роботу
алгоритм программы алгоритм программы

 

Робот программировался из среды Arduino IDE.

 

Ниже приводится головной файл проекта, на момент написание статьи его имя "machinePrefspZh16microstep2.ino"

Также в проекте присутствуют файлы:

irq_robot.h - описываются функции обработки прерываний (управления шагами двигателей).

motorstep.h - функции управления состоянием шаговых моторов.

move_case.h - обработчик Bluetooth команд.

robots_way.h - запись/чтение пути в/из FLASH-память контроллера.

Эти файлы представляют из себя одну программу, которая для лучшего понимания и удобства разбита на несколько частей.

К программе подключены две библиотеки Preferences.h и BluetoothSerial.h.

Preferences.h отвечает за запись чтение информации из/в встроенную в контроллер ESP32  FLASH память.

BluetoothSerial.h отвечает за обмен данными по каналу Bluetooth.

Полный программный код расположен в архиве



#include < Preferences.h >
#include < BluetoothSerial.h >


#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif
BluetoothSerial SerialBT;

Preferences preferences;

hw_timer_t * timerL = NULL;
hw_timer_t * timerR = NULL;

#include "robots_way.h"
#include "motorstep.h"
#include "move_case.h"

bool bool_start = false; //Начало работы робота
bool LOW_POWER = false;
//============================================================================================================
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//============================================================================================================

#include "irq_robot.h" //Функции обработки прерываний - управление моторами


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

  pinMode(32, INPUT);
  pinMode(33, INPUT);
  pinMode(35, INPUT);
  preferences.begin("robot_storage", false); //Открываем хранилище с именем  "robot_storage"

  //Инициализируем моторы
  setup_motor_system();


  //_stop();
  Serial.println("Do");
  motor_off();
  num_commands = 0;
  LOW_POWER = false;
  max_div_sp = max_f_div_sp;
  Steps_For_maxspeedf = Steps_For_maxspeedS();
  max_div_sp = max_t_div_sp;
  Steps_For_maxspeedt = Steps_For_maxspeedS();

  Serial.print("Steps_For_maxspeed"); Serial.println(Steps_For_maxspeed);

}
//============================================================================================================
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//============================================================================================================

void loop()
{
  const int delta = 5;
  const int LOW_Power = 2300;
  const int button_Do = 176; //Нажато воспроизведение
  const int button_Write = 1776; //Нажата запись
  int x;
  x = analogRead(35);
  if ((x < LOW_Power) || LOW_POWER) {
    LOW_POWER = true;
    motor_off();
    return;
  }

  if (!bool_start)
  {

    // Фиксируем нажатие кнопки
    int button = analogRead(32);
    if ((button > (button_Do - delta)) && (button < (button_Do + delta)))
    {
      motor_on();
      delay(50);
      // Use 1st timer of 4 (counted from zero).
      // Set 80 divider for prescaler (see ESP32 Technical Reference Manual for more
      // info).
      timerL = timerBegin(0, 80, true);
      timerR = timerBegin(1, 80, true);

      // Attach onTimer function to our timer.
      timerAttachInterrupt(timerL, &onTimerL, true);
      timerAttachInterrupt(timerR, &onTimerR, true);

      // Set alarm to call onTimer function (value in microseconds).
      // Repeat the alarm (third parameter)
      timerAlarmWrite(timerL, speed_stepL, true);
      timerAlarmWrite(timerR, speed_stepR, true);

      // Таймеры пока отключены
      //timerAlarmEnable(timerL);
      //timerAlarmEnable(timerR);
      timer_volumeL  = false;
      timer_volumeR  = false;


      RECORD_T = false; //Начать воспроизведение
      bool_start = true;
      command_counter = preferences.getUInt("numb_of_com", 0); // Сколько у нас команд...
    }
    else if ((button > (button_Write - delta)) && (button < (button_Write + delta)) )
    {
      // Use 1st timer of 4 (counted from zero).
      // Set 80 divider for prescaler (see ESP32 Technical Reference Manual for more
      // info).
      timerL = timerBegin(0, 80, true);
      timerR = timerBegin(1, 80, true);
      // Attach onTimer function to our timer.
      timerAttachInterrupt(timerL, &onTimerL_rec, true);
      timerAttachInterrupt(timerR, &onTimerR_rec, true);
      // Set alarm to call onTimer function (value in microseconds).
      // Repeat the alarm (third parameter)
      timerAlarmWrite(timerL, speed_stepL, true);
      timerAlarmWrite(timerR, speed_stepR, true);
      // Таймеры пока отключены
      //timerAlarmEnable(timerL);
      //timerAlarmEnable(timerR);
      timer_volumeL  = false;
      timer_volumeR  = false;

      SerialBT.begin("MONSTERESP32"); //Bluetooth device name
      flagTimeOff = millis() + 50;
      RECORD_T = true; //Начать запись
      bool_start = true;
      num_commands = 0;
      motor_on();
    }
  }
  else
  {
    if (RECORD_T) //Начать запись
    {
      move_case();
      if (flagTimeOff < millis())
      {
        _stop();
        flagTimeOff += 50;
      }
    }
    else
    {
      run_trajectory();
    }
  }
}
//============================================================================================================
//============================================================================================================
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//============================================================================================================
//============================================================================================================
void run_trajectory()//Проиграть траекторию
{
  // RECORD_T = false;
  if ((counter_stepL == 0) && (counter_stepR == 0)) // Если закончена текущая команда
  {
    if (command_counter > num_commands)
    {
      num_commands++;
      read_from_pref(num_commands);
      if (timer_volumeL)
      {
        timerAlarmDisable(timerL);
        timer_volumeL  = false;
      }
      if (timer_volumeR)
      {
        timerAlarmDisable(timerR);
        timer_volumeR  = false;
      }
      Set_trajectory(); //Устанавливаем данные
      Current_acceleration_braking();
      //delay(50); // Небольшая задержка
      //Включаем таймеры
      if (!timer_volumeL) {
        timerAlarmEnable(timerL);
        timer_volumeL = true;
      }
      if (!timer_volumeR) {
        timerAlarmEnable(timerR);
        timer_volumeR = true;
      }
    }
    else
      _stop();
  }
}
//============================================================================================================
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//============================================================================================================

uint32_t Steps_For_maxspeedS() //Расчет типового (не берется во внимание возможная длина участка движения в шагах) количества шагов необходимое на разгон / торможение
{
  uint32_t Steps = 0;
  uint32_t ZZ = 0, ZS;
  uint32_t speed__ = min_div_sp;
  while (speed__ > max_div_sp) //Если мы в начале движения
  {
    ZZ += (speed__ - max_div_spX);
    ZS = ZZ >> SDVIG;
    if (ZS) {
      speed__ -= ZS;
      ZZ -= ZS << SDVIG; //ZZ = 0;
    }
    Steps++;
  }
  return Steps; // количество шагов требуемое на разгон
}

uint32_t Steps_For_minspeedS() //Расчет типового (не берется во внимание возможная длина участка движения в шагах) количества шагов необходимое на разгон / торможение
{
  uint32_t Steps = 0;
  uint32_t ZZ = 0, ZS;
  uint32_t speed__ = max_div_sp;
  while (speed__ < min_div_sp) //Если мы в начале тормозного пути
  {
    ZZ += (speed__ - max_div_spX);
    ZS = ZZ >> SDVIG;
    if (ZS) {
      speed__ += ZS;
      ZZ -= ZS << SDVIG; //ZZ = 0;
    }
    Steps++;
  }
  return Steps; // количество шагов требуемое на разгон
}

//============================================================================================================
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//============================================================================================================

uint32_t Current_acceleration_braking() //Расчет для текущего участка количества шагов необходимое на разгон / торможение
{
  if (DIR_L_level == DIR_R_level) // Если будет воспроизведен поворот - уменьшим макс скорость
  {
    max_div_sp = max_t_div_sp; //Уменьшаем максимальную скорость для поворота
    Steps_For_maxspeed = Steps_For_maxspeedt;
  }
  else
  {
    max_div_sp = max_f_div_sp; //Увеличиваем максимальную скорость для прямолинейного движения
    Steps_For_maxspeed = Steps_For_maxspeedf;
  }
  speed_stepL = min_div_sp; // Это вдвое больше частоты шагов
  if (counter_stepL < (Steps_For_maxspeed * 2)) // Если количество шагов на участке меньше чем нужно для полного разгона торможения
  {
    // Найти :
    //Steps_to_speed_L - через сколько шагов закончить разгон
    //Steps_to_stop_L - когда начать торможение
    Steps_to_speed_L = Steps_to_stop_L = counter_stepL / 2; // делим участок на два через сдвиг т.к. беззнаковое целое
  }
  else
  {
    Steps_to_speed_L = Steps_to_stop_L = Steps_For_maxspeed;
  }
  Steps_to_speed_L = counter_stepL - Steps_to_speed_L;
  //Serial.print("Steps_to_speed_L="); Serial.println(Steps_to_speed_L);
  //Serial.print("Steps_to_stop_L="); Serial.println(Steps_to_stop_L);
  //Serial.print("counter_stepL="); Serial.println(counter_stepL);
  //Serial.print("Steps_For_maxspeed="); Serial.println(Steps_For_maxspeed);

  speed_stepR = min_div_sp; // Это вдвое больше частоты шагов

  if (counter_stepR < (Steps_For_maxspeed * 2)) // Если количество шагов на участке меньше чем нужно для полного разгона торможения
  {
    // Найти :
    //Steps_to_speed_R - через сколько шагов закончить разгон
    //Steps_to_stop_R - когда начать торможение
    Steps_to_speed_R = Steps_to_stop_R = counter_stepR / 2; // делим часток на два через сдвиг т.к. беззнаковое целое
  }
  else
  {
    Steps_to_speed_R = Steps_to_stop_R = Steps_For_maxspeed;
  }
  Steps_to_speed_R = counter_stepR - Steps_to_speed_R;
  Start_F_L = true;
  Start_F_R = true;
  Stop_F_L = false;
  Stop_F_R = false;
}


В начало к сборке механики

 

X

Написать сообщение:

Укажите свой номер телефона И e-mail для обратной связи
- e-mail
И
- номер телефона

Текст сообщения: