ESP 32 – Kontroler

https://hr.szks-kuongshun.com/info/mastering-the-esp32-a-comprehensive-guide-88876427.html

Kako koračni motor radi:

https://cnc-centar.ba/aktuelnosti/sta-je-step-koracni-motor/

Simulator za stepper motore:

https://wokwi.com/projects/433915938304722945

const int dirPin = 4;
const int stepPin = 5;
const int stepsPerRevolution = 200;
 
void setup()
{
  // Declare pins as Outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
}
void loop()
{
  // Set motor direction clockwise
  digitalWrite(dirPin, HIGH);
 
  // Spin motor slowly
  for(int x = 0; x < stepsPerRevolution; x++)
  {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(2000);
  }
  delay(1000); // Wait a second
  
  // Set motor direction counterclockwise
  digitalWrite(dirPin, LOW);
 
  // Spin motor quickly
  for(int x = 0; x < stepsPerRevolution; x++)
  {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
  delay(1000); // Wait a second
}

const int dirPin = 2;
const int stepPin = 3;
const int stepsPerRevolution = 200;
 
void setup()
{
  // Declare pins as Outputs
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
}
void loop()
{
  // Set motor direction clockwise
  digitalWrite(dirPin, HIGH);
 
  // Spin motor slowly
  for(int x = 0; x < stepsPerRevolution; x++)
  {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(2000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(2000);
  }
  delay(1000); // Wait a second
  
  // Set motor direction counterclockwise
  digitalWrite(dirPin, LOW);
 
  // Spin motor quickly
  for(int x = 0; x < stepsPerRevolution; x++)
  {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
  delay(1000); // Wait a second
}

Izrada robota kontroler i stepper motor:

https://how2electronics.com/control-stepper-motor-with-a4988-driver-arduino/

Instalacija kontrolera:

Installing the ESP32 Board in Arduino IDE (Windows, Mac OS X, Linux)

Pokretanje robota

#include <AccelStepper.h>

// Pinovi za prvi motor
#define dirPin1 25
#define stepPin1 26

// Pinovi za drugi motor
#define dirPin2 27
#define stepPin2 14

// Tip drajvera (1 = DRIVER)
#define motorInterfaceType 1

// Kreiranje objekata za oba motora
AccelStepper motor1(motorInterfaceType, stepPin1, dirPin1);
AccelStepper motor2(motorInterfaceType, stepPin2, dirPin2);

void setup() {
  // Postavi maksimalnu brzinu za oba motora
  motor1.setMaxSpeed(1000);
  motor2.setMaxSpeed(1000);

  // Postavi istu brzinu naprijed za oba motora
  motor1.setSpeed(300);
  motor2.setSpeed(300);
}

void loop() {
  // Pokreni oba motora unaprijed
  motor1.runSpeed();
  motor2.runSpeed();
}

Kretanje robota i skretanje

#include <AccelStepper.h>
#include <math.h>

// Motor pinovi i postavke
#define dirPinLeft 25
#define stepPinLeft 26
#define dirPinRight 27
#define stepPinRight 14
#define motorInterfaceType 1

AccelStepper motorLeft(motorInterfaceType, stepPinLeft, dirPinLeft);
AccelStepper motorRight(motorInterfaceType, stepPinRight, dirPinRight);

// Parametri robota
const float promjerKotaca = 6.4; // cm
const float opsegKotaca = 3.1416 * promjerKotaca;
const int koraciPoOkretaju = 200;
const int mikrostep = 16;
const float koraciPoCm = (koraciPoOkretaju * mikrostep) / opsegKotaca;
const float razmakIzmeduKotaca = 12.0; // cm

// Trenutna pozicija robota
float pozX = 0.0, pozY = 0.0, orijentacija = 0.0;

void setup() {
  motorLeft.setMaxSpeed(1000);
  motorRight.setMaxSpeed(1000);
}

void loop() {
  idiDo(0, 20);   // Idi naprijed 20 cm
  delay(1000);

  idiDo(20, 20);  // Skreni desno i idi 20 cm
  delay(1000);

  idiDo(20, 0);   // Skreni opet i idi dolje
  delay(5000);
}

void idiNaprijed(float cm) {
  long koraci = cm * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(300);     // naprijed
  motorRight.setSpeed(-300);   // naprijed (zrcalno)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void skreniDesno(float stupnjeva) {
  float putanja = (3.1416 * razmakIzmeduKotaca) * (stupnjeva / 360.0);
  long koraci = putanja * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(300);    // naprijed
  motorRight.setSpeed(300);   // unatrag (zrcalno montiran)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void skreniLijevo(float stupnjeva) {
  float putanja = (3.1416 * razmakIzmeduKotaca) * (stupnjeva / 360.0);
  long koraci = putanja * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(-300);   // unatrag
  motorRight.setSpeed(-300);  // naprijed (zrcalno montiran)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void idiDo(float x, float y) {
  float dx = x - pozX;
  float dy = y - pozY;
  float d = sqrt(dx * dx + dy * dy);
  float kut = atan2(dx, dy) * 180.0 / 3.1416;

  float deltaKut = kut - orijentacija;
  while (deltaKut > 180) deltaKut -= 360;
  while (deltaKut < -180) deltaKut += 360;

  if (deltaKut > 0) {
    skreniDesno(deltaKut);
    orijentacija += deltaKut;
  } else {
    skreniLijevo(-deltaKut);
    orijentacija += deltaKut;
  }

  while (orijentacija < 0) orijentacija += 360;
  while (orijentacija >= 360) orijentacija -= 360;

  idiNaprijed(d);

  pozX = x;
  pozY = y;
}

Ultrazvučni senzor

#include <AccelStepper.h>

// Pinovi za motore
#define DIR_L 25
#define STEP_L 26
#define DIR_R 27
#define STEP_R 14

AccelStepper motorL(1, STEP_L, DIR_L);
AccelStepper motorR(1, STEP_R, DIR_R);

// Ultrazvučni senzor
#define TRIG 32
#define ECHO 13

const int brzina = 300;         // Konstantna brzina kretanja
const float prag = 15.0;        // Prag udaljenosti u cm

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  motorL.setMaxSpeed(brzina);
  motorR.setMaxSpeed(brzina);
}

float izmjeriUdaljenost() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  long trajanje = pulseIn(ECHO, HIGH);
  return trajanje * 0.034 / 2;
}

void loop() {
  float udaljenost = izmjeriUdaljenost();
  Serial.print("Udaljenost: ");
  Serial.print(udaljenost);
  Serial.println(" cm");

  if (udaljenost > prag) {
    motorL.setSpeed(brzina);
    motorR.setSpeed(brzina); // Oba motora naprijed
  } else {
    motorL.setSpeed(0);
    motorR.setSpeed(0);      // Zaustavi se
  }

  motorL.runSpeed();
  motorR.runSpeed();
}

#include <AccelStepper.h>

// Pinovi za motore
#define DIR_L 25
#define STEP_L 26
#define DIR_R 27
#define STEP_R 14

AccelStepper motorL(1, STEP_L, DIR_L);
AccelStepper motorR(1, STEP_R, DIR_R);

// Ultrazvučni senzor
#define TRIG 32
#define ECHO 13

const int brzina = 1000;        // Konstantna brzina kretanja
const float prag = 15.0;        // Prag udaljenosti u cm

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);

  motorL.setMaxSpeed(brzina);
  motorR.setMaxSpeed(brzina);
}

float izmjeriUdaljenost() {
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  long trajanje = pulseIn(ECHO, HIGH);
  return trajanje * 0.034 / 2;
}

void loop() {
  float udaljenost = izmjeriUdaljenost();
  Serial.print("Udaljenost: ");
  Serial.print(udaljenost);
  Serial.println(" cm");

  if (udaljenost > prag) {
    motorL.setSpeed(brzina);
    motorR.setSpeed(brzina); // Ide ravno naprijed
  } else {
    motorL.setSpeed(0);
    motorR.setSpeed(0);
  }

  motorL.runSpeed();
  motorR.runSpeed();
}

#include <AccelStepper.h>

// PINS za motore
#define dirPin1 25
#define stepPin1 26
#define dirPin2 27
#define stepPin2 14

// Ultrazvučni senzor
const int trigPin = 32;
const int echoPin = 13;

// Parametri za motor
#define motorInterfaceType 1
AccelStepper motorDesni(motorInterfaceType, stepPin1, dirPin1);
AccelStepper motorLijevi(motorInterfaceType, stepPin2, dirPin2);

// Parametri robota
const float korakaPoOkr = 200.0;
const float promjerKotacaCM = 6.4;
const float opsegKotaca = 3.1416 * promjerKotacaCM;

// Senzor udaljenosti
#define SOUND_SPEED 0.034
#define SAFE_DISTANCE_CM 20.0  // minimalna udaljenost za zaustavljanje

long duration;
float distanceCm;

// Pretvori cm u korake
int cmUKorake(float cm) {
  return int(cm * korakaPoOkr / opsegKotaca);
}

// Funkcija za mjerenje udaljenosti
float mjeriUdaljenostCm() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  duration = pulseIn(echoPin, HIGH, 20000); // Timeout 20ms
  float udaljenost = duration * SOUND_SPEED / 2;
  return udaljenost;
}

// Idi naprijed s provjerom udaljenosti
void idiNaprijedSigurno(float cm, int brzina = 500) {
  int koraci = cmUKorake(cm);
  motorDesni.setSpeed(brzina);
  motorLijevi.setSpeed(brzina);
  for (int i = 0; i < koraci; i++) {
    distanceCm = mjeriUdaljenostCm();
    if (distanceCm < SAFE_DISTANCE_CM) {
      motorDesni.setSpeed(0);
      motorLijevi.setSpeed(0);
      Serial.println("? Prepreka detektirana! Zaustavljanje.");
      break;
    }
    motorDesni.runSpeed();
    motorLijevi.runSpeed();
  }
}

// Postavi okretanje, bez provjere udaljenosti
void okreniLijevo(int koraka, int brzina = 500) {
  motorDesni.setSpeed(brzina);
  motorLijevi.setSpeed(-brzina);
  for (int i = 0; i < koraka; i++) {
    motorDesni.runSpeed();
    motorLijevi.runSpeed();
  }
}

void okreniDesno(int koraka, int brzina = 500) {
  motorDesni.setSpeed(-brzina);
  motorLijevi.setSpeed(brzina);
  for (int i = 0; i < koraka; i++) {
    motorDesni.runSpeed();
    motorLijevi.runSpeed();
  }
}

void setup() {
  Serial.begin(115200);
  motorDesni.setMaxSpeed(1000);
  motorLijevi.setMaxSpeed(1000);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.println("? Pokretanje robota...");
}

void loop() {
  Serial.println("➡️ Krećem naprijed 100 cm");
  idiNaprijedSigurno(100);  // pokušaj ići 100 cm naprijed, ali stani ako ima prepreka
  delay(1000);

  Serial.println("↪️ Okret udesno");
  okreniDesno(cmUKorake(10));
  delay(1000);
}

#include <AccelStepper.h>

// Motor pinovi
#define dirPinLeft 25
#define stepPinLeft 26
#define dirPinRight 27
#define stepPinRight 14
#define motorInterfaceType 1

AccelStepper motorLeft(motorInterfaceType, stepPinLeft, dirPinLeft);
AccelStepper motorRight(motorInterfaceType, stepPinRight, dirPinRight);

// Ultrazvučni pinovi
const int trigPin = 32;
const int echoPin = 13;

// Parametri motora i kotača
const int koraciPoOkretaju = 200;
const int mikrostep = 4;          // prilagodi mikrostep postavku
const float promjerKotaca = 6.4;  
const float opsegKotaca = 3.1416 * promjerKotaca;
const float koraciPoCm = (koraciPoOkretaju * mikrostep) / opsegKotaca;

// Brzina i prag za zaustavljanje
const int brzinaKoraka = 1500;
const float pragUdaljenostiCm = 15.0;  // robot staje ako je prepreka bliže od 15 cm

long duration;
float distanceCm;

void setup() {
  Serial.begin(115200);

  motorLeft.setMaxSpeed(brzinaKoraka);
  motorLeft.setAcceleration(1500);

  motorRight.setMaxSpeed(brzinaKoraka);
  motorRight.setAcceleration(1500);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

float izmjeriUdaljenost() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * 0.034 / 2;
  return distanceCm;
}

void loop() {
  float udaljenost = izmjeriUdaljenost();
  Serial.print("Udaljenost: ");
  Serial.print(udaljenost);
  Serial.println(" cm");

  if (udaljenost > pragUdaljenostiCm) {
    motorLeft.setSpeed(brzinaKoraka);
    motorRight.setSpeed(-brzinaKoraka);
  } else {
    motorLeft.setSpeed(0);
    motorRight.setSpeed(0);
  }
  
  motorLeft.runSpeed();
  motorRight.runSpeed();
}

#include <AccelStepper.h>
#include <math.h>

// Motor pinovi i postavke
#define dirPinLeft 25
#define stepPinLeft 26
#define dirPinRight 27
#define stepPinRight 14
#define motorInterfaceType 1

AccelStepper motorLeft(motorInterfaceType, stepPinLeft, dirPinLeft);
AccelStepper motorRight(motorInterfaceType, stepPinRight, dirPinRight);

// Parametri robota
const float promjerKotaca = 6.4; // cm
const float opsegKotaca = 3.1416 * promjerKotaca;
const int koraciPoOkretaju = 200;
const int mikrostep = 16;
const float koraciPoCm = (koraciPoOkretaju * mikrostep) / opsegKotaca;
const float razmakIzmeduKotaca = 12.0; // cm

// Trenutna pozicija robota
float pozX = 0.0, pozY = 0.0, orijentacija = 0.0;

void setup() {
  motorLeft.setMaxSpeed(1000);
  motorRight.setMaxSpeed(1000);
}

void loop() {
  idiDo(0, 20);   // Idi naprijed 20 cm
  delay(1000);

  idiDo(20, 20);  // Skreni desno i idi 20 cm
  delay(1000);

  idiDo(20, 0);   // Skreni opet i idi dolje
  delay(5000);
}

void idiNaprijed(float cm) {
  long koraci = cm * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(300);     // naprijed
  motorRight.setSpeed(-300);   // naprijed (zrcalno)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void skreniDesno(float stupnjeva) {
  float putanja = (3.1416 * razmakIzmeduKotaca) * (stupnjeva / 360.0);
  long koraci = putanja * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(300);    // naprijed
  motorRight.setSpeed(300);   // unatrag (zrcalno montiran)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void skreniLijevo(float stupnjeva) {
  float putanja = (3.1416 * razmakIzmeduKotaca) * (stupnjeva / 360.0);
  long koraci = putanja * koraciPoCm;

  motorLeft.setCurrentPosition(0);
  motorRight.setCurrentPosition(0);

  motorLeft.setSpeed(-300);   // unatrag
  motorRight.setSpeed(-300);  // naprijed (zrcalno montiran)

  while (abs(motorLeft.currentPosition()) < koraci || abs(motorRight.currentPosition()) < koraci) {
    if (abs(motorLeft.currentPosition()) < koraci) motorLeft.runSpeed();
    if (abs(motorRight.currentPosition()) < koraci) motorRight.runSpeed();
  }
}

void idiDo(float x, float y) {
  float dx = x - pozX;
  float dy = y - pozY;
  float d = sqrt(dx * dx + dy * dy);
  float kut = atan2(dx, dy) * 180.0 / 3.1416;

  float deltaKut = kut - orijentacija;
  while (deltaKut > 180) deltaKut -= 360;
  while (deltaKut < -180) deltaKut += 360;

  if (deltaKut > 0) {
    skreniDesno(deltaKut);
    orijentacija += deltaKut;
  } else {
    skreniLijevo(-deltaKut);
    orijentacija += deltaKut;
  }

  while (orijentacija < 0) orijentacija += 360;
  while (orijentacija >= 360) orijentacija -= 360;

  idiNaprijed(d);

  pozX = x;
  pozY = y;
}