Autonomous RC car help

I have a standard Arduino Uno, with 5 HC-SR04 ultrasonic sensors and a GY-521 gyro/accelerometer. I am trying to run a stripped down RC car, running the motors with a Cytron Motor shield, autonomously, attempting to detect it's surroundings and plot optimal racing lines through them as it goes. This is my code so far, I am just looking for improvements/ideas.

My code is:

#include <Wire.h>
#include <MPU6050.h>
#include "CytronMotorDriver.h"

// Configure the motor driver.
CytronMD drivemotor(PWM_DIR, 6, 7);  // PWM = Pin 6, DIR = Pin 7.
CytronMD turnmotor(PWM_DIR, 9, 8);

MPU6050 mpu;

// Define Node structure for pathfinding
struct Node {
  int x;
  int y;
  Node* parent;
};

// Define movement speeds
const int FORWARD_SPEED = 100;
const int TURN_SPEED = 200; // Adjust as needed

// Define sensor pins for front, front-right, front-left, side-right, and side-left sensors
const int trigPinFront = A0;
const int echoPinFront = A1;
const int trigPinFrontRight = A2;
const int echoPinFrontRight = A3;
const int trigPinFrontLeft = A4;
const int echoPinFrontLeft = A5;
const int trigPinSideRight = A6;
const int echoPinSideRight = A7;
const int trigPinSideLeft = 8;
const int echoPinSideLeft = 9;

// Define safe distance from obstacles
const int SAFE_DISTANCE = 20; // Adjust as needed

// Variables for gyro-based position estimation
float angleX = 0, angleY = 0;
float dt;
float positionX = 0, positionY = 0;

// Function to measure distance using ultrasonic sensor
float measureDistance(int trigPin, int echoPin) {
  // Measure distance using ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) / 58.0;  // Convert pulse duration to distance (cm)
}

// Function to move the robot forward
void moveForward(int speed) {
  drivemotor.setSpeed(-speed);
}

// Function to move the robot backward
void moveBackward(int speed) {
  drivemotor.setSpeed(speed);
}

// Function to turn the robot right
void turnRight(int speed) {
  turnmotor.setSpeed(-speed);
}

// Function to turn the robot left
void turnLeft(int speed) {
  turnmotor.setSpeed(speed);
}

// Function to stop the robot
void stop() {
  drivemotor.setSpeed(0);
  turnmotor.setSpeed(0);
}

// Function to estimate position using gyro
void estimatePosition() {
  // Read gyro data
  int16_t gyroX, gyroY, gyroZ;
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);
  
  // Convert gyro readings to degrees per second
  float gyroX_degPerSec = gyroX / 131.0; // 131 LSB/(°/s) for MPU6050
  float gyroY_degPerSec = gyroY / 131.0;

  // Update angleX and angleY based on gyro data
  angleX += gyroX_degPerSec * dt;
  angleY += gyroY_degPerSec * dt;

  // Estimate position using gyro data
  positionX += cos(angleY * DEG_TO_RAD) * cos(angleX * DEG_TO_RAD) * dt;
  positionY += cos(angleY * DEG_TO_RAD) * sin(angleX * DEG_TO_RAD) * dt;
}

// Function to estimate current X position using gyro
float estimatePositionX() {
  return positionX; // Return the current estimated X position
}

// Function to estimate current Y position using gyro
float estimatePositionY() {
  return positionY; // Return the current estimated Y position
}


// Function to check for obstacles on the sides
bool detectObstacleSide(int trigPin, int echoPin) {
  float distance = measureDistance(trigPin, echoPin);
  return distance < SAFE_DISTANCE;
}

// Function to move the robot along the path while avoiding obstacles
void moveAlongPath(Node* startNode, Node* goalNode) {
  Node* current = startNode;
  while (current != goalNode) {
    // Calculate direction to move
    int dx = goalNode->x - current->x;
    int dy = goalNode->y - current->y;

    // Adjust movement based on direction
    if (dx == 0 && dy == 0) {
      // Reached the goal
      break;
    } else if (dx == 0) {
      // Move vertically
      if (dy > 0) {
        if (measureDistance(trigPinFront, echoPinFront) < SAFE_DISTANCE ||
            detectObstacleSide(trigPinSideLeft, echoPinSideLeft)) {
          moveBackward(FORWARD_SPEED); // If obstacle in front or on the side, move backward
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      } else {
        if (measureDistance(trigPinFront, echoPinFront) < SAFE_DISTANCE ||
            detectObstacleSide(trigPinSideRight, echoPinSideRight)) {
          moveBackward(FORWARD_SPEED); // If obstacle in front or on the side, move backward
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      }
    } else if (dy == 0) {
      // Move horizontally
      if (dx > 0) {
        if (measureDistance(trigPinFrontRight, echoPinFrontRight) < SAFE_DISTANCE) {
          turnRight(TURN_SPEED); // If obstacle on the right, turn right
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      } else {
        if (measureDistance(trigPinFrontLeft, echoPinFrontLeft) < SAFE_DISTANCE) {
          turnLeft(TURN_SPEED); // If obstacle on the left, turn left
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      }
    } else {
      // Move diagonally
      if (dx > 0 && dy > 0) {
        if (measureDistance(trigPinFrontRight, echoPinFrontRight) < SAFE_DISTANCE) {
          turnRight(TURN_SPEED); // If obstacle on the right, turn right
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      } else if (dx > 0 && dy < 0) {
        if (measureDistance(trigPinFrontLeft, echoPinFrontLeft) < SAFE_DISTANCE) {
          turnLeft(TURN_SPEED); // If obstacle on the left, turn left
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      } else if (dx < 0 && dy > 0) {
        if (measureDistance(trigPinFrontRight, echoPinFrontRight) < SAFE_DISTANCE) {
          turnRight(TURN_SPEED); // If obstacle on the right, turn right
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      } else {
        if (measureDistance(trigPinFrontLeft, echoPinFrontLeft) < SAFE_DISTANCE) {
          turnLeft(TURN_SPEED); // If obstacle on the left, turn left
          delay(500); // Adjust as needed
        } else {
          moveForward(FORWARD_SPEED); // Move forward if no obstacle
        }
      }
    }
    delay(100);  // Adjust as needed
    estimatePosition(); // Update position estimate
    current = current->parent;  // Move to the next node
  }
}
// Function to calculate the endpoint based on obstacles
Node* calculateEndpoint() {
  // Measure distance in front
  float distanceFront = measureDistance(trigPinFront, echoPinFront);

  // Measure distance on the left side
  float distanceLeft = measureDistance(trigPinFrontLeft, echoPinFrontLeft);

  // Measure distance on the right side
  float distanceRight = measureDistance(trigPinFrontRight, echoPinFrontRight);

  // Define variables to store endpoint coordinates
  int endX, endY;

  // Check if there's space in front
  if (distanceFront >= SAFE_DISTANCE) {
    // If space in front, place the endpoint about 2 feet in front of the robot
    endX = estimatePositionX() + 60; // Adjust as needed (2 feet = 60 cm)
    endY = estimatePositionY();      // Keep Y-coordinate the same
  } else {
    // If space not available in front, check left and right
    if (distanceLeft >= SAFE_DISTANCE && distanceRight >= SAFE_DISTANCE) {
      // If both left and right have space, choose the furthest direction
      if (distanceLeft >= distanceRight) {
        // If left is further, place endpoint to the left
        endX = estimatePositionX();
        endY = estimatePositionY() - 60; // Adjust as needed (2 feet = 60 cm)
      } else {
        // If right is further, place endpoint to the right
        endX = estimatePositionX();
        endY = estimatePositionY() + 60; // Adjust as needed (2 feet = 60 cm)
      }
    } else if (distanceLeft >= SAFE_DISTANCE) {
      // If only left has space, place endpoint to the left
      endX = estimatePositionX();
      endY = estimatePositionY() - 60; // Adjust as needed (2 feet = 60 cm)
    } else if (distanceRight >= SAFE_DISTANCE) {
      // If only right has space, place endpoint to the right
      endX = estimatePositionX();
      endY = estimatePositionY() + 60; // Adjust as needed (2 feet = 60 cm)
    } else {
      // If no space available in front, left, or right, choose the furthest direction
      if (distanceLeft >= distanceRight) {
        // If left is further, place endpoint to the left
        endX = estimatePositionX();
        endY = estimatePositionY() - 60; // Adjust as needed (2 feet = 60 cm)
      } else {
        // If right is further, place endpoint to the right
        endX = estimatePositionX();
        endY = estimatePositionY() + 60; // Adjust as needed (2 feet = 60 cm)
      }
    }
  }

  // Create the endpoint node
  Node* endpoint = new Node;
  endpoint->x = endX;
  endpoint->y = endY;
  endpoint->parent = nullptr; // No parent as it's the endpoint

  return endpoint;
}



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

}


void loop() {
  // Perform pathfinding algorithm to calculate the optimal path
    // Define the start node based on current estimated position
  Node* startNode = new Node;
  startNode->x = estimatePositionX();
  startNode->y = estimatePositionY();
  startNode->parent = nullptr; // No parent as it's the start node

  Node* goalNode = calculateEndpoint();
  // Find the shortest path from startNode to goalNode
  moveAlongPath(startNode, goalNode);

}

You missed showing your annotated schematic.

yeah sorry, i tried my best to include everything that people seemed pissed about missing, this has just been a spur of the moment project, and i dont have a schematic

These seem useless:

yeah i just wanted to have functions for it

Sorry to say I am not upset, I just do not have a "I am trying to run a stripped down RC car, running the motors with a Cytron Motor shield, autonomously, attempting to detect it's surroundings and plot optimal racing lines through them as it goes." or access to one. Without knowing what it is i cannot answer your question.

So it is a cytron motor shield running two DC motors, one that controls the front wheels turning, and one that controls the back wheels speed. there are three ultrasonic sensors arrayed on the front, one forward and two diagonal. there are two ultrasonic sensors, one on either side. the ultrasonic sensor pinouts are in the code, and the gyro is on A4 and A5. the motors are connected to 6, 7, 8, 9.

"Seem" - present tense... and do I really look drubnk?

The drawings are procedural, just like you posted your description and your program. It helps you (the poster) because it helps the helpers. Spread the word!

yeah i just have no clue how to create the drawings

no ik that loll but like i have no clue how to do technical drawings was my point

Interesting, we are at a standstill, you cannot describe it so I can understand what you have. Therefore I can not give you a good answer. Since you do not know how present it so I can understand is what you are saying. I cannot help you, Good Luck! If you take the time to learn how to do a schematic, the language of electronics you will be way ahead.

1 Like
  • Draw a box. Label the box. Label pin numbers on the box.
  • Draw a second box. Label the second box. Label pin numbers on the seconds box.
  • Connect pins from the first box to the second box.
  • Repeat for more boxes.
  • Your first draft will be a complete mess... re-arrange the boxes to make it readable.
  • Post your final draft.
  • A.M.A.

oh, i thought it was referencing more like circuit diagrams

Only if you want these guys to be your best friend. A clear wiring diagram will suffice.

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.