План уроку

Робота з сервомоторами
Біаси. Їх фільтрування для збільшення точності

Матеріали

ESP32
Сервомотор SG90
Гіроакселерометр MPU6050

Результат уроку

Результатом даного уроку має бути:

ESP32 із MPU6050, що вимірюють прискорення та кутову швидкість, і розраховують орієнтацію в просторі методом інтегрування Ейлера та передають дані кути для обертання сервомоторів

Урок

Робота із сервомоторами

1. Підключення сервомотора MG90 до ESP32

Для підключення використовуємо наступні піни на ESP32:

VIN – Живлення +5В (червоний)
GPIO26 – сигнал керування (жовтий)
GND – земля, мінус (чорний)

2. Встановлення бібліотеки

Існує декілька варіантів бібліотеки Servo для ESP32. Найпоширеніші з них – ESP32Servo та ServoESP32. Список функцій, класів та методів у них однаковий, використання теж однакове. Різниця тільки в підключенні.

ВАЖЛИВО: При використанні бібліотеки ServoESP32 єдина робоча версія – 1.0.3!

Для ESP32Servo підключення відбувається за допомогою коду <ESP32Servo.h>

Для ServoESP32 просто <Servo.h>

3. Прошивка

В даному коді використовується бібліотека ESP32Servo.

Приклад коду для тестування:

#include <ESP32Servo.h>

// Пін, до якого підключено сервопривід
static const int servoPin = 26;

// Створення об'єкта сервоприводу
Servo servo1;

void setup() {
	// Ініціалізація послідовного порту на швидкості 115200 біт/сек
	Serial.begin(115200);
	
	// Вказуємо для об'єкта servo1 на якому піні подається сигнал керування
	servo1.attach(servoPin);
}

void loop() {
	// В циклі спочатку прокручуємо вал сервомотору від 0 до 180 градусів
	for(int posDegrees = 0; posDegrees <= 180; posDegrees++) {
	    servo1.write(posDegrees);   // Виведення значення кута на сервомотор
	    Serial.println(posDegrees);
	    delay(1);
	}

	// А потім повертаємо зі 180 градусів до 0
	for(int posDegrees = 180; posDegrees >= 0; posDegrees--) {
	    servo1.write(posDegrees);  // Виведення значення кута на сервомотор
	    Serial.println(posDegrees);
	    delay(1);
	}
}

Після прошивки, вал сервомотору повинен почергово повертатись на 180 градусів в обидва напрямки

Робота з гіроакселерометрами

За основу візьмемо код з другого уроку:

Reading MPU6050 on Arduino

1. Для збільшення точності наших розрахунків потрібно вирахувати помилку стабільного зміщення даних, так званий bias(зміщення).

Для цього створимо змінні, куди запишемо похибку:

double deltaX, deltaY, deltaZ;

В функції setup додаємо блок коду, який розрахує похибки:

// Розрахуємо відхилення за допомогою знаходження середнього значення (500 ітерацій)
for (size_t i = 0; i < 500; i++)
{
	// Читаємо дані з давача
	mpu.getEvent(&a, &g, &temp);
	deltaX = deltaX + g.gyro.x; //
	deltaY = deltaY + g.gyro.y; // сумуємо для кожної з осей значення від давачів
	deltaZ = deltaZ + g.gyro.z; //
}

deltaX = deltaX / 500; //
deltaY = deltaY / 500; // Ділимо кожне значення на кількість ітерацій
deltaZ = deltaZ / 500; // і знаходимо середнє

2. Розраховуємо кути із кутової швидкості

Додаємо змінні, куди будемо записувати розрахований кут:

double angleX, angleY, angleZ;

В функції loop замість попереднього коду додаємо наступний:

if((millis() - lastSendTime) >= 10) {
  // Отримуємо дані від давача
	mpu.getEvent(&a, &g, &temp);
	
	// Розраховуємо кут інтегруючи кутову швидкість (також віднімаємо біаси)
	angleX = angleX + (g.gyro.x - deltaX) * 0.01;
	angleY = angleY + (g.gyro.y - deltaY) * 0.01;
	angleZ = angleZ + (g.gyro.z - deltaZ) * 0.01;
	//         ^               ^             ^
	//     попередній     компенсація   множення кутової швидкості на час (10 мс)
	//        кут            біасів            для знаходження кута
	
	
	Serial.print(angleX); //
	Serial.print("\t\t"); //
	Serial.print(angleY); // Виводимо значення в послідовний порт
	Serial.print("\t\t"); //
	Serial.print(angleZ); //
	Serial.println();     //
	
	lastSendTime = millis();
}

3. Крутимо сервомотори

Підключаємо бібліотеку та задаємо піни для серво:

#include <ESP32Servo.h>

#define SERVO_X_PIN 13
#define SERVO_Y_PIN 15

Створюємо глобальні об’єкти типу Servo :

Servo servoX;
Servo servoY;

В функції setup додаємо блок коду, який ініціалізує сервомотори:

servoX.attach(SERVO_X_PIN);
servoY.attach(SERVO_Y_PIN);

В фукнції loop всередину if((millis() - lastSendTime) >= 10) додаємо блок коду, який оновлює кути на сервомоторах з розрахованих раніше X та Y:

servoX.write(angleX);
servoY.write(180 - angleY);

Весь код:

#include <Arduino.h>

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#include <ESP32Servo.h>

#define SERVO_X_PIN 13
#define SERVO_Y_PIN 15 

Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

uint32_t lastSendTime = 0;

double deltaX, deltaY, deltaZ;
double angleX, angleY, angleZ;

Servo servoX;
Servo servoY;


void setup()
{
	// Init Serial Monitor
	Serial.begin(115200);

	if (!mpu.begin())
	{
		Serial.println("Failed to find MPU6050 chip");
		while (1)
		{
			delay(10);
		}
	}

	mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
	mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
	mpu.setFilterBandwidth(MPU6050_BAND_260_HZ);
	delay(100);

	// Розрахуємо відхилення за допомогою знаходження середнього значення (500 ітерацій)
	for (size_t i = 0; i < 500; i++)
	{
		// Читаємо дані з давача
		mpu.getEvent(&a, &g, &temp);
		deltaX = deltaX + g.gyro.x; //
		deltaY = deltaY + g.gyro.y; // сумуємо для кожної з осей значення від давачів
		deltaZ = deltaZ + g.gyro.z; //
	}
	
	deltaX = deltaX / 500; //
	deltaY = deltaY / 500; // Ділимо кожне значення на кількість ітерацій
	deltaZ = deltaZ / 500; // і знаходимо середнє

	// Ініціалізуємо сервомотори
	servoX.attach(SERVO_X_PIN);
	servoY.attach(SERVO_Y_PIN);
}

void loop()
{	
	if((millis() - lastSendTime) >= 10) {
	  // Отримуємо дані від давача
		mpu.getEvent(&a, &g, &temp);
		
		// Розраховуємо кут інтегруючи кутову швидкість (також віднімаємо біаси)
		angleX = angleX + (g.gyro.x - deltaX) * 0.01;
		angleY = angleY + (g.gyro.y - deltaY) * 0.01;
		angleZ = angleZ + (g.gyro.z - deltaZ) * 0.01;
		//         ^               ^             ^
		//    попередній      компенсація    множення кутової швидкості на час 
		//        кут            біасів        (10 мс) для знаходження кута     

		// Оновлюємо кути сервомоторів
		servoX.write(angleX);
	    servoY.write(180 - angleY); 
		lastSendTime = millis();
	}
}

3 Використання розрахованих кутів для роботи з сервомоторами