Przeszukaj forum

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