Подключение 3х осевого гироскопа MPU-6050 к Arduino и 2 сервопривода

Mpu_iic.c/mpu_iic.h

mpu_iic.hIt mainly defines the operation of pin level and function declaration.

#ifndef __MPU_IIC_H#define __MPU_IIC_H#include"stm32f10x.h"#include"delay.h"#define MPU_SDA_IN()  {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}  #define MPU_IIC_SDA_1() GPIO_SetBits( GPIOB, GPIO_Pin_11 )#define MPU_IIC_SDA_0() GPIO_ResetBits( GPIOB, GPIO_Pin_11 )#define MPU_IIC_SCL_1() GPIO_SetBits( GPIOB, GPIO_Pin_10 )#define MPU_IIC_SCL_0() GPIO_ResetBits( GPIOB, GPIO_Pin_10 )#define MPU_IIC_AD0_1() GPIO_SetBits( GPIOA, GPIO_Pin_15 )#define MPU_IIC_AD0_0() GPIO_ResetBits( GPIOA, GPIO_Pin_15 )#define MPU_IIC_SDA_READ() GPIO_ReadInputDataBit( GPIOB, GPIO_Pin_11 )#define MPU_IIC_Delay() delay_us(2)voidMPU_IIC_Init(void);voidMPU_IIC_Start(void);voidMPU_IIC_Stop(void);
uint8_t MPU_IIC_Wait_Ack(void);voidMPU_IIC_Ack(void);voidMPU_IIC_NAck(void);voidMPU_IIC_Send_Byte( uint8_t data );
uint8_t MPU_IIC_Read_Byte( uint8_t ack );#endif

mpu_iic.cThere is nothing to say about simulating IIC code through software.

#include"mpu_iic.h"#include"usart.h"voidMPU_IIC_Init(void){
	GPIO_InitTypeDef  GPIO_InitStruct;RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE );
	
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;GPIO_Init( GPIOB,&GPIO_InitStruct );MPU_IIC_SDA_1();MPU_IIC_SCL_1();}voidMPU_IIC_Start(void){MPU_SDA_OUT();MPU_IIC_SDA_1();MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SDA_0();delay_us(2);MPU_IIC_SCL_0();}voidMPU_IIC_Stop(void){MPU_SDA_OUT();MPU_IIC_SDA_0();MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SDA_1();MPU_IIC_SCL_1();delay_us(2);}
uint8_t MPU_IIC_Wait_Ack(void){
	uint8_t count;MPU_SDA_IN();MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SDA_1();delay_us(2);while(MPU_IIC_SDA_READ()==1){
		count  ;if( count>250){MPU_IIC_Stop();return1;}}MPU_IIC_SCL_0();return0;}voidMPU_IIC_Ack(void){MPU_IIC_SCL_0();MPU_SDA_OUT();MPU_IIC_SDA_0();delay_us(2);MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SCL_0();}voidMPU_IIC_NAck(void){MPU_IIC_SCL_0();MPU_SDA_OUT();MPU_IIC_SDA_1();delay_us(2);MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SCL_0();}voidMPU_IIC_Send_Byte( uint8_t data ){
	uint8_t t;MPU_SDA_OUT();MPU_IIC_SCL_0();for( t=0;t<8;t  ){if(((data&0x80)>>7)==1)MPU_IIC_SDA_1();elseMPU_IIC_SDA_0();
		data<<=1;MPU_IIC_SCL_1();delay_us(2);MPU_IIC_SCL_0();delay_us(2);}}
uint8_t MPU_IIC_Read_Byte( uint8_t ack ){
	uint8_t t,data=0;MPU_SDA_IN();for( t=0;t<8;t  ){MPU_IIC_SCL_0();delay_us(2);MPU_IIC_SCL_1();
		
		data<<=1;if(MPU_IIC_SDA_READ()==1)
			data  ;delay_us(2);}if(!ack )MPU_IIC_NAck();elseMPU_IIC_Ack();return data;}

DMP related codes

If you want to use DMP to find the Euler angle code, you can include the following files. The interface functions are listed below. When you use them, you can directly use the interface functions.

Interface macro definition provided to DMP algorithm
Only need to provide: read and write operation function and delay function of MPU6050

#define i2c_write   MPU_Write_Continue#define i2c_read    MPU_Read_Continue#define delay_ms    delay_ms

DMP initialization

uint8_t mpu_dmp_init(void){
	uint8_t res=0;MPU_IIC_Init();if(mpu_init()==0){	 
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);if(res)return1; 
		res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);if(res)return2; 
		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);if(res)return3; 
		res=dmp_load_motion_driver_firmware();if(res)return4; 
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));if(res)return5; 
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|
		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
		    DMP_FEATURE_GYRO_CAL);if(res)return6; 
		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);if(res)return7;   
		res=run_self_test();if(res)return8;    
		res=mpu_set_dmp_state(1);if(res)return9;}elsereturn10;return0;}

Obtain Euler angle

uint8_t mpu_dmp_get_data(float*pitch,float*roll,float*yaw){float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;unsignedlong sensor_timestamp;short gyro[3], accel[3], sensors;unsignedchar more;long quat[4];if(dmp_read_fifo(gyro, accel, quat,&sensor_timestamp,&sensors,&more))return1;if(sensors&INV_WXYZ_QUAT){
		q0 = quat[0]/ q30;
		q1 = quat[1]/ q30;
		q2 = quat[2]/ q30;
		q3 = quat[3]/ q30;*pitch =asin(-2* q1 * q3  2* q0* q2)*57.3;*roll  =atan2(2* q2 * q3  2* q0 * q1,-2* q1 * q1 -2* q2* q2  1)*57.3;*yaw   =atan2(2*(q1*q2   q0*q3),q0*q0 q1*q1-q2*q2-q3*q3)*57.3;}elsereturn2;return0;}

Mian() function

After the MPU6050 is initialized in the main function, DMP is also initialized. Then you can directly use MPU DMP get data() to get the Euler angle and the temperature value.

#include"stm32f10x.h"#include"usart.h"#include"delay.h"#include"mpu6050.h"#include"inv_mpu.h"#include"inv_mpu_dmp_motion_driver.h"intmain(void){
	uint8_t x=0;float pitch,roll,yaw;short aacx,aacy,aacz;short gyrox,gyroy,gyroz;short temp;NVIC_PriorityGroupConfig(2);delay_init();USART1_Init(115200);printf("Program startn");if(MPU_Init()!=0){printf("MPU6050 Initialization error!n");return0;}if(mpu_dmp_init()){printf("DMP Initialization error!n");return0;}while(1){if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0){ 
			temp=MPU_Get_Temperature();MPU_Get_Accelerometer(&aacx,&aacy,&aacz);MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);}delay_ms(100);printf("pitch:f  roll:f  yaw:fn",pitch,roll,yaw);}}

This is the MPU6050 that I understand. When doing the balance car later, I need to read the Euler angle. First, I want to summarize. If there is any problem, I can communicate with you and make progress together!!!q: 2723808286

Drv8825

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

На рисунке название ряда контактов сверху имеют линию, это знак «Инверсии» сигнала, он означает, что активный сигнал на данной линии – отрицательный:

• ENABLE – включение управления мотором (если 1 – выключен, 0 – включен);
• М0,М1,М2 – задают режим деления шага (дробления шага) см.таблицу 3.1;
• RESET – перезагрузка (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
• SLEEP – сон (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
• STEP – шаг, импульс на данном входе, это команда мотору сделать шаг;
• DIR – задает направление вращения вала мотора (само направление условно и зависит от порядка подключения обмоток);
• VMOT – ( ) контакт для подачи напряжения питания положительный;
• GNDMOT – (–/GND) контакт для подачи напряжения питания – (ЗЕМЛЯ);
• B1 и B2 – разъем для подключение первой обмотки (номера обмоток и номера контактов в обмотках условны);
• A1 и A2 – разъем для подключение второй обмотки (номера обмоток и номера контактов в обмотках условны);
• FAULT – выход сигнала аварии (действие не изучено, возможно устанавливается в низкий уровень при превышении нагрузки, замыкании или обрыве обмотки мотора);
• GNDLOGIC – GND (земля) логической части (контроллера управления), если источники питания логики и силовой части различны и не имеют общей «земли», если источник питания один, данный контакт можно не подключать.

Некоторые современные драйверы шаговых моторов могут дробить шаги на более мелкие, за счет плавного изменения тока обмоток. DRV8825 может делить шаг на 32 микрошага, таким микрошагом мы пользоваться не будем, но возможно воспользуемся данной возможностью для дробления шага до 1/16 (в роботе дробление шага 1/4), это увеличит плавность хода, что важно в нашем случае важно.

На драйвере есть подстроечный резистор (он находится под небольшим напряжением), он нужен для настройки тока фазы.

Для подстройки тока фазы требуется мультиметр и тоненькая плоская отвертка. На мультиметре устанавливаем режим измерения напряжения (до 2-5 вольт). Подключаем драйвер к силовому питанию. Измеряем напряжение между GND и металлической вращающейся головкой подстроечного резистора.

Измерив на подключенном к питанию драйвере напряжение между резистором и GND, и умножив полученное значение на 2, получим ток фазы. В нашем случае следует добиться значения в пределах 0.7-1.0В, что соответствует току через обмотки мотора 1.4-2А.

Большие значения поставить можно, но это приведет к нагреву драйвера и мотора, а также ускорит разряд батареи робота. Меньшие значения могут стать причиной того, что робот под нагрузкой (в режиме управления роботом) не будет поворачивать колеса или будет «пропускать» шаги. Вращаем головку (без усилий — можно повредить резистор), добиваемся нужного значения.

Для защиты драйвера от перегрева следует использовать радиатор (он идет в комплекте с драйвером). Радиатор снабжен клейкой основой. Снимите бумагу с клейкой части радиатора. Установите радиатор на микросхему драйвера, прижмите, произведите вращательные движения на 5-10 градусов, чтобы удалить пузыри воздуха между радиатором и микросхемой.

Значение микрошага шагового мотора в зависимости от подключения контактов M0,M1,M2 драйвера DRV8825.

Gy-521 – модуль с гироскопом, акселерометром и термометром mpu-6050 для ардуино

Подписка

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

Также, его можно применить в светящемся шлеме! Удобно, не правда ли? Наклонил голову вправо — загорелось правое «полушарие» =) влево — левое, согнул голову в шее — сзади на шлеме высветился стоп-сигнал! Вот только, думаю, моей шеи хватит минут на 5 таких упражнений, потом — коллдаун, в виде боли на неделю обеспечен.

Итак, заказал — прислали, теперь нужно разобраться и продемонстрировать функционал для Вас, уважаемые читатели Муськи. Пришло с треком, не известно, почему, но в Киеве посылку держали неделю (или это глюки системы треккинга). Дошла чуть менее, чем за три недели. Упаковано в три слоя утеплителя — для амортизации.

Сам модуль — внутри запаянного пакетика со штрих-кодом на наклейке:Размеры платы: 20×16мм. В комплекте два набора штыревых контактов: ровные и загнутые — удобно, не придется гнуть или ровнять.

Большие отверстия не металлизированы, как на картинке в магазине, поэтому, если Вы будите их прикручивать болтами к «минусу», эффекта не будет. Я заливаю их термоклеем, а он, образуя «шапочки», надёжно фиксирует плату.Отвертия контактов металлизированы отлично, паяются без проблем.

Схема

Взята из статьи про такой же модуль — radiocopter.ru/mc/mc324.php

MPU-6050 снабжен акселерометром, гироскопом и термометром. Зачем нужен термометр — не понятно, вероятно, его было удобно разместить в этой микросхеме.

Или, действительно, есть такие варианты применения, о которых я не знаю, где тебя вертят и греют нужно вертеться в пространстве и знать температуру =) Или, в процессе интенсивной работы с устройством, оно может нагреваться и нужно контролировать его температуру и нагрузку.

— 16-битный АЦП, — напряжение питания 3-5В, — поддержка протокола «IIC» (может, I2C ?), — диапазон ускорений: ± 2 ± 4 ± 8 ± 16g, — диапазон «гиро»: ± 250 500 1000 2000 ° / s, — покрытие иммерсионным золотом вместо лужения, — ток при работе последнего примера составил 5.3 мА и 1.

2 мА когда устройство не успело стартовать (питание на модуль было подано после выполнения setup() контроллером) Для интерфейса I2C у Ардуино имеются контакты A4 (SDA) и A5 (SCL), да-а, это те, которые расположены чёрти-где (на одной плате у меня они были справа от контроллера, на другой с левого края).

В коде нужно использовать библиотеку Wire, прочитать о ней можно туточки. Минимальная схема во Fritzing такая:… а значит у нас уже не 8 лишних штырьков, а целых двенадцать! Термометр тестировать проще всего: залил скетч отсюда, открыл монитор порта, выставил скорость на 9600, —

побежали такие строки

AcX = 1624 | AcY = -808 | AcZ = 15176 | Tmp = 25.14 | GyX = 11 | GyY = 314 | GyZ = -138 AcX = 1584 | AcY = -876 | AcZ = 15112 | Tmp = 25.00 | GyX = 0 | GyY = 193 | GyZ = -163 AcX = 1616 | AcY = -904 | AcZ = 15172 | Tmp = 25.05 | GyX = -2 | GyY = 264 | GyZ = -181 AcX = 1648 | AcY = -836 | AcZ = 14948 | Tmp = 25.

09 | GyX = 3 | GyY = 146 | GyZ = -192 AcX = 1792 | AcY = -800 | AcZ = 15216 | Tmp = 25.09 | GyX = 27 | GyY = -181 | GyZ = -218 AcX = 1864 | AcY = -900 | AcZ = 14932 | Tmp = 25.09 | GyX = 11 | GyY = 48 | GyZ = -206 AcX = 2144 | AcY = -796 | AcZ = 14860 | Tmp = 25.

05 | GyX = 8 | GyY = 100 | GyZ = -191 AcX = 2088 | AcY = -916 | AcZ = 14952 | Tmp = 25.14 | GyX = 11 | GyY = 158 | GyZ = -189 AcX = 2180 | AcY = -752 | AcZ = 14964 | Tmp = 25.14 | GyX = 6 | GyY = 334 | GyZ = -182 AcX = 2296 | AcY = -796 | AcZ = 15076 | Tmp = 25.

05 | GyX = -3 | GyY = 184 | GyZ = -189 AcX = 2160 | AcY = -788 | AcZ = 15176 | Tmp = 25.14 | GyX = -8 | GyY = 184 | GyZ = -172 AcX = 2036 | AcY = -852 | AcZ = 14988 | Tmp = 25.09 | GyX = 3 | GyY = 292 | GyZ = -172 AcX = 1984 | AcY = -836 | AcZ = 14892 | Tmp = 25.

09 | GyX = 38 | GyY = 90 | GyZ = -205 AcX = 2136 | AcY = -708 | AcZ = 14976 | Tmp = 25.19 | GyX = -5 | GyY = 270 | GyZ = -148 AcX = 2000 | AcY = -788 | AcZ = 14888 | Tmp = 25.14 | GyX = -35 | GyY = 239 | GyZ = -157 AcX = 2008 | AcY = -784 | AcZ = 15048 | Tmp = 25.

19 | GyX = -3 | GyY = 342 | GyZ = -183 AcX = 1884 | AcY = -868 | AcZ = 15140 | Tmp = 25.19 | GyX = -3 | GyY = 214 | GyZ = -194 AcX = 2072 | AcY = -820 | AcZ = 15020 | Tmp = 25.28 | GyX = 41 | GyY = 157 | GyZ = -205 AcX = 2008 | AcY = -780 | AcZ = 15144 | Tmp = 25.

24 | GyX = 0 | GyY = 220 | GyZ = -204 AcX = 1924 | AcY = -828 | AcZ = 14968 | Tmp = 25.24 | GyX = -9 | GyY = 254 | GyZ = -187 AcX = 1920 | AcY = -828 | AcZ = 14936 | Tmp = 25.33 | GyX = 7 | GyY = 253 | GyZ = -185 AcX = 2023 | AcY = -728 | AcZ = 14904 | Tmp = 25.

14 | GyX = 16 | GyY = 190 | GyZ = -167 AcX = 1900 | AcY = -744 | AcZ = 15048 | Tmp = 25.42 | GyX = -4 | GyY = 162 | GyZ = -177 AcX = 1940 | AcY = -780 | AcZ = 14992 | Tmp = 25.28 | GyX = -34 | GyY = 271 | GyZ = -191 AcX = 1960 | AcY = -900 | AcZ = 15080 | Tmp = 25.38 | GyX = 2 | GyY = 194 | GyZ = -182

AcX = 1840 | AcY = -952 | AcZ = 15012 | Tmp = 25.38 | GyX = 19 | GyY = 272 | GyZ = -167

Вследствие нагревания феном для волос, значение Tmp = взлетело до 80. Далее покрутили в пространстве платкой — другие показания тоже изменяются, но это не наглядно.

Данные, выводимые вторым скетчем

Step 6: understand the code

Basically in this example, we will see if our sensor is working so we will display the sensor data on serial monitor.

So we begin the serial monitor in setup part.

Serial.begin(115200);

In this While Loop, the sensor test sequence is executed.

while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{    Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}

Sometimes we are making a project, and have to set our sensor in a specific orientation, we need offsets, we don’t need it for this tutorial,

• but to change offset, simply un-comment these lines.

// mpu.setGyroOffsetX(155);
// mpu.setGyroOffsetY(15);
// mpu.setGyroOffsetZ(15);
( uncomment by removing "//" )

• there is a calibration line, which will virtually set our sensor flat.

// Calibrate gyroscope. The calibration must be at rest.
// If you don't want calibrate, comment this line.
mpu.calibrateGyro();

Remember offset and calibration are two different things, offset will give you defined calibration, for example, you can mount this sensor at weird angle and yet it will act as reference point for zero.

• Next we have sensitivity, which at default is 3.

// Set threshold sensivty. Default 3.
// If you don't want use threshold, comment this line or set 0.
mpu.setThreshold(3);

• In check loop section, basic hardware checking is done.

void checkSettings()
{
Serial.println();
Serial.print(" * Sleep Mode:        ");
Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
Serial.print(" * Clock Source:      ");
switch(mpu.getClockSource())
{
case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
}
Serial.print(" * Gyroscope:         ");
switch(mpu.getScale())
{
case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
}
Serial.print(" * Gyroscope offsets: ");
Serial.print(mpu.getGyroOffsetX());
Serial.print(" / ");
Serial.print(mpu.getGyroOffsetY());
Serial.print(" / ");
Serial.println(mpu.getGyroOffsetZ());
Serial.println();
}

I highly suggest to leave this loop as it is.

• in loop section, which is most important part of this entire code, that is getting the values from our sensor. First we need to call the values, using mpu.readRawGyro or mpu.readNormalizeGyro, now the concept of raw and normalized is such that raw are basically numbers and normalized values are values which go through filters and calculations or you can say, processed data.

void loop()
{
Vector rawGyro = mpu.readRawGyro();
Vector normGyro = mpu.readNormalizeGyro();

• We have 3 axis, called as x y and z, which can be called using variable name which we set as rawGyro, followed by axis name, to make a project, we will need these 3 values of x, y and z using this variablename.axis command.

Serial.print(" Xraw = ");
Serial.print(rawGyro.XAxis);
Serial.print(" Yraw = ");
Serial.print(rawGyro.YAxis);
Serial.print(" Zraw = ");
Serial.println(rawGyro.ZAxis);
Serial.print(" Xnorm = ");
Serial.print(normGyro.XAxis);
Serial.print(" Ynorm = ");
Serial.print(normGyro.YAxis);
Serial.print(" Znorm = ");
Serial.println(normGyro.ZAxis);

• atlast, to view the values at comfortable speed, lets increase the delay from 10 to 1000

delay(10);
}

since our example code is ready and we understand what we did in code, its time to upload the code and view results.

Code can be Found here!

Step 7: Check Results in Serial Port

once the upload is done, its time to open up the serial Monitor and observe output:

Don’t forget to match the serial port with the baud rate we defined in start of code which is 115200

Using stm32 dma and i2c to read data from mpu6050 – old library

Update 11/2023:this was posted in 2023 using the old STM32 stdlib, which might be tedious to setup and get it working. The newer STM32CubeMX and STM32CubeIDE support DMA setup much quicker and easier. Nevertheless, the explanation in this post is still valid and useful to briefly layout how DMA in STM32 actually works. The code, however, is outdated and for reference only.

In the previous post, an example of using STM32 DMA to perform a simple data copy between 2 arrays was introduced. Now, I will show another example with DMA and I2C to read raw data from MPU6050 acceleration and gyroscope sensor directly. Besides, a comparison to show the timing difference between using and not using DMA is also mentioned.

stm32 mpu6050

MPU6050 is a very popular MEMS acceleration and gyroscope sensor and other devices can connect and get data from it through an I2C connection. There are a lot of libraries for Arduino that are available on the internet for connecting with MPU6050 and a few libraries for STM32. Harinadha has done the porting job from MPU6050 Arduino library of Jeff Rowberg to STM32 here as well: http://harinadha.wordpress.com/2023/05/23/mpu6050lib/ without using INT pin (interrupt pin) of MPU6050. I also used this library for the first time and found it was quite difficult to get the most updated gyro data for calculation. So, I got the wrong gyro angle all the time. Moreover, I noticed that the code took lots of time to read 14 bytes of data (including 6 bytes acceleration, 2 bytes of temperature, and 6 bytes of the gyro), nearly 2ms, so there is no chance to get the sample rate at 1ms.

MPU6050 STM32 connection

Then I tried to look back at the code for Arduino and they actually used the INT pin of MPU6050 to trigger the reading routine. So, I tried to edit the code of Harinadha to implement both INT triggering and DMA reading from I2C with some fine tunes to give the best processing time. First, let’s have a look at the initialized routine for MPU6050. Again, Stdperiph driver V3.5.0 of ST was used here for basic I2C peripheral functions:

void MPU6050_Initialize(void)  
{
MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 1<<7);//reset the whole module first

delay(50);    //wait for 50ms for the gyro to stable

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO);//PLL with Z axis gyroscope reference

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x01);        //DLPF_CFG = 1: Fs=1khz; bandwidth=42hz 

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);    //500Hz sample rate ~ 2ms

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000);    //Gyro full scale setting

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_16);    //Accel full scale setting

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 1<<4);        //interrupt status bits are cleared on any read operation

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 1<<0);        //interupt occurs when data is ready. The interupt routine is in the receiver.c file.   

MPU6050_Write(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x07);//reset gyro and accel sensor  
}

With the MPU6050_Write function and I2C routine as follow:

void MPU6050_Write(uint8_t slaveAddr, uint8_t regAddr, uint8_t data)  
{
    uint8_t tmp;  
    tmp = data;
    MPU6050_I2C_ByteWrite(slaveAddr,&tmp,regAddr);   
}
//------------------------------------------------------------------
void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8* pBuffer, u8 writeAddr)  
{

/* Send START condition */
I2C_GenerateSTART(MPU6050_I2C, ENABLE);  
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));  
/* Send MPU6050 address for write */
I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter);  
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));  
/* Send the MPU6050's internal address to write to */
I2C_SendData(MPU6050_I2C, writeAddr);  
/* Test on EV8 and clear it */
//while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTING));
/* Send the byte to be written */
if (pBuffer!=0) I2C_SendData(MPU6050_I2C, pBuffer);  
/* Test on EV8_2 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));  
/* Send STOP condition */
I2C_GenerateSTOP(MPU6050_I2C, ENABLE);

}

And those defined registers of MPU6050 can be found here:

MPU6050.h

Here, we finish setting up the MPU6050 sensor. From now on, the sensor will run with the following configuration:

• Sample rate: 2ms
• Gyro full scale for X, Y, and Z-axis: – 2000 degree/second. This means for example if the sensor is rotated in X-axis with a maximum angular velocity of 2000 degrees per second, the gyro X data will be a maximum value of 16bit integer variable: 32768. On the other hand, the readout value will be -32768 if the angular velocity is -2000 degrees per second. From here, we can come out with the conversion ratio from raw sensor data to real angular velocity: r = 32768 / full scale value = 32768 / 2000 = 16.384.
• Accelerometer full scale: – 16g.
• Fire interrupt signal when data is available. Clear interrupt flag whenever the data is completely readout.

Next, we need to configure DMA peripheral to connect with I2C as well:

NVIC_InitTypeDef NVIC_InitStructure;  
DMA_InitTypeDef  DMA_InitStructure;

DMA_DeInit(MPU6050_DMA_Channel); //reset DMA1 channe1 to default values;

DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)I2C_DR_Address; //=0x40005410 : address of data reading register of I2C1  
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)I2C_Rx_Buffer; //variable to store data  
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //channel will be used for peripheral to memory transfer  
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;    //setting normal mode (non circular)  
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;    //medium priority  
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;    //Location assigned to peripheral register will be source  
DMA_InitStructure.DMA_BufferSize = 14;    //number of data to be transfered  
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //automatic memory increment disable for peripheral  
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;    //automatic memory increment enable for memory  
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;    //source peripheral data size = 8bit  
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;    //destination memory data size = 8bit  
DMA_Init(MPU6050_DMA_Channel, &DMA_InitStructure);  
DMA_ITConfig(MPU6050_DMA_Channel, DMA_IT_TC, ENABLE);

NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel7_IRQn; //I2C1 connect to channel 7 of DMA1  
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x05;  
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x05;  
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;  
NVIC_Init(&NVIC_InitStructure);  

The purpose of this configuration is to connect the I2C1 RX with the memory buffer directly using the corresponding DMA channel. We have some important points to notice here in order for you to be able to edit the code yourself in the future if you use another I2C peripheral or different type of sensor with different bytes to transfer through DMA:

As you can see from the two tables from STM32F1 reference manual, there are 2 DMA blocks connected to different types of peripherals using different channels. Here we connect MPU6050 with I2C1 of STM32 so that the only choice is DMA1. Then, for reading data, we need to consider channel 7 that is connected to the RX register of I2C1 where all incoming data is stored. Later, if you want to use other peripherals with DMA in your own project, this table can be useful.

• What is the physical peripheral address:

Each peripheral inside the STM32 has a boundary address which can be found in Table 3 of the reference manual.

And inside that peripheral, there are several registers whose addresses are inside that peripheral boundary. For example, in our case, we need to locate the address of I2C_DR register (Data register) to assign to DMA controller.

Notice the “Offset” column in Table 189, it means the physical address of I2C_DR register will be offset from the initial address of I2C1 (0x40005400) by 0x10 -> I2C_DR address is 0x40005410

• The number of byte to transfer:

As mentioned before, 14 bytes will be read from MPU6050, so DMA_Buffersize here should be 14 bytes.

After finishing the configuration parts, we move to the reading part. MPU6050 uses INT pin to trigger STM32 to read out its data as set before and we can use External Interrupt peripheral to capture it. The interrupt routine is as follow:

void EXTI4_IRQHandler(void)  
  {
  if (EXTI_GetITStatus(MPU6050_INT_Exti))            //MPU6050_INT
    {
    EXTI_ClearITPendingBit(MPU6050_INT_Exti);
    #ifndef USE_I2C_DMA
      Prepare_Gyro_Data();    //Read out the accel and gyro data whenever interrupt occurs.
    #else
      I2C_DMA_Read(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,MPU6050);
    #endif
}
}

Noted the define “USE_I2C_DMA” I used to choose between regular way and DMA way of reading. Here the INT pin is connected to GPIO_Pin_4 of GPIOB so EXTI4 is activated. Then the I2C_DMA_Read function is presented:

void I2C_DMA_Read(u8 slaveAddr, u8 readAddr, u8 sensor)  
  {
  /* Disable DMA channel*/
  DMA_Cmd(MPU6050_DMA_Channel, DISABLE);
  /* Set current data number again to 14 for MPu6050, only possible after disabling the DMA channel */
  DMA_SetCurrDataCounter(MPU6050_DMA_Channel, 14);

  /* While the bus is busy */
  while(I2C_GetFlagStatus(MPU6050_I2C, I2C_FLAG_BUSY));

  /* Enable DMA NACK automatic generation */
  I2C_DMALastTransferCmd(MPU6050_I2C, ENABLE);                    //Note this one, very important

  /* Send START condition */
I2C_GenerateSTART(MPU6050_I2C, ENABLE);

  /* Test on EV5 and clear it */
  while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

  /* Send MPU6050 address for write */
  I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Transmitter); 

  /* Test on EV6 and clear it */
  while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));

  /* Clear EV6 by setting again the PE bit */
  I2C_Cmd(MPU6050_I2C, ENABLE);

  /* Send the MPU6050's internal address to write to */
  I2C_SendData(MPU6050_I2C, readAddr);

  /* Test on EV8 and clear it */
  while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

  /* Send STRAT condition a second time */
  I2C_GenerateSTART(MPU6050_I2C, ENABLE);

  /* Test on EV5 and clear it */
  while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_MODE_SELECT));

  /* Send MPU6050 address for read */
  I2C_Send7bitAddress(MPU6050_I2C, slaveAddr, I2C_Direction_Receiver);

  /* Test on EV6 and clear it */
  while(!I2C_CheckEvent(MPU6050_I2C, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));

  /* Start DMA to receive data from I2C */
  DMA_Cmd(MPU6050_DMA_Channel, ENABLE);
  I2C_DMACmd(MPU6050_I2C, ENABLE);

  // When the data transmission is complete, it will automatically jump to DMA interrupt routine to finish the rest.
  //now go back to the main routine
}

And DMA interrupts routine:

void DMA1_Channel7_IRQHandler(void)  
  {
  if (DMA_GetFlagStatus(DMA1_FLAG_TC7))
    {
    /* Clear transmission complete flag */
    DMA_ClearFlag(DMA1_FLAG_TC7);

    I2C_DMACmd(MPU6050_I2C, DISABLE);
    /* Send I2C1 STOP Condition */
    I2C_GenerateSTOP(MPU6050_I2C, ENABLE);
    /* Disable DMA channel*/
    DMA_Cmd(MPU6050_DMA_Channel, DISABLE);

    //Read Accel data from byte 0 to byte 2
    for(i=0; i<3; i  ) 
      AccelGyro[i]=((s16)((u16)I2C_Rx_Buffer[2*i] << 8)   I2C_Rx_Buffer[2*i 1]);
      //Skip byte 3 of temperature data
    //Read Gyro data from byte 4 to byte 6
    for(i=4; i<7; i  )
      AccelGyro[i-1]=((s16)((u16)I2C_Rx_Buffer[2*i] << 8)   I2C_Rx_Buffer[2*i 1]);    
    }
  }

Now, the reading sequence will be done automatically and stored into AccelGyro variable with the minimum time needed. I have also done a timing test to check how efficient this method could be. The following figures show the timing consumption of two methods: regular reading and DMA reading.

Channel 3 in both figures shows the timing period when the CPU is dealing with I2C reading. With the normal way of reading I2C data, the CPU is busy for the whole period and cannot do anything else. This could lead to a delay in reading other sensor data as well. By using DMA, we can free the CPU to do another task as DMA handles all the reading parts from the I2C peripheral.

Hope you can find this article useful and let’s wait for more to come 😀

Вычисление смещения с использованием акселерометра и гироскопа (mpu6050)

Я знаю, что это старый пост, но я думал, что опубликую некоторые исправления из экспериментов, которые я сделал с MPU 6050 и arduino.

Во-первых, уравнение, используемое вами для поиска смещения, неверно, вам нужно использовать уравнение кинематики.

однако уравнение Xf = 1/2at ^ 2 Vot Xo также неверно, потому что оно ТОЛЬКО для ПОСТОЯННЫХ ускорений.

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

Xf = 1/4 (Af Ao) t ^ 2 Vot Xo

Где Xf – конечное расстояние в метрах, Af – окончательное ускорение ТОКА в m/s ^ 2, Ao – предыдущее ускорение последнего набора данных в m/s ^ 2, t – ИЗМЕНЕНИЕ во времени BETWEEN Af и Ao устанавливает данных в SECONDS, Vo – мгновенная скорость последнего набора данных в м/с, а Xo – конечное расстояние последнего набора данных или сумма всех предыдущих расстояний в метрах.

Vo = 1/2 (Ao Ao-1) * t Vo-1

Где Vo – предыдущая мгновенная скорость в m/s, Ao – предыдущее ускорение в m/s ^ 2, Ao-1 – ускорение от двух наборов данных назад в m/s ^ 2, t – изменение времени между Ao и Ao-1 в SECONDS, а Vo-1 – мгновенная скорость в м/с набора данных Ao-1 или двух наборов данных назад.

Во-вторых, вам нужно использовать более надежные часы. Я рекомендую использовать функцию micros() и помнить, что t является CHANGE по времени между наборами данных. Я не уверен в надежности, но это лучшее, что я могу придумать. УБЕДИТЕСЬ, чтобы преобразовать из микросекунд в SECONDS при использовании указанных уравнений.

В-третьих, я рекомендую вам откалибровать ваши смещения так часто или даже каждый раз в самом начале вашего кода, комбинируя свой код с помощью калибровочного эскиза, такого как Luis Ródenas. Вы можете поместить его в процедуру setup() и можете использовать небольшое значение буфера или набор данных, например 200 или 300, чтобы вы не слишком долго ждали экспериментов.

В-четвертых, вы можете либо работать, используя среднее значение между двумя ускорениями набора данных (это то, что мы делаем выше), либо сделать это еще дальше, и использовать среднее количество различных наборов данных, например, использовать буферный массив fifo для хранения различных значений ускорения и принимать среднее значение всех значений в буфере.

Буферы Fifo требуют, чтобы определенное количество значений оставалось в них постоянно, но по мере того, как новое значение вступает в силу, уходит более старое. Чем больше fifo, тем более неточными будут расчеты расстояний, но буфер fifo позволит более высоким значениям ускорения слишком сильно влиять на ваши данные.

Xf = 1/2At ^ 2 Vot Xo

Vo = Aold * t Vo-1

Где A – новое среднее ускорение, полученное из гипотетического буфера FIFO, Aold – это старое среднее ускорение от последнего среднего FIFO, а t – изменение времени между двумя отдельными точками набора данных. Все в стандартных единицах, конечно, м/с и т.д.

Вы неплохо выполнили преобразование исходных значений ускорения в m/s ^ 2 путем деления на 16384 и умножения на 9.8m/s ^ 2. Значение 16384 зависит от стандартной настройки чувствительности -2g, которая может измениться, если вы выберете другую настройку, такую как -4g.

Наконец, даже при всех вышеперечисленных изменениях Чрезвычайно сложно получить точное считывание из-за многих различных факторов, таких как температура.

Важно поддерживать контролируемую среду для вашего акселерометра, используя вентиляторы или что угодно, чтобы поддерживать гироскоп/акселерометр при комнатной температуре или 25 ° C.

В библиотеке Jeff Rowberg MPU6050 есть функция для получения текущей температуры, mpu.getTemperature() Я верю.

Даже при всех этих изменениях все равно будет очень сложно получить хорошее точное считывание из-за математических причин и небольших неточностей.

Вы можете попробовать настроить свой гироскоп на менее чувствительную настройку, потому что я знаю, что mpu 6050 установлен по умолчанию -2g, более высокая настройка может помешать многим проблемам влиять на ваши показания, но это станет менее чувствительным к небольшим смещениям.

Всегда есть больше способов, с помощью которых вы можете оптимизировать свои значения гироскопа/акселерометра, одним из способов является увеличение частоты потока данных MPU для получения большего количества данных за определенный промежуток времени, это может позволить вам получать более точные показания, Я считаю, что в библиотеке Джеффа Роуберга есть функция, позволяющая это делать.

Комплементарный фильтр

При помощи угловой скорости невозможно измерить угол склонения робота, можно измерить только изменение этого угла, да и то постоянно будет накапливаться погрешность. В теории нам было бы достаточно показаний акселерометра, чтобы получить склонение робота вперед/назад или вправо/влево но акселерометр MPU6050 сильно шумит, его показания довольно сильно изменяются, хотя и вокруг одного значения.

Ниже приведен пример измерения угла склонения робота по показаниям акселерометра. Я наклонял робота плавно, но показания довольно сильно пляшут.

Но на помощь нам приходит комплементарный фильтр! В нем смешиваются в определенных пропорциях показания по гироскопу и акселерометру, что позволяет получать довольно точную картину наклона робота. К сожалению такой фильтр невозможно применить при повороте робота в горизонтальной плоскости — не будет задействованы показания гравитации в акселерометре.

При объединение показаний акселерометра и гироскопа на основе комплементарного фильтра можно рассчитать значение угла наклона по формуле:

где .

Итоговая величина является долевой суммой интегрированного значения показаний гироскопа и мгновенного значения показаний акселерометра

Доля показаний акселерометра мала, поскольку они умножаются на малую величину

Но благодаря акселерометру, компенсируются ошибки измерений, которые обязательно возникают и накапливаются, если пользоваться только гироскопом..

Коэффициент

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

Цитирование «Мобильные роботы на базе Arduino» закончено.

Используйте скетч MPU6050test для тестирования гироприбора.

1. Прошейте робота данной программой.

2. В Arduino IDE откройте «Монитор порта», установите скорость обмена 115200, как показано на следующем рисунке.

3. 
   Перезагрузите ESP32 кнопокой «Reset», не трогайте робота в течении 4х секунд, в это время производится расчет смещения показаний акселерометра.

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

Gyro =показания угловой скорости в радианах.

Acsel= показания угла наклона по акселерометру в радианах.

AcYsum =показания угла наклона Комплементарный фильтр.

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

Если при действии «наклон вперед» угловая скорость по оси имеет отрицательный знак, в дальнейшем требуется изменить знак (инвертировать). То же касается показаний угла Acsel. При проведении наклона вперед угловая скорость — положительна.

Запомним те изменения, которые внесены в типовой пример, они потребуются в полной программе балансировки.

Подключаем акселерометр / гироскоп mpu-6050 (gy-521) к arduino и серво двигателям (стабилизатор для камеры)

Теория и практика балансировки

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

Фактически при балансировке мы получаем колебательный контур,а колебания в контуре могут быть затухающими и незатухающими. Если наш робот колеблясь совершает каждый раз колебание с увеличивающейся амплитудой, то …. нужно что то подкрутить — изменить звенья обратной связи.

Хотя, пойдем от простого к сложному.

Все описанные далее формулы должны применяться к роботу периодически, через 5-10 миллисекунд.

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

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

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

скорость ответа колес робота Speed должна быть пропорциональна углу склонения робота Ang. Это первая фаза построения регулятора.

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

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

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

Допустим ситуацию, когда робот склонён но уже не падает, а поднимается, логично предположить, что в этом случае робот должен хотя-бы остановиться, но он, «видя» свой наклон, продолжит ехать вперед, что приведет в обратному склонению (перерегулированию) и в дальнейшем к падению.

As – угловая скорость (скорость падения) , Kd —коэффициент пропорциональности.

Если применить данную формулу, то можно невооруженным глазом заметить, что балансировка стала плавнее. Для подбора Kd, примите Kp = 0, и добейтесь существенного противодействия падению одним вторым звеном. Робот будет падать но должен существенно этому сопротивляться.

Но еще один вопрос как быть, если робот уже движется с определенной скоростью и падает, где и как мы учитываем текущую линейную скорость робота? – нигде. А это желательно сделать!

Если заданная скорость совпадает со скоростью движения, а это практически так для шаговых моторов, то формула превращается в следующую:

где Speed_old – текущая скорость.

Если теперь запустить робота, он сразу уйдет в критическое отклонение, это связано с тем, что Kp и Kd для данного режима будут велики и должны быть существенно снижены.

Настройка похожа на предыдущую: сначала принимаем Kd=0, добиваемся балансировки, затем Kp=0, также добиваемся существенного сопротивления робота падению, затем оба коэффициента снижаем вдвое от полученных значений и подключаем оба.

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

Вы уже задали вопрос — «почему робот не стоит на месте?» – пора задать!

Дело в том, что точно отцентрировать робота относительно гироприбора сложно и робот будет всегда немного наклонен вперед или назад, наклон задается смещением центра тяжести. Для компенсации этого наклона применяется обратная связь по пройденному расстоянию, а это не что иное как скорость умноженная на dt – время действие данной скорости. Так как скорость робота изменяется периодически, то следует использовать накопительную (интегральную) форму:

Move – пройденный роботом путь в относительных единицах (для шаговых моторов это могут быть шаги), dt- время действия скорости (период между опросами в секундах),

Ki —коэффициент пропорциональности.

Кажется все нормально, но на деле робот начинает колебаться с огромной амплитудой — изменяя Ki добейтесь этого. Дело в том, что работе интегральной составляющей Move нужно помогать останавливая робота за счет составляющей, которая уменьшается с уменьшением скорости робота, это как раз сама скорость робота — Speed, добавим ее небольшую часть для компенсации раскачивания робота интегральным звеном…

где Kki – коэффициент затухания для интегрального звена.

Теперь наш робот будет устойчиво стоять и даже если его толкнуть — вернет себе устойчивое равновесие. Но, он еще не может двигаться!

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

Если принять заданную скорость движения за SpZ, то формула примет вид:

Вот теперь робот будет двигаться с указанной скоростью и не только по равнине, но и в гору и с горы!

Можно и далее сглаживать движение робота, введением звеньев высшего порядка, но их влияние, как показывает практика, не велико.