Робот балансир на ардуино

Нам понадобится:

Arduino: http://ali.pub/1x1jri

Драйвер двигателей L298N: http://ali.pub/1wptjz

2 моторредуктора: http://ali.pub/1x1kic

Гироскоп: http://ali.pub/1x1lem

Аккумуляторы 18650: http://ali.pub/1x1llm

Отличная зарядка 18650: http://ali.ski/vHvuN

box под 18650: http://ali.pub/1x1mse

Макетная плата: http://ali.ski/mvDWz2

Контактные провода: http://ali.pub/1th4xf

Прежде чем приступить к материалу, я Вас попрошу, если нравится то, что я делаю и хотите следить за моей деятельностью, то рекомендую подписаться на мой телеграмм канал: https://t.me/ypavla
Там я публикую новости о вышедших видео, статьях и разные устройства для умного дома и не только показываю.
Спасибо за внимание, теперь продолжим.

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

Размер боковин балансира указан на фото:

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

С вертикальными частями разобрались, теперь нужно сделать 3 горизонтальных. Я их склеивал тоже между собой, поэтому сделал вот таких частей 6 шт.:

После склейки должна получиться лесенка. Все детали я склеивал между собой термоклеем, картон он очень хорошо склеивает.

Моторредукторы я крепил на стяжки к боковинам получившейся “лесенки”

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

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

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

ВАЖНО: Гироскоп нужно расположить ровно в плоскости, иначе роботу будет тяжело держать равновесие.

Ну и сами аккумуляторы прикрепил на стяжки в самом верху конструкции.

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

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

Ну и собственно сам скетч:

#include <Wire.h>
#include "Kalman.h"
#include "PID_v1.h"

Kalman kalmanY;

uint8_t IMUAddress = 0x68;

/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;

int16_t gyroY;

double accYangle;
double gyroYangle = 180;
double compAngleY = 180;
double kalAngleY;

double in, out, setpoint;
PID pid(&in, &out, &setpoint, 30, 410 , 0.8, DIRECT);

uint32_t timer;

void setup() {  
  Serial.begin(115200);
  Wire.begin();  
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("MPU-6050 with address 0x"));
    Serial.print(IMUAddress,HEX);
    Serial.println(F(" is not connected"));
    while(1);
  }      
  kalmanY.setAngle(180);

  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255,255);
  pid.SetSampleTime(10);
  setpoint = 181.8;
  
  timer = micros();
}

double ang;
int pwm;

void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  gyroY = ((data[10] << 8) | data[11]);
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
  double gyroYrate = -((double)gyroY/131.0);
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
  
  in = kalAngleY;
  pid.Compute();
  if (out < 0) {
    pwm = -1 * out;
    analogWrite(11, 0);
    analogWrite(10, pwm);
  } else {
    pwm = out;
    analogWrite(10, 0);
    analogWrite(11, pwm);
  }
  
  delay(1); // The accelerometer's maximum samples rate is 1kHz
}


void i2cWrite(uint8_t registerAddress, uint8_t data){
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data);
  Wire.endTransmission(); // Send stop
}

uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];  
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.endTransmission(false); // Don't release the bus
  Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
  for(uint8_t i = 0; i < nbytes; i++)
    data[i] = Wire.read();
  return data;
}

Для корректной работы скетча нам еще понадобятся две библиотеки kalman и PID_v1

Все это можно скачать здесь: https://yadi.sk/d/_IPFIZLX3NkEqg

Ну и собственно демонстрация работы:

Нам понадобится:

Footer