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

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

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

+7 923-503-6074

Ископаемое снова в деле
или самобалансирующий робот на Arduino Nano 

(учимся пользоваться одним прерыванием для управления несколькими шаговыми моторами)

От старых экспериментов с балансировкой для роботов у меня остался вот такой экспонат

http://zizibot.ru/articles/electronics/balancing-robot-on-step-motors/

 

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

Selfbalancing bot (esp32) - part 2

 

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

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

http://zizibot.ru/articles/programming/selfbalance-robot-esp32-bno055-mpu6050/

Все это время робот на шаговых моторах с Arduino Nano пылился в чулане, а алгоритмы совершенствовались и ускорялись. Пришло его время. Для него был адаптирован стандартный на начало лето 2019 года алгоритм балансировки. И... его производительности вполне хватило.


Модуль selfbalNANOmpu6050.ino
Основной запускающий программный модуль. В функции loop() производится расчет балансировки. Расчет аналогичен http://zizibot.ru/articles/programming/selfbalance-robot-esp32-bno055-mpu6050/ , добавлен блок обработки максимального наклона, после которого робот уже не сможет сбалансировать.

 

#include "defin.h"
#include "gyro_acsel.h"
#include "step_motor.h" // Обслуживание шаговых моторов
#include "fromsmart.h" // Обслуживание шаговых моторов
 
void setup() {
  setup_motor_system();
  motor_off();
 
  Serial.begin(115200);
  giroscop_setup();
  delay(100);
  Calc_CompensatorZ(2000);
  t0 = micros() - 5000;
  t2 = micros();
  Speed = 0;
  stepcoder_setup();
   motor_off();
}
 
void loop()
{
 
//    SpeedL = 1000;
//    SpeedR = 1000;
//    SetSpeed();
//    delay(1000);
//    return;
  t_period = 5000;
  //static int i = 0; // для торможения вывода
  unsigned long micros_;
  micros_ = micros();
  if (micros_ < t2) return;//Если период < t2 анализ. прошло 5000 если да, измеряем угловую скорость.
  BT_input();
  //Опрос гироприбора:
  dt = micros() - t0; // Длительность предыдущего периода регулирования.
  t0 += dt; //Точка начала нового периода регулирования.
  double Dt = double(dt) * 0.000001;
  // Нужно поймать угол, рядом с которым угловая скорость колеблется в районе нуля.
  Data_mpu6050();
  //Расчет угла по показаниям акселерометра
  Acsel = (atan2(AcX, AcZ));// * RAD_TO_DEG;
  Acsel_other_side = Acsel_other_side *0.99 + (atan2(AcY, AcZ))*0.01;
  // скорость угловая В радианах - падения
  Gyro = - (float(GyY) - CompensatorY)  * _1_d_131;
  //Комплементарный фильтр
  AcYsum = ONE_ALFA * (AcYsum + Gyro * Dt) + ALFA * Acsel;
 
  t2 = t0 + t_period;
//  static int ddd=0;
//  if(ddd==30)
//  {
//    Serial.println(AcYsum);
//    ddd=0;
//  }
//  ddd++;
  // Обработка падений для робота без возможности самостоятельного вставания.
  if ((fabs(AcYsum) > 0.57)||(fabs(Acsel_other_side)>0.57)) //Это порог в радианах 30 градусов
  {
    XSpeed = 0;
    OldCommandSpeed = 0;
    CommandSpeed = 0;
    Move = 0;
    SpeedL = 0;
    SpeedR = 0;
    OldXSpeedL = SpeedL;
    OldXSpeedR  = SpeedR;
    Speed = 0;
    if(!flag_crash) SetSpeed();
    //motor_off();
    flag_crash = true;
    return;
  }
  if (flag_crash)
  {
    Move = MoveStab;
    flag_crash = false;
  }
 
 
 
  //  XL = counter_stepL; counter_stepL = 0;
  //  XR = counter_stepR; counter_stepR = 0;
  XSpeedL =  SpeedL;// double(XL) / Dt; //Реальная скорость колес L //SpeedL;//
  XSpeedR =  SpeedR;//double(XR) / Dt; //Реальная скорость колес R //SpeedR;//
  XSpeed =  (XSpeedL + XSpeedR) * 0.5; //Реальная скорость колес
 
  Move += Dt * (XSpeed - OldCommandSpeed); //Это как раз пройденный путь
 
  //dSpeedL = (XSpeedL - OldXSpeedL) / Dt;
  //dSpeedR = (XSpeedR - OldXSpeedR) / Dt;
  OldXSpeed = XSpeed;
  double DT = CommandSpeed - OldCommandSpeed;
  DT = constrain(DT, -20, 20);
  OldCommandSpeed += DT;
  Speed = (AcYsum) *  8.0 * 800.0 * 20.0 / (2.0 * PI * 40.0) + 20.0  * Gyro * 800.0 / (40.0 * 2.0 * PI) + XSpeed + Move * 0.13 + (XSpeed - OldCommandSpeed) * 0.056; // - dSpeed * 0.002; //
  OldXSpeedL = SpeedL;
  OldXSpeedR  = SpeedR;
  DT = Turn - TurnOld;
  DT = constrain(DT, -20, 20);
  TurnOld += DT;
  SpeedL = Speed + TurnOld;// - dSpeedL * 0.002 + TurnOld;
  SpeedR = Speed - TurnOld;//- dSpeedR * 0.002 - TurnOld;
  Speed = (SpeedL + SpeedR) * 0.5;
  if (fabs(Speed) < 10.0) //Стабильное значение интегрального звена
  {
    MoveStab = MoveStab * 0.99 + 0.01 * Move;
  }
 
  // OldCommandSpeed = CommandSpeed; //
  //Speed - это число шагов в секунду
  //Нужно именять длительность периода подстраиваясь под шаг, чтобы был сделан хотябы один шаг.
  if (fabs(SpeedL) < 20.0)
  {
    SpeedL = 0;
  }
  else
  {
    SpeedL = constrain(SpeedL, -maxSpeed, maxSpeed);
  }
  if (fabs(SpeedR) < 20.0)
  {
    SpeedR = 0;
  }
  else
  {
    SpeedR = constrain(SpeedR, -maxSpeed, maxSpeed);
  }
  SetSpeed();
}

 

Модуль defin.h

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


 

const float alfa = 0.001;
 
//Переменные управления интервалами опроса гироприбора и интервалами управления
unsigned long dt;
unsigned long t0;
unsigned long t2;
 
double AcYsum;
// Длина окружности колеса 25см, 250мм
// Количество шагов на оборот - 200 (360/1.8)
// Количество микрошагов - 200*4 = 800 на оборот
// Количество срабатываний таймера для борота -3200 (1600*2) , так как шаг состоит из импульса, а импульс формируется за два такта
//==============================================================================
// Расстояние от оси колеса до гироприбора 225мм - потребуется для расчета пройденного до выпрямления расстояния
//==============================================================================
// Следует учесть, что чем ниже центр масс робота тем меньше момент инерции, тем быстрее нужно компенсировать наклон
// В то время, как чем выше центр масс тем дальше расстояние, которое нужно пройти роботу, чтобы компенсировать угол наклона...
 
const double _1_d_131 = 1.0 * DEG_TO_RAD / 131.0;
double Acsel = 0;
double Gyro = 0;
 
double Speed = 0;
double max_dSpeed = 60; //Максимальное изменение скорости/секунду (шагов в секунду)
double  maxSpeed = 3000; //Шагов в секунду
double Move = 0; //Пройденный путь.
double OldSpeed = 0;
double Acsel_other_side = 0;
 
double Kp, Kd, Ki; //Коэффициенты
 
unsigned long t_period;
double CommandSpeed = 0; //Скорость управляемого движения.
double OldCommandSpeed = 0; //Скорость управляемого движения из предыдущего опроса
double Turn = 0; //Поворот.
double TurnOld = 0; // Поворот из прошлой итерации.
double  SpeedR = 0;
double  SpeedL = 0;
double dSpeed = 0; //Ускорение на робота
double dSpeedL = 0; //Ускорение на колесо
double dSpeedR = 0; //Ускорение на колесо
 
double MoveStab = 0;
bool flag_crash = true;
bool flag_time_standup = true;// Флаг того, что робот встает.
unsigned long time_standup = 0; // Время на подъем робота
double XSpeed = 0; //Реальная скорость по шагам в моторах
double XSpeedL = 0; //Реальная скорость по шагам в моторах
double XSpeedR = 0; //Реальная скорость по шагам в моторах
double  OldXSpeed = 0; //Реальная скорость по шагам в моторах
double  OldXSpeedL = 0; //Реальная скорость по шагам в моторах
double  OldXSpeedR = 0; //Реальная скорость по шагам в моторах
//Нужно его использовать
bool flag_to_off_robot  = false; //Флаг выключения робота
long time_to_off_robot = 0; //Время на присадку робота


 

Модуль gyro_acsel.h

Инициализация гироприбора, чтения данных из гироприбора, устранение дрейфа нуля гироскопа.


 

 

// Библиотека для работы с протоколом I2C (порты A5/SCL и A4/SDA)
#include 
 
#define ALFA 0.002
#define ONE_ALFA 0.998//1.0-1.0E-3
 
// упрощеный I2C адрес нашего гироскопа/акселерометра MPU-6050.
const int MPU_addr = 0x68;
// переменные для хранения данных возвращаемых прибором.
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
double CompensatorZ, CompensatorX, CompensatorY,  CompensatorAcX;
unsigned long timer = 0;
/////////////////////////////////////////////////////////////////////////
//// Считывание данных с mpu6050
/////////////////////////////////////////////////////////////////////////
void Data_mpu6050()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  //Готовим для чтения регистры  с адреса 0x3B.
  Wire.endTransmission(false);
  // Запрос 14 регистров.
  Wire.requestFrom(MPU_addr, 14, true);
  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcX = Wire.read() << 8 | Wire.read();
  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcY = Wire.read() << 8 | Wire.read();
  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  AcZ = Wire.read() << 8 | Wire.read();
  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  Tmp = Wire.read() << 8 | Wire.read();
  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyX = Wire.read() << 8 | Wire.read();
  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyY = Wire.read() << 8 | Wire.read();
  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  GyZ = Wire.read() << 8 | Wire.read();
}
/////////////////////////////////////////////////////////////////////////
/// Запуск гироскопа
/////////////////////////////////////////////////////////////////////////
void giroscop_setup()
{
 
  /* Enable I2C */
  Wire.begin(); //SDA //SCL
 
  // BNO055 clock stretches for 500us or more!
#ifdef ESP8266
  Wire.setClockStretchLimit(1000); // Allow for 1000us of clock stretching
#endif
 
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // Производим запись в регистр энергосбережения MPU-6050
  Wire.write(0);     // устанавливем его в ноль
  Wire.endTransmission(true);
  CompensatorZ = 0;
  CompensatorX = 0;
  CompensatorY = 0;
}
 
/////////////////////////////////////////////////////////////////////////
// Компенсация нуля гироскопа
void Calc_CompensatorZ(float mill_sec)
{
  long ms = mill_sec;
  float i = 0;
  CompensatorZ = 0;
  CompensatorX = 0;
  CompensatorY = 0;
  timer = millis();
  unsigned long endtime = millis() + ms;
  while (endtime > timer) {
    timer = millis();
    Data_mpu6050();
   // Acsel = (atan2(AcX, AcZ));
    AcYsum = AcYsum*0.99+ (atan2(AcX, AcZ))*0.01;
    Acsel_other_side = Acsel_other_side *0.99 + (atan2(AcY, AcZ))*0.01;
    CompensatorZ += (float)(GyZ);
    CompensatorX += (float)(GyX);
    CompensatorY += (float)(GyY);
    delay(2);  
    i++;
  }
  CompensatorZ /= i;
  CompensatorX /= i;
  CompensatorY /= i;
}


 

Модуль step_motor.h

Обслуживает шаговые моторы. Включает в себя функцию void i_stepcoder(), которая вызывается по прерыванию 16-разрядного таймера (1 раз в 20микросекунд), и генерирует шаги, согласно условий, которые задаются в void SetSpeed(), которая в свою очередь принимает данные из расчета скорости по показаниям гироприбора.

Для генерации прерываний используется библиотека TimerOne.h, которая позволяет использовать 16разрядный таймер для генерации прерывания, которые нужны для обработки событий, которые невозможно отложить.

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


 

#include "TimerOne.h"
 
#define MAX_LONGuint16_t 65535 // Бесконечно длинный шаг, для эмуляции простоя.
 
#define STEP_L 10 //Пин левого шага 
#define STEP_R 12 //Пин правого шага 
 
#define portN_STEP_L B00000100 //Бит левого мотора в порту PORTB соответствует 10 пину, 
#define portN_STEP_R B00010000 //Бит правого мотора в порту PORTB соответствует 12 пину
#define portN_notSTEPs B11101011 //Байт-маска для опускание к нулю пинов шага обоих моторов, работает через 
 
#define STEPER_EN 8 //Пин врключения моторов// Функция инициализации уравления моторами.//////////////////////////////
 
#define DIR_R 11 //Пин направления левого шага 
#define DIR_L 9 //Пин направления правого шага
 
#define DIR_L_FRONT false //направления левого шага 
#define DIR_R_FRONT true //направления правого шага
 
#define DIR_L_BACK true //направления левого шага 
#define DIR_R_BACK false //направления правого шага
 
//=========================================
uint32_t speed_stepL;
uint32_t speed_stepR;
//=========================================
//=========================================
bool DIR_L_level = false;
bool DIR_R_level = false;
//=========================================
//=========================================
bool flag_off = false; //Флаг отключения двигателя
//=========================================
bool STEPER_EN_level = true;
volatile uint16_t schetchik_long_step_L = MAX_LONGuint16_t;
volatile uint16_t schetchik_long_step_R = MAX_LONGuint16_t;
 
volatile uint16_t tek_step_long_L_activ;
volatile uint16_t tek_step_long_R_activ;
 
byte xbyte;
//===============================
void setup_motor_system()
{
  DIR_L_level = false;
  DIR_R_level = false;
  STEPER_EN_level = true;
 
  pinMode(STEP_L, OUTPUT);
  pinMode(STEP_R, OUTPUT);
  pinMode(DIR_L, OUTPUT);
  pinMode(DIR_R, OUTPUT);
  pinMode(STEPER_EN, OUTPUT);
 
  digitalWrite(STEP_L, false);
  digitalWrite(STEP_R, false);
  digitalWrite(DIR_L, DIR_L_level);
  digitalWrite(DIR_R, DIR_R_level);
  digitalWrite(STEPER_EN, STEPER_EN_level);
 
}
//===============================
//===============================
 
// == Включение /отключние моторов
void motor_off()
{
  // Отключаем все колеса.
  if (!STEPER_EN_level)
  {
    STEPER_EN_level = true;
    digitalWrite(STEPER_EN, STEPER_EN_level);
  }
}
//===============================
void motor_on()
{
  // Включаем все колеса.
  if (STEPER_EN_level)
  {
    STEPER_EN_level = false;
    digitalWrite(STEPER_EN, STEPER_EN_level);
  }
}
//===============================
// Устанавливаем скорость согласно расчетам
void SetSpeed()
{
  motor_on();
  if (SpeedL == 0)
  {
    schetchik_long_step_L = tek_step_long_L_activ = MAX_LONGuint16_t;  //НЕ шагаем
  }
  else
  {
    // Для шага внутри прерывания
    float corStep = 50000.0 / SpeedL; 
    if (SpeedL > 0)
    {
      DIR_L_level = DIR_L_FRONT;
    }
    else
    {
      DIR_L_level = DIR_L_BACK;
      corStep = - corStep;
    }
    corStep = constrain(corStep, 16.0, 65000.0);
    digitalWrite(DIR_L, DIR_L_level);
    schetchik_long_step_L = tek_step_long_L_activ = uint16_t(round(corStep)); 
  }
//======================
 if (SpeedR == 0)
  {
    schetchik_long_step_R = tek_step_long_R_activ = MAX_LONGuint16_t;  //НЕ шагаем
  }
  else
  {
    // Для шага внутри прерывания
    float corStep = 50000.0 / SpeedR; 
    if (SpeedR > 0)
    {
      DIR_R_level = DIR_R_FRONT;
    }
    else
    {
      DIR_R_level = DIR_R_BACK;
      corStep = - corStep;
    }
    corStep = constrain(corStep, 16.0, 65000.0);
    //Serial.println(corStep);
    digitalWrite(DIR_R, DIR_R_level);
    schetchik_long_step_R = tek_step_long_R_activ = uint16_t(round(corStep)); 
    //Serial.println(xbyte2, BIN);
 }
}
 
 
//=========================================
//=========================================
void i_stepcoder() //Обработка по прерыванию
{
  xbyte = PORTB & portN_notSTEPs;
  //=========================================
  if (!schetchik_long_step_L)
  {
    schetchik_long_step_L = tek_step_long_L_activ;
    xbyte |= portN_STEP_L;
  }
  schetchik_long_step_L--;
  //=========================================
  if (!schetchik_long_step_R)
  {
    schetchik_long_step_R = tek_step_long_R_activ;
    xbyte |= portN_STEP_R;
  }
  schetchik_long_step_R--;
  PORTB = xbyte;
}
//=========================================
//=========================================
 
void stepcoder_setup()
{
  schetchik_long_step_L = MAX_LONGuint16_t;
  schetchik_long_step_R = MAX_LONGuint16_t;
  PORTB = PORTB &  portN_notSTEPs; //Отключаем сигнал шага
  tek_step_long_L_activ = MAX_LONGuint16_t;
  tek_step_long_R_activ = MAX_LONGuint16_t;
 
  Timer1.initialize(20);  //Перерывание срабатывает 1 раз в 20 microseconds,
  Timer1.attachInterrupt(i_stepcoder);
}

 

 

Архив с программой для робота

ОСТАВИТЬ КОММЕНТАРИЙ

Форма авторизации

ВОЙТИ С ПОМОЩЬЮ:
ИЛИ Авторизация на сайте:

или


03.04.2020 22:04 PavelBl
На Java в АндроидСтудио
03.04.2020 19:04 Михаил Момот
Здорово, а программу управления в чем писали?
03.04.2020 18:04 PavelBl
В связи с "каникулами" я вспомнил про своего Ведроида, сделанного по Вашей "старой" программе. Нашел его под диваном, сдул с него пыль и он заработал! А тут Вы, оказывается, программу новую сделали. Спасибо, теперь есть с чем "карантинить")) При постройке своего, я сделал упор на программе управления. Готов поделиться, если кому интересно. Короткое описание: https://drive.google.com/open?id=1xb8m23B2s6fXb1HO7Oqh-goSZ_totC0X
31.03.2020 18:03 Константин Ульянов
Перелопатил все и нашел: У колеса был люфт в десяток градусов относительно вала двига. При чем люфт только под нагрузкой. Руками когда крутищь, его нет. Но у двигов хороший момент и прокручивается. Спасибо за помощь!!! Нашел ещё несколько аналогичных проектов. Доведу до ума по вашему алгоритму и займусь другими. У нас карантин и самоизоляция....
30.03.2020 18:03 Михаил Момот
Проверьте правильность - адекватность работы звеньев 1. Пропорционального: Speed = (AcYsum) * 8.0 * 800.0 * 20.0 / (2.0 * PI * 40.0); 2. Затем дифферинцального Speed = 20.0 * Gyro * 800.0 / (40.0 * 2.0 * PI); Если робот реагирует адекватно, то уже хорошо...
30.03.2020 18:03 Константин Ульянов
https://drive.google.com/file/d/13bjc2vAyFypnXURbu8MfOe0e3CVHIw9b/view?usp=sharing
30.03.2020 17:03 Михаил Момот
Speed = (AcYsum) * 8.0 * 800.0 * 20.0 / (2.0 * PI * 40.0) + 20.0 * Gyro * 800.0 / (40.0 * 2.0 * PI) + XSpeed + Move * 0.13 + (XSpeed - OldCommandSpeed) * 0.056; Вот здесь правьте константы (все эти числа можете посчитать и записать одним, они ничего не значат), в других местах ничего править не нужно, настраивайте ПИДы здесь, изменяя числа.
30.03.2020 17:03 Константин Ульянов
Arduino UNO a4988 X4=800, глупый вопрос, как у Вас стоит MPU6050, Вот это периметр колеса (2 * PI * 40.0)? Где указан параметр от земли -до центра тяжести? 131 вернул, легче не стало :( У меня чуть другая разпиновка, но я думаю это не смертельно #define STEP_L 8 //Пин левого шага #define STEP_R 9 //Пин правого шага #define portN_STEP_L B00000001 //Бит левого мотора в порту PORTB соответствует 10 пину, #define portN_STEP_R B00000010 //Бит правого мотора в порту PORTB соответствует 12 п
30.03.2020 17:03 Михаил Момот
const double _1_d_131 = 1.0 * DEG_TO_RAD / 131.0; Верните на место, это не диаметр колеса, это перевод из единиц измерения гироскопа в радианы...
30.03.2020 17:03 Михаил Момот
Скорее всего нужно ПИДЫ настраивать...
30.03.2020 17:03 Михаил Момот
Хм... А какой так контроллер? какое деление шага? какой MPU?
30.03.2020 14:03 Константин Ульянов
Старт лежа https://www.youtube.com/watch?v=rB6OZolemCQ
30.03.2020 14:03 Михаил Момот
После включения он должен лежать в покое 2 секунды (вроде две), за это время рассчитывается компенсация дрейфа нуля гироскопа, а вы его сразу дергаете, компенсация нуля идет в разнос...
30.03.2020 11:03 Константин Ульянов
Я повторил Вашего робота в корпусе JJrobot EVO2. Поменял центр тяжести в _1_d_131 и радиус колес. Но малыш у меня какой то нервный, дерганый... https://youtu.be/-X6v8bbz26w
29.03.2020 16:03 Михаил Момот
Константин, этот файл от другого робота остался. Вот от этого https://www.youtube.com/watch?v=uzCcdzIEEos
29.03.2020 16:03 Константин Ульянов
В файле info.txt, написано что добавлены светодиоды уровня заряда батареи и направления движения. Но в теле программы, я не нашел их... Так они есть?

X

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

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

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