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);
}