add

AccelMotor/AccelMotor.cpp

@@ -0,0 +1,184 @@
+#include "AccelMotor.h"
+#define _sign(x) ((x) > 0 ? 1 : -1)
+
+bool AccelMotor::tick(long pos) {
+	_currentPos = pos;
+	if (millis() - _tmr2 > _dt) {
+		_tmr2 += _dt;
+		_curSpeed = (long)(_currentPos - _lastPos) / _dts;	// ищем скорость в тиках/секунду
+		_curSpeed = filter(_curSpeed);  // медиана + RA
+		_lastPos = _currentPos;
+
+		switch (_runMode) {		
+		case ACCEL_POS:
+			{
+				long err = _targetPos - controlPos;												// "ошибка" позиции
+				if (err != 0) {
+					float accelDt = _accel * _dts;
+					float accel = accelDt;
+					if (abs(err) < _stopzone && abs(_lastSpeed - controlSpeed) < 2) {			// условие завершения движения
+						controlPos = _targetPos;
+						controlSpeed = 0;
+						accel = 0;
+					}
+					if (abs(err) < (float)(controlSpeed * controlSpeed) / (2.0 * accelDt)) {	// если пора тормозить
+						err = -err;																// разворачиваем ускорение
+						accel = (float)(controlSpeed * controlSpeed) / (2.0 * abs(err));		// пересчёт ускорения для плавности
+						if (_sign(controlSpeed) == _sign(err)) err = -err;						// поворачивай обратно
+					}
+					controlSpeed += accel * _sign(err);											// применяем ускорение
+					controlSpeed = constrain(controlSpeed, -_maxSpeed*_dts, _maxSpeed*_dts);	// ограничим
+					controlPos += controlSpeed;													// двигаем позицию
+					_lastSpeed = controlSpeed;	
+
+				}
+				PIDcontrol(controlPos, _currentPos, true);
+			}
+			break;
+		case PID_POS:				
+			PIDcontrol(_targetPos, _currentPos, true);
+			break;
+		case ACCEL_SPEED:
+			{
+				int err = _targetSpeed - _curSpeed;						// ошибка скорости
+				float reducer = min(abs(err) / _accel, 1.0);			// уменьшает ускорение, если шаг будет дальше чем установка
+				_dutyF += (float)_sign(err) * _accel * _dts * reducer;	// ускоряем/замедляем
+				_dutyF = constrain(_dutyF, -_maxDuty, _maxDuty);		// ограничитель 8/10 бит
+				setSpeed(_dutyF);
+			}			
+			break;
+		case PID_SPEED:			
+			PIDcontrol(_targetSpeed, _curSpeed, false);			
+			break;
+		}		
+	}
+	if (_runMode > 1) return (getState() != 0);
+	else return (getState() != 0 || abs(_targetPos - _currentPos) > _stopzone);
+}
+
+void AccelMotor::PIDcontrol(long target, long current, bool cutoff) {
+	// cutoff нужен только для стабилизации позиции, обнуляет integral и учитывает мёртвую зону мотора
+	long err = target - current;
+	_dutyF = err * kp;										// P составляющая	
+	_dutyF += (float)(_prevInput - current) * kd / _dts;	// D составляющая
+	_prevInput = current;
+	integral += (float)err * ki * _dts;
+	_dutyF += integral;										// I составляющая	
+	if (cutoff) {		// отсечка
+		if (abs(err) > _stopzone) _dutyF += _sign(err)*_minDuty;	// учитываем _minDuty - мёртвую зону мотора (метод setMinDuty)
+		else integral = 0;		
+	} else {
+		if (err == 0 && target == 0) integral = 0;
+		else _dutyF += _sign(err)*_minDuty;
+	}
+	_dutyF = constrain(_dutyF, -_maxDuty, _maxDuty);	// ограничиваем по 8 или 10 бит
+	setSpeed(_dutyF);									// и поехали
+}
+
+bool AccelMotor::isBlocked() {
+	return (abs(_dutyF) > _minDuty && _curSpeed == 0);
+}
+
+// ===== settings =====
+void AccelMotor::setRatio(float ratio) {
+	_ratio = ratio;
+}
+void AccelMotor::setDt(int dt) {
+	_dt = dt;
+	_dts = dt / 1000.0;
+}
+void AccelMotor::setCurrent(long pos) {	
+	_currentPos = pos;
+}
+void AccelMotor::setRunMode(runMode mode) {
+	_runMode = mode;
+	if (mode == ACCEL_POS) controlPos = _currentPos;		// костыль!
+}
+
+// ===== current position =====
+long AccelMotor::getCurrent() {
+	return _currentPos;
+}
+long AccelMotor::getCurrentDeg() {
+	return (long)_currentPos * 360 / _ratio;
+}
+
+// ===== current speed =====
+int AccelMotor::getSpeed() {
+	return _curSpeed;
+}
+int AccelMotor::getSpeedDeg() {
+	return (long)_curSpeed * 360 / _ratio;
+}
+float AccelMotor::getDuty() {
+	return _dutyF;
+}
+
+// ===== target position mode =====
+void AccelMotor::setTarget(long pos) {
+	_targetPos = pos;
+	_mode = AUTO;
+}
+void AccelMotor::setTargetDeg(long pos) {
+	_targetPos = (long)pos * _ratio / 360;
+	_mode = AUTO;
+}
+long AccelMotor::getTarget() {
+	return _targetPos;
+}
+long AccelMotor::getTargetDeg() {
+	return (long)_targetPos * 360 / _ratio;
+}
+
+void AccelMotor::setStopZone(int zone) {
+	_stopzone = zone;
+}
+
+// ===== target speed mode =====
+void AccelMotor::setTargetSpeed(int speed) {
+	_targetSpeed = speed;
+	_mode = AUTO;
+}
+void AccelMotor::setTargetSpeedDeg(int speed) {
+	_targetSpeed = (long)speed * _ratio / 360;
+	_mode = AUTO;
+}
+int AccelMotor::getTargetSpeed() {
+	return _targetSpeed;
+}
+int AccelMotor::getTargetSpeedDeg() {
+	return (long)_targetSpeed * 360 / _ratio;
+}
+
+// ===== max speed / acceleration =====
+void AccelMotor::setMaxSpeed(int speed) {
+	_maxSpeed = speed;
+}
+void AccelMotor::setMaxSpeedDeg(int speed) {
+	_maxSpeed = (long)speed * _ratio / 360;
+}
+void AccelMotor::setAcceleration(float accel) {
+	_accel = accel;
+}
+void AccelMotor::setAccelerationDeg(float accel) {
+	_accel = (float)accel * _ratio / 360.0;
+}
+
+// ==== filter ====
+int AccelMotor::filter(int newVal) {
+	_buf[_count] = newVal;
+	if (++_count >= 3) _count = 0;
+	int middle = 0;
+	if ((_buf[0] <= _buf[1]) && (_buf[0] <= _buf[2])) {
+		middle = (_buf[1] <= _buf[2]) ? _buf[1] : _buf[2];
+	} else {
+		if ((_buf[1] <= _buf[0]) && (_buf[1] <= _buf[2])) {
+			middle = (_buf[0] <= _buf[2]) ? _buf[0] : _buf[2];
+		}
+		else {
+			middle = (_buf[0] <= _buf[1]) ? _buf[0] : _buf[1];
+		}
+	}
+	_middle_f += (middle-_middle_f) * 0.7;
+	return _middle_f;
+}

AccelMotor/AccelMotor.h

@@ -0,0 +1,178 @@
+#pragma once
+#include <Arduino.h>
+#include <GyverMotor.h>
+
+// БЕТА! ЭТО БЕТА! beta 0.0000 кек
+
+/*
+	Библиотека для расширенного управления и стабилизации мотора с энкодером
+	- Наследует все фишки из библиотеки GyverMotor (поддержка разных драйверов и режимов)
+	- Режим поддержания скорости с обратной связью
+	- Режим поворота на заданный угол с обратной связью
+	- Настраиваемые коэффициенты PID регулятора
+	- Ограничение ускорения и скорости
+	- Библиотека принимает любой тип обратной связи: энкодер, потенциометр, и т.д.
+	- Поддержка мотор-редукторов, настройка передаточного отношения энкодера
+	- Регулятор учитывает "мёртвую зону" мотора
+	- Все функции работают в градусах и "тиках" энкодера
+*/
+
+enum runMode {	
+	ACCEL_POS,
+	PID_POS,
+	ACCEL_SPEED,
+	PID_SPEED,
+	IDLE_RUN,
+};
+
+class AccelMotor : public GMotor {
+public:
+	using GMotor::GMotor;
+
+	// управляет мотором. Вызывать как можно чаще (внутри таймер с периодом dt)
+	// принимает текущее положение вала мотора (по счёту энкодера)
+	// возвращает true если мотор всё ещё движется к цели
+	bool tick(long pos);							
+
+	// установка передаточного отношения редуктора и энкодера
+	// пример: если редуктор 1:30 - передаём в функцию 30
+	// пример: если редуктор 1:30 и энкодер на 12 тиков - передаём 30*12
+	void setRatio(float ratio);	
+
+	// установка периода регулятора (рекомендуется 2-50 миллисекунд)
+	void setDt(int dt);
+
+	// установка и получение текущей позиции в тиках энкодера и градусах.
+	// setCurrent(pos) равносильна вызову tick(pos) и в принципе не нужна!
+	void setCurrent(long pos);
+	long getCurrent();
+	long getCurrentDeg();
+
+	// установка и получение целевой позиции в тиках энкодера и градусах
+	void setTarget(long pos);
+	void setTargetDeg(long pos);
+	long getTarget();
+	long getTargetDeg();
+
+	// установка максимальной скорости в тиках энкодера/секунду и градусах/секунду
+	void setMaxSpeed(int speed);
+	void setMaxSpeedDeg(int speed);
+
+	// установка ускорения тиках энкодера и градусах в секунду
+	void setAcceleration(float accel);
+	void setAccelerationDeg(float accel);
+
+	// установка и получение целевой скорости в тиках энкодера/секунду и градусах/секунду
+	void setTargetSpeed(int speed);
+	void setTargetSpeedDeg(int speed);
+	int getTargetSpeed();
+	int getTargetSpeedDeg();
+
+	// получить текущую скорость в тиках энкодера/секунду и градусах/секунду
+	int getSpeed();
+	int getSpeedDeg();	
+
+	// получить текущий ШИМ сигнал (float из ПИД регулятора)
+	float getDuty();
+
+	// ручная установка режима работы
+	// IDLE_RUN - tick() не управляет мотором. Может использоваться для отладки
+	// ACCEL_POS - tick() работает в режиме плавного следования к целевому углу
+	// PID_POS - tick() работает в режиме резкого следования к целевому углу
+	// ACCEL_SPEED - tick() работает в режиме плавного поддержания скорости (с заданным ускорением)
+	// PID_SPEED - tick() работает в режиме поддержания скорости по ПИД регулятору
+	void setRunMode(runMode mode);
+
+	// возвращает true, если вал мотора заблокирован, а сигнал подаётся
+	bool isBlocked();
+
+	// коэффициенты ПИД регулятора
+	// пропорциональный - от него зависит агрессивность управления, нужно увеличивать kp
+	// при увеличении нагрузки на вал, чтобы регулятор подавал больший управляющий ШИМ сигнал
+	float kp = 2.0;		// (знач. по умолчанию)
+
+	// интегральный - позволяет нивелировать ошибку со временем, имеет накопительный эффект
+	float ki = 0.9;		// (знач. по умолчанию)
+
+	// дифференциальный. Позволяет чуть сгладить рывки, но при большом значении
+	// сам становится причиной рывков и раскачки системы!
+	float kd = 0.1;		// (знач. по умолчанию)
+
+	// установить зону остановки мотора для режима стабилизации позиции (по умолч. 8)
+	void setStopZone(int zone);
+
+private:
+	int filter(int newVal);
+	int _buf[3];
+	byte _count = 0;
+	float _middle_f = 0;
+
+	float _lastSpeed = 0;	
+	void PIDcontrol(long target, long current, bool cutoff);
+	float integral = 0;
+	int _dt = 20;
+	float _dts = 0.02;
+	long _lastPos = 0, _currentPos = 0, _targetPos = 0;
+	int _curSpeed = 0;
+	int _maxSpeed = 0, _targetSpeed = 0;
+	float _ratio = 1;
+	uint32_t _tmr2 = 0;
+	float _accel = 1;
+	float _dutyF = 0;
+	long controlPos = 0;
+	float controlSpeed = 0;
+	int _stopzone = 8;
+	long _prevInput = 0;
+	runMode _runMode = IDLE_RUN;
+};
+
+/*
+	======= НАСЛЕДУЕТСЯ ИЗ GYVERMOTOR =======
+	GMotor(driverType type, int8_t param1 = NC, int8_t param2 = NC, int8_t param3 = NC, int8_t param4 = NC);
+	// три варианта создания объекта в зависимости от драйвера:
+	// GMotor motor(DRIVER2WIRE, dig_pin, PWM_pin, (LOW/HIGH) )
+	// GMotor motor(DRIVER3WIRE, dig_pin_A, dig_pin_B, PWM_pin, (LOW/HIGH) )
+	// GMotor motor(RELAY2WIRE, dig_pin_A, dig_pin_B, (LOW/HIGH) )
+	
+	// установка скорости 0-255 (8 бит) и 0-1023 (10 бит)
+	void setSpeed(int16_t duty);			
+	
+	// сменить режим работы мотора:	
+	// FORWARD - вперёд
+	// BACKWARD - назад
+	// STOP - остановить
+	// BRAKE - активное торможение
+	void setMode(workMode mode);
+	
+	// направление вращения	
+	// NORM - обычное
+	// REVERSE - обратное
+	void setDirection(dir direction);
+	
+	// установить минимальную скважность (при которой мотор начинает крутиться)
+	void setMinDuty(int duty);
+	
+	// установить выход в 8 бит
+	void set8bitMode();		
+
+	// установить выход в 10 бит
+	void set10bitMode();					
+	
+	// установить deadtime (в микросекундах). По умолч 0
+	void setDeadtime(uint16_t deadtime);	
+	
+	// установить уровень драйвера (по умолч. HIGH)
+	void setLevel(int8_t level);			
+	
+	// плавное изменение к указанной скорости (к величине ШИМ)
+	void smoothTick(int16_t duty);
+	
+	// скорость изменения скорости
+	void setSmoothSpeed(uint8_t speed);
+	
+	// дать прямую команду мотору (без смены режима)
+	void run(workMode mode, int16_t duty);	
+	
+	// возвращает -1 при вращении BACKWARD, 1 при FORWARD и 0 при остановке и торможении
+	int getState();
+*/