Программное обеспечение робота (Arduino ESP32) на шаговых моторах с памятью команд - траектории движения
- Описание сборки механики робота
- Описание сборки электроники робота
- Программное обеспечение робота
- Архив с программой (Arduino) для робота
- Ссылка на ролик 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;
}