Robot balansujący "Trybik"

[FlyingDutch]

Cześć,

w tym roku bardzo wcześnie zabrałem się za kupno prezentów świątecznych dla rodziny - tak mam już kupione wszystkie prezenty (szczególnie duzo dla wnuczki). Aby przeciwdziałać wypaleniu zawodowemu postanowiłem postanowiłem zrobić także sobie prezent. Mój wybór padł na robota balansującego ponieważ od dawna chodził mi po głowie pomysł budowy takiego robota. Części postanowiłem kupić na AliExpress, aby nie wyszedł za drogi. Nabyłem następujące podzespoły:

1) Podwozie z silnikami:

https://pl.aliexpress.com/item/1005006776833931.html?spm=a2g0o.order_list.order_list_main.100.65991c24Nn77Nu&gatewayAdapt=glo2pol

2) Sterownik silników L298N:

https://pl.aliexpress.com/item/1005004521836030.html?spm=a2g0o.order_list.order_list_main.41.65991c24Nn77Nu&gatewayAdapt=glo2pol

3) MPU 6050 (akcelometr i gyro):

https://pl.aliexpress.com/item/1005006995849520.html?spm=a2g0o.order_list.order_list_main.94.65991c24Nn77Nu&gatewayAdapt=glo2pol

4) Arduino UNO R3 (chiński klon) miałem w szufladzie

5) Ultradźwiękowy czujnik odległości HC-SR04 (miałem razem z mocowaniem)

6) Serwomechanizm dwu-osiowy do zamocowania czujnika ultradźwiękowego i kamery (na razie nie obsługiwany)

7) Kamera - na razie nie obsługiwana

7) Pojemnik na dwa ogniwa i ogniwa 18650 (dwie sztuki) oraz kawałki pleksy, kabelki,śrubki, kołki dystansowe także miałem

W ciagu jednego dnia złożyłem tak wyglądającego robota:

Część podzespołow na razie nie jest podłączona i obsługiwana - mam zamiar to zrobić w nastepnym etapie. Nie są obsługiwane:

1) serwomechanizm z zamocowaną kamerą i czujnikiem odległości

2) kamera

Na obudowie robota jest miejsce na płytkę z mikrokontrolerem ESP32 do którego mam zamiar podłączyć kamerę (na ESP32 można uruchomić bibliotekę OpenCV) i zrobić prostą obróbkę obrazu.

Jeśli chodzi o oprogramowanie (dla płytki Arduino UNO R3) to korzystałem z tego repozytorium na Githubie:

https://github.com/rppelayo/arduino-self-balancing-robot/tree/master

W tym repozytorium oprócz samego szkicu są także pliki potrzebnych bibliotek. Biblioteki te należy skopiować do katalogu "libraries" naszej wersji "Arduino IDE".

I tak należy utworzyć w "libraries" podkatalogi:

1) "MPU6050" - wrzucamy pliki nagłówkowe i żródła C z MPU6050 w nazwie

2) "I2Cdev" - wrzucamy pliki: I2Cdev.cpp i I2Cdev.h

3) "LMotorController" - wrzucamy pliki: LMotorController.h i LMotorController.cpp

4) "PID_v1" - wrzucamy pliki PID_v1.h i PID_v1.cpp

Robot jest zasilany z dwóch ogniw 18650 połączonych szeregowo co daje napięcie około 7,4 V (napięcie to jest także podawane na gniazdko zasilające Arduino UNO R3).

Napięcie 7,4 V jest także podawane na zasilanie sterownika silników L298N. Silniki są podłączone do wyjść sterownika L298N. Pinologia podłączeń jest następująca:

1) Sterownik silników L298N: "ENA"- pin 9 Arduino, "ENB"-10, "IN1"-3, "IN2"-4, "IN3"-5,"IN4"-6

Pod tym linkiem można sprawdzić jak są rozmieszczone wyprowadzenia na płytce kontrolera silników:

https://projecthub.arduino.cc/lakshyajhalani56/l298n-motor-driver-arduino-motors-motor-driver-l298n-7e1b3b

a w następnym linku opis sterownika:

https://botland.com.pl/sterowniki-silnikow-moduly/8227-dwukanalowy-sterownik-silnikow-l298n-modul-wb291111-iduino-st1112-5903351241236.html?cd=18298825651&ad=&kd=&gad_source=1&gclid=CjwKCAiA6aW6BhBqEiwA6KzDc54vIlaPMLV-OkuUD1sdPH5eRLeIe25a7LllSrCWE2T_prWmjEVndxoCdS0QAvD_BwE

Dla  modułu MPU6050 podłączenia do Arduino UNO są następujące:

"SCL" z płytki MPU do pinu SCL Arduino

"SDA" z lytki MPU do pinu SDA Arduino

"INT" z płytki MPU -do pin 2 Arduino

"VCC" z płytki MPU do 5V Arduino

GND MPU  do GND Arduino

Pod linkiem poniżej opis MPU6050:

https://circuitdigest.com/microcontroller-projects/interfacing-mpu6050-module-with-arduino

Dla czujnika ultradźwiękowego podłączenia są następujące:

"VCC" do 5V Arduino

GND do GND

"Trig" do pinu 12 Arduino

"Echo" do pinu 11 Arduino

Miałem trochę problemów z uruchomieniem oprogramowania:

1) Na początku nie podłączyłem pinu "INT" z płytki MPU6050 do Arduino UNO, a program wykorzystywał odbiór danych z MPU właśnie za pomocą przerwania

2) Jak już podłączyłem pin "INT" do Arduino to generacja przerwania zewnętrznego nie działała - było to spowodowane faktem, że oryginalny kod był pisany dla Arduino Leonardo i tam pin 0 obsługiwał przerwanie zewnętrzne. Dla Arduino UNO tylko piny numer 2 i 3 obsługują zewnętrzne przerwania. Po podłączerniu pinu "INT" do pinu 2 Arduino UNO i zmianie kodyu w szkicu odbiór danych z MPU zaczął działać poprawnie

3) Okazało się, że kąt odchyłki od pionu był liczony w innej płaszczyźnie w MPU niż myślałem, po obróceniu płytki MPU na obudowie o 90 stopni zaczęło to działać poprawnie:

4) ostatnia rzecz musiałem dla drugiego silnika zamienić miejscami piny sterujące IN3 i IN4, aby silnik kręcił się w dobrą stronę

Poniżej zmodyfikowany przeze mnie kod programu, który mi zadziałał poprawnie:

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

#define MIN_ABS_SPEED 20
#define LOG_INPUT 1

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

//PID
double originalSetpoint = 175.8;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

#define interruptPin  2
double motorSpeedFactorLeft = 0.7;
double motorSpeedFactorRight = 0.7;
//MOTOR CONTROLLER
int ENA = 9;
int IN1 = 3;
int IN2 = 4;
int IN3 = 5;
int IN4 = 6;
int ENB = 10;
//LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
LMotorController motorController(ENA, IN1, IN2, ENB, IN4, IN3, motorSpeedFactorLeft, motorSpeedFactorRight);

//timers
long time1Hz = 0;
long time5Hz = 0;

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}


void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    
    pinMode(interruptPin, INPUT_PULLUP);
    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        //attachInterrupt(0, dmpDataReady, RISING);
        attachInterrupt(digitalPinToInterrupt(interruptPin), dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
}


void loop()
{
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors
        
        pid.Compute();
        motorController.move(output, MIN_ABS_SPEED);
        Serial.println(output);     
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #if LOG_INPUT
//            Serial.print("ypr\t");
//            Serial.print(ypr[0] * 180/M_PI);
//            Serial.print("\t");
//            Serial.print(ypr[1] * 180/M_PI);
//            Serial.print("\t");
//            Serial.println(ypr[2] * 180/M_PI);
        #endif
        input = ypr[1] * 180/M_PI + 180;
   }
}

 

Uwaga! - kod zadziałał poprawnie robot utrzymywał równowagę podczas ruchu, ale musiałem podczepić z tyłu taśmą dwie baterie 18650 jako przeciw wagę dla baterii zasilających. Bez tej przeciw wagi robot nie potrafił utrzymywać równowagi - przechylał się po czasie do przodu, gdzie znajdował się pojemnilk na baterie 18650.

Jak wyważę robota poprawnie (dodam regulowaną przeciw wage) i wkomponuje obsługę czujnika odległości HC-SR04, tak, aby robot omijał przeszkody to zrobię film i podam link do niego w tym poście. Ogólnie robot wyszedł dość ciężki i duży, a to nie pomaga w utrzymywaniu równowagi.

Pozdrawiam

Edytowano Grudzień 3, 2024 przez FlyingDutch

[Treker (Damian Szymański)]

@FlyingDutch dzięki za szczegółowy opis - bardzo dawno nie było tutaj takiego robota  

[FlyingDutch]

54 minuty temu, Treker napisał:

@FlyingDutch dzięki za szczegółowy opis - bardzo dawno nie było tutaj takiego robota  

Cześć @Treker,

jeszcze trochę zostało do zrobienia - mówię tu o nie podłączonych elementach i ulepszenie oprogramowania. Postaram się to w najbliższym czasie dopracować.

Pozdrawiam

[jand]

Gratuluję - w szczególności tempa poskładania całości do kupy (jeden dzień).

Jednak w opisie zabrakło mi najciekawszego elementu - zastosowanej procedury strojenia regulatora PID.

Poza tym obecnie chyba nie warto kupować staruteńkiego L298N, ja bym zastosował raczej coś takiego

 

Edytowano Grudzień 4, 2024 przez jand

[FlyingDutch]

18 godzin temu, jand napisał:

Jednak w opisie zabrakło mi najciekawszego elementu - zastosowanej procedury strojenia regulatora PID.

Cześć,

na razie w ogóle nie dostrajałem parametrów regulatora PID, a to z tego powodu, że parametry które były w kodzie szkicu pobranym z Githuba zadziałały poprawnie. Oczywiście nie jest to dobór optymaly i dlatego w najbliższej przyszłości mam zamiar (podczas ulepszania programu) zastosować jedną ze znanych metod doboru nastaw eregulatora PID - np. metodę Zieglera-Nicholsa:

https://home.agh.edu.pl/~romus/Podstawy Automatyki/Lab 6 Dostrajanie_PID.pdf

W najbliższych dniach będę miał na to czas najprędzej w sobotę - opiszę wtedy moje dziłania.

Co do sterownika silników to masz absolutną rację - wybrałem ten model, bo był on użyty w kodzie z Github'a i nie chciałem tego na początku zmieniać.

BTW: zastanawiam się, czy OpenCV na ESP32 z kamerą działa wystarczająco wydajnie, aby zastosować je do prostego podążania za obiektem np. człowiekiem?

Pozdrawiam

Edytowano Grudzień 5, 2024 przez FlyingDutch

[jand]

Do planów rozwojowych dodałbym jeszcze:

• Skalibrowanie MPU6050. W tej chwili ładujesz jako poprawki wartości, które nijak się mają do Twojego konkretnego egzemplarza. 
• Wykorzystanie enkoderów, które masz przy silnikach. Taki robot zachowuje się bardziej stabilnie, gdy dodatkowy regulator PID steruje tak wartością zadaną pochylenia, by utrzymać stałą prędkość jazdy (w szczególności równą zeru dla robota mającego stać w miejscu).

 

[jand]

Dnia 3.12.2024 o 08:30, FlyingDutch napisał:

Bez tej przeciw wagi robot nie potrafił utrzymywać równowagi

Jako wartość zadaną wychylenia przyjąłeś niezmienioną wielkość podaną w oryginalnym  programie:

double originalSetpoint = 175.8;

która jest dość odległa od teoretycznej wartości 180 stopni. Rozbieżności wynikają z niedokładności montażu robota (płaszczyzna czujnika nie jest idealnie prostopadła do osi robota) i w przypadku każdej konstrukcji optymalna wartość jest nieco inna.

Zastosuj tu na początku 180, a potem - obserwując zachowanie robota - dostosuj jej wartość tak, by osiągnąć najlepsze rezultaty.

Mam nadzieję że wtedy dodatkowe baterie jako obciążniki nie będą potrzebne.

[FlyingDutch]

40 minut temu, jand napisał:

Jako wartość zadaną wychylenia przyjąłeś niezmienioną wielkość podaną w oryginalnym  programie:

double originalSetpoint = 175.8;

która jest dość odległa od teoretycznej wartości 180 stopni. Rozbieżności wynikają z niedokładności montażu robota (płaszczyzna czujnika nie jest idealnie prostopadła do osi robota) i w przypadku każdej konstrukcji optymalna wartość jest nieco inna.

Zastosuj tu na początku 180, a potem - obserwując zachowanie robota - dostosuj jej wartość tak, by osiągnąć najlepsze rezultaty.

Hej próbowałem wartości 178.9 ale nie pomogło to.

Próbowałem też dobrać parametry dla regulatora PID - metodą Zieglera-Nicholsa (jak w podanym dokumencie) i wyszły mi wartości:

Kp = 150

Ki = 135

Kd  = 180

ale były to za duże wartości bo w rzeczywistym układzie regulator dostawał oscylacji.

Pozdrawiam

Edytowano Grudzień 5, 2024 przez FlyingDutch

[ethanak]

@FlyingDutch Skąd te 178.9? Robiłem kiedyś parę konstrukcji z tym modułem i wszędzie miałem kalibrację akcelerometru.

[jand]

Rzeczywiście te teoretycznie dobrane wartości są całkiem inne od tych, przy których Twój robot zachowuje się w miarę poprawnie, ale wiem z doświadczenia, że dobranie odpowiednich parametrów jest żmudne.

Można też, aby uniknąć wielokrotnego kompilowania programu, zastosować potencjometry do regulowania wartości parametrów w trakcie pracy robota. Jest to niby dodatkowa fatyga, ale per saldo może sie opłacać.

[FlyingDutch]

Cześć,

zrobiłem przeciwwagę - wystarczył kawałek pleksy z pojemnikiem na baterie. Ręcznie pozmieniałem trochę parametry PID i teraz robot zachowuje się całkiem poprawnie. Oto wartości parametrów PID'a :

//PID
double originalSetpoint = 180;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth
double Kp = 58.0;//50;
double Kd = 1.9;//1.4;
double Ki = 68;//60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

#define interruptPin  2
double motorSpeedFactorLeft = 0.85;
double motorSpeedFactorRight = 0.85;

Zrobiłem krótki filmik z jazdy robota:

Muszę wkomponować czujnik ultradźwiekowy do programu, aby robot omijał przeszkody i dodać jakieś proste sterowanie kierunkiem jazdy (zdalnie).

Pozdrawiam

Edytowano Grudzień 5, 2024 przez FlyingDutch

[ethanak]

Ja bym dał ToF a nie ultradźwięk - pamiętaj że musisz wyłaczyć przewania na czas pomiaru, a to może być niespecjalnie dobre dla reszty robota zakładając, że to UNO.

[jand]

Na filmiku robot zachowuje się w miarę spokojnie, ale ma tendencję do uciekania - powinien stać w miarę w jednym miejscu, a pchnięty powracać do punktu wyjścia.

[jand]

1 godzinę temu, ethanak napisał:

musisz wyłaczyć przewania na czas pomiaru

Niekoniecznie.

Jest taka odmiana czujnika (oznaczona "2020"), która mierzy odległość autonomicznie i przesyła gotowy wynik.

[ethanak]

Owszem, ale to nie ten czujnik