Robot_Arduino_explore_and_sensor

// LAZARIDIS DIMITRIS

#include <Servo.h> #include <SoftwareSerial.h>

// Servo motor objects Servo leftServo; Servo rightServo;

// Ultrasonic sensor pins #define TRIG_PIN 12 #define ECHO_PIN 11

SoftwareSerial nodemcuSerial(2, 3); // RX, TX (connect to NodeMCU TX, RX)

//volatile int finish;

// Constants const int forwardSpeed = 100; // Speed for forward motion (0-180) const int stopSpeed = 90; // Stop (neutral) position const int safeDistance = 20; // Minimum distance from obstacles (cm) const float distancePerSecond = 14.0; // Speed of the robot in cm/s //10 //const int turnSpeed = 90; // Speed for turning

// Movement history struct Movement { float distance; // Distance in cm float angle; // Turn angle in degrees }; Movement movementHistory100; //200 int movementIndex = 0;

// Function prototypes void moveForward(float distance); void turn(float angle); void returnToStart(); long readDistance(); void avoidObstacle(); //long avoidObstacle();

void setup() { // Attach servo motors leftServo.attach(9); rightServo.attach(10);

// Attach ultrasonic sensor pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT);

// Initialize servos to stopped position leftServo.write(stopSpeed); rightServo.write(stopSpeed);

// Start serial communication for debugging Serial.begin(9600); nodemcuSerial.begin(9600); //SoftwareSerial nodemcuSerial(2, 3); // RX, TX (connect to NodeMCU TX, RX) (2, 3); }

void loop() {

// Explore the area for (int i = 0; i < 4; i++) { // Explore in 4 steps long distance = readDistance(); //distance = readDistance(); delay(1); if (distance < safeDistance && distance > 0) { avoidObstacle(); //long complete = avoidObstacle(); //if (complete == 1) { //finish = 0; i = i-1; continue; } else { moveForward(30); // Move forward 40 - 30 cm } turn(90); // Turn 90 degrees for exploration // here can increase the radious }

// Return to starting point returnToStart();

while (1) //;// Stop execution or remove to go on {readDistance(); // moveForward(100); delay(500);

} }

// Function to move forward by a specific distance / void moveForward(float distance) { int duration = (abs(distance) / distancePerSecond) 1000; // Time in milliseconds

if (distance > 0) { leftServo.write(180 -forwardSpeed); // 180 ,200 is the slowest speed (80); //(180 - forwardSpeed); rightServo.write(200 -forwardSpeed); // (100); //(295 -forwardSpeed); } else { leftServo.write(200 - forwardSpeed); rightServo.write(180 - forwardSpeed); }

delay(duration); long distance_fresh = readDistance();

if (distance_fresh < safeDistance && distance_fresh > 0) { avoidObstacle();

}

leftServo.write(stopSpeed); rightServo.write(stopSpeed);

// Log movement movementHistorymovementIndex++ = {distance, 0};

// Debugging Serial.print("Moved forward: "); //Serial.print(distance); //Serial.println(" cm"); } */

void moveForward(float distance) {

int duration = (abs(distance) / distancePerSecond) * 1000; int step = 20; // μικρό delay int elapsed = 0; int test = duration / 2;

// Κατεύθυνση if (distance > 0) { leftServo.write(180 - forwardSpeed); rightServo.write(200 - forwardSpeed); } else { leftServo.write(200 - forwardSpeed); rightServo.write(180 - forwardSpeed); }

delay(test);
 long distance_fresh = readDistance();

 if (distance_fresh < safeDistance && distance_fresh > 0) {
     avoidObstacle();

  }
  if (distance > 0) {
leftServo.write(180 - forwardSpeed);
rightServo.write(200 - forwardSpeed);

} else { leftServo.write(200 - forwardSpeed); rightServo.write(180 - forwardSpeed); } delay(test); Serial.print("2second delay");

// Σταμάτημα leftServo.write(stopSpeed); rightServo.write(stopSpeed); //readDistance();
movementHistorymovementIndex++ = {distance, 0}; Serial.print("Moved forward: "); }

// Function to turn by a specific angle void turn(float angle) { int duration = abs(angle) 17; //12; //10; // Approximation: Adjust timing for your robot // int duration = abs(angle) 100; // Approximation: Adjust timing for your robot //int duration = (abs(angle) / distancePerSecond -1) * 1000; // 1000 Time in milliseconds + 0.5

if (angle > 0) {

//leftServo.write(90);   //180 - forwardSpeed);  //100 forwardSpeed  90 is stall speed  85 is slow 0 is fast
//rightServo.write(100);           // 100 is slow 180 is fast   right in front side  reverst placed
leftServo.write(240 - forwardSpeed);   
rightServo.write(240 - forwardSpeed);     

} else { leftServo.write(140 - forwardSpeed); //180 same degrees because the motors are revers in placed left motor normal degrees rightServo.write(140 - forwardSpeed);

} / if (angle == 5 ) {delay(480);} else { delay(duration);} / delay(duration);

// Stop the servos leftServo.write(stopSpeed); rightServo.write(stopSpeed);

// Log turn movementHistorymovementIndex++ = {0, angle};

// Debugging //Serial.print("Turned: "); //Serial.print(angle); //Serial.print(" degrees"); //Serial.println(duration); //while(1) {}; }

// Function to return to the starting point void returnToStart() { Serial.println("Returning to start...");

for (int i = movementIndex - 1; i >= 0; i--) { if (movementHistoryi.distance > 0) { moveForward(-movementHistoryi.distance); // Reverse forward movement //erial.println("Back_test"); //Serial.println(-movementHistoryi.distance); } else if (movementHistoryi.angle != 0) { turn(-movementHistoryi.angle); // Reverse turn //Serial.println(-movementHistoryi.angle); } else Serial.println("error"); }

Serial.println("Returned to start."); }

// Function to read distance from ultrasonic sensor long readDistance() { long duration; int distance;

// Trigger HC-SR04 digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2); digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIG_PIN, LOW);

// Read echo pulse duration = pulseIn(ECHO_PIN, HIGH); distance = duration * 0.034 / 2; // Convert to cm

// Send data to NodeMCU nodemcuSerial.println(distance);

// Debugging //Serial.print("F_Distance: "); //Serial.println(distance); //Serial.println(" cm");

return (distance);

}

// Function to avoid obstacles void avoidObstacle() { //long avoidObstacle() {

Serial.println("Obstacle detected! Avoiding...");

//delay(10000); // moveForward(-5); // Move forward 10 cm //delay(500); //movementHistorymovementIndex++ = {5, 0}; //-10

turn(90); // Turn right //delay(1); //movementHistorymovementIndex++ = {0, 5};

//moveForward(20); // Move forward 20 cm //movementHistorymovementIndex++ = {20, 0};

//turn(-90); // Turn back to original direction //movementHistorymovementIndex++ = {0, -90};

Serial.println("Obstacle avoided."); // finish = 1; //return (finish); }