X
Ископаемое снова в деле
или самобалансирующий робот на 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); }
ОСТАВИТЬ КОММЕНТАРИЙ
Форма авторизации
ВОЙТИ С ПОМОЩЬЮ:
ИЛИ Авторизация на сайте: