Mobilny robot z 4-osiowym ramieniem

PiotrSz03 · Marzec 17, 2025

W ramach projektu stworzony został mobilny robot z 4 osiowym ramieniem – oparty o mikrokontroler ESP32. Sterowanie zarówno silnikami DC jak i serwonapędami odbywa się poprzez WiFi (aplikację Blynk). Dzięki wykorzystaniu mostka H (L298N) możliwy jest wybór prędkości jak i kierunku obrotu kół. Cały robot powstał dzięki połączeniu dwóch dostępnych w internecie kitów z własnymi elementami.

Na marginesie dodam, że projekt był robiony jako zaliczenie programowania mikroprocesorów na 3 semestrze studiów – sam pomysł jest całkowicie nasz własny.

Zdjęcie interfejsu:

Zdjęcie robota:

Schemat połączeń:

Kod programu:

#define BLYNK_TEMPLATE_ID "" //Blynk template id
#define BLYNK_TEMPLATE_NAME "" //Blynk template name
#define BLYNK_AUTH_TOKEN "" //Blynk auth token

// Biblioteki
#include <BlynkMultiClient.h>
#include <WiFi.h>
#include <ESP32Servo.h>

// Ustawienia WiFi
const char* ssid = ""; //ssid
const char* pass = ""; //password

// Obiekt serwomechanizmu
Servo base;      //obiekt serwa podstawy
Servo arm1;        // obiekt serwa ramienia 1
Servo arm2;        // obiekt serwa ramienia 2
Servo gripper;       // obiekt serwa chwytaka

short basepos = 80;     //pozycja podstawy robota
short armpos1 = 0;      // pozycja ramienia pierwszego
short armpos2 = 90;      // pozycja ramienia drugiego
short gripos = 90;       // pozycja chwytaka

//obsluga przyciskow cyfrowych
bool forward = false;
bool reverse = false;
bool left = false;
bool right = false;
bool speedup = false;
bool slowdown = false;

int speed = 150;
int actual_speed;

//Zdefiniowanie wirtualnych pinów
//Sygnały cyfrowe
#define VIRTUAL_PIN_0 V0 //Forward
#define VIRTUAL_PIN_1 V1 //Reverse
#define VIRTUAL_PIN_2 V2 //Left
#define VIRTUAL_PIN_3 V3 //Right
#define VIRTUAL_PIN_4 V4 //Speed up
#define VIRTUAL_PIN_5 V5 //Speed down
//Sygnały analogowe
#define VIRTUAL_PIN_6 V6 //Base position
#define VIRTUAL_PIN_7 V7 //Arm position1
#define VIRTUAL_PIN_8 V8 //Arm position2
#define VIRTUAL_PIN_9 V9 //Grip position
#define VIRTUAL_PIN_10 V10 //Speed display

static WiFiClient blynkWiFiClient;

// Łączenie się z WiFi
void connectWiFi()
{
  Serial.print("Connecting to "); //Wyswietlanie z jakim WiFi sie laczymy
  Serial.println(ssid);

  if (pass && strlen(pass)) { 
    WiFi.begin((char*)ssid, (char*)pass);
  } else {
    WiFi.begin((char*)ssid);
  }

  while (WiFi.status() != WL_CONNECTED) { 
    delay(500);
    Serial.print(".");
  }
}

//Definiowanie pinow h bridge
const int in1 = 26;
const int in2 = 27;
const int in3 = 14;  
const int in4 = 12;
const int enA = 25;
const int enB = 13;


//funkcje sterowania silnikami dc
int F_actual_speed(){

  actual_speed = speed;
  return actual_speed;

}

int F_speedup(){

  speed = speed + 50;
  if (speed > 250) {
    speed = 250;
}

  return speed;
}

int F_slowdown(){

  speed = speed - 50;
  if (speed < 150) {
    speed = 150;
}
  return speed;
}


void F_forward(int actual_speed){
  
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, actual_speed);
  analogWrite(enB, actual_speed);
}

void F_stop(int actual_speed){
  
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  analogWrite(enA, actual_speed);
  analogWrite(enB, actual_speed);
}

void F_left(int actual_speed){

  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, actual_speed);
  analogWrite(enB, actual_speed);

}

void F_right(int actual_speed){

  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  analogWrite(enA, actual_speed);
  analogWrite(enB, actual_speed);
}

void F_reverse(int actual_speed){

  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, actual_speed);
  analogWrite(enB, actual_speed);
}


//Sterowanie silnikami dc
BLYNK_WRITE(V0) {
  forward = param.asInt();
  if (forward == 1) {
  actual_speed = F_actual_speed(); // Pobranie aktualnej prędkości
  F_forward(actual_speed);        // Przekazanie jej do funkcji
} 
  else {
  actual_speed = F_actual_speed();
  F_stop(actual_speed);
}
}
BLYNK_WRITE(V1) {
  reverse = param.asInt();
  if (reverse == 1) {
  actual_speed = F_actual_speed();
  F_reverse(actual_speed);
} 
  else {
  actual_speed = F_actual_speed();
  F_stop(actual_speed);
}  
}
BLYNK_WRITE(V2) {
  left = param.asInt(); 
  if (left == 1) {
  actual_speed = F_actual_speed();
  F_left(actual_speed);
}  
  else {
  actual_speed = F_actual_speed();
  F_stop(actual_speed);
}
}
BLYNK_WRITE(V3) {
  right = param.asInt();  
  if (right == 1) {
  actual_speed = F_actual_speed();
  F_right(actual_speed);
} 
  else {
  actual_speed = F_actual_speed();
  F_stop(actual_speed);
}
}
BLYNK_WRITE(V4) {
  speedup = param.asInt();
  if (speedup == 1) {
  F_speedup();
  actual_speed = F_actual_speed();
  Serial.println(actual_speed);
}   
}
BLYNK_WRITE(V5) {
  slowdown = param.asInt();  
  if (slowdown == 1) {
  F_slowdown();
  actual_speed = F_actual_speed();
}
}

//sterowanie servomechanizmami
BLYNK_WRITE(V6) {
  int basepos = param.asInt(); // Pobierz wartość z suwaka
  base.write(basepos);  // Ustaw kąt serwomechanizmu
}
BLYNK_WRITE(V7) {
  int armpos1 = param.asInt(); // Pobierz wartość z suwaka
  arm1.write(armpos1);  // Ustaw kąt serwomechanizmu
}
BLYNK_WRITE(V8) {
  int armpos2 = param.asInt(); // Pobierz wartość z suwaka
  arm2.write(armpos2);  
  delay(50);
  arm2.write(90);
}
BLYNK_WRITE(V9) {
  int gripos = param.asInt(); // Pobierz wartość z suwaka
  gripper.write(gripos);  
  delay(100);
  gripper.write(90);
}


void setup() {

  Serial.begin(115200);

  connectWiFi();

  // Ustawienie Blynk
  Blynk.addClient("WiFi", blynkWiFiClient, 80);
  Blynk.config(BLYNK_AUTH_TOKEN);

  //Konfiguracja servo
  base.attach(5);
  arm1.attach(18);
  arm2.attach(19);
  gripper.attach(21);

  //Ustawienie pozycji ramienia
  base.write(basepos);
  arm1.write(armpos1);
  arm2.write(armpos2);
  gripper.write(gripos);

  //KONTROLA PREDKOSCI
  pinMode(enA, OUTPUT);  
  analogWrite(enA, 50);   //ENABLE A - PWM

  pinMode(enB, OUTPUT);  
  analogWrite(enB, 50);    //ENABLE B - PWM


  // H BRIDGE - INPUT 1,2,3,4 - KONTROLA KIERUNKU
  pinMode(in1, OUTPUT);  
  digitalWrite(in1, LOW);

  pinMode(in2, OUTPUT);  
  digitalWrite(in2, LOW);

  pinMode(in3, OUTPUT);  
  digitalWrite(in3, LOW);

  pinMode(in4, OUTPUT);  
  digitalWrite(in4, LOW);
}

void loop() {
  
   // Reconnect WiFi
  if (WiFi.status() != WL_CONNECTED) {
    connectWiFi();
    return;
  }

  //Blynk.virtualWrite(V10, actual_speed); //Wyświetlanie prędkości w aplikacji

  // Obsługa klienta Blynk
  Blynk.run();
}

Działanie mostka H:

Poważnie rozważaliśmy wprowadzenie współbieżności programu – uznaliśmy jednak za niepotrzebne w tym zastosowaniu, przez aplikację raczej będziemy klikać tylko jeden przycisk naraz. Filmik z działania:

Największymi wyzwaniami podczas tworzenia projektu okazały się:

Konstrukcja mechaniczna – zamówione elementy okazały się wyjątkowo mało wytrzymałe, nie do końca pasowne. Dużo pracy pochłonęła stabilizacja wszystkiego
Komunikacja przez wifi – nie mając doświadczenia napociliśmy się niemiłosiernie próbując zmusić robota do „gadania”.
Niepewność oryginalnych połączeń elektrycznych – wtyczki wypadały przy ruchu, trzeba było to odpowiednio usztywniać. Być może najlepiej byłoby zlutować.

Linki do przykładowych elementów: