#include <Encoder.h>
// Define motor pins
#define MOTOR1_IN1 2
#define MOTOR1_IN2 3
#define MOTOR2_IN1 4
#define MOTOR2_IN2 5
#define MOTOR3_IN1 6
#define MOTOR3_IN2 7
#define MOTOR4_IN1 8
#define MOTOR4_IN2 9
// Define ultrasonic sensor pins
#define TRIG_PIN1 10
#define ECHO_PIN1 11
#define TRIG_PIN2 12
#define ECHO_PIN2 13
#define TRIG_PIN3 A0
#define ECHO_PIN3 A1
// Define encoders
Encoder motor1Encoder(14, 15); // Motor 1 encoder pins
Encoder motor2Encoder(16, 17); // Motor 2 encoder pins
Encoder motor3Encoder(18, 19); // Motor 3 encoder pins
Encoder motor4Encoder(20, 21); // Motor 4 encoder pins
// Ultrasonic sensor distance variables
long distance1, distance2, distance3;
// Encoder counts and surface detection variables
long prevEncoder1 = 0;
long prevEncoder2 = 0;
long prevEncoder3 = 0;
long prevEncoder4 = 0;
bool surfaceChanged = false;
// PID control variables
float kp = 0.6;
float ki = 0.3;
float kd = 0.05;
float lastError1 = 0, lastError2 = 0, lastError3 = 0, lastError4 = 0;
float iTerm1 = 0, iTerm2 = 0, iTerm3 = 0, iTerm4 = 0;
// Thresholds
int safeDistance = 20; // Ultrasonic sensor threshold (in cm)
int surfaceThreshold = 100; // Encoder threshold for detecting surface change
// Constants for voltage measurement
#define VOLTAGE_PIN A2 // Analog pin for reading battery voltage
float voltageMultiplier = 11.0; // Based on the voltage divider resistor ratio
float nominalVoltage = 12.0; // Nominal operating voltage of the robot's battery
float lowVoltageThreshold = 9.0; // Voltage below which power drops significantly
void setup() {
// Initialize motor pins
pinMode(MOTOR1_IN1, OUTPUT);
pinMode(MOTOR1_IN2, OUTPUT);
pinMode(MOTOR2_IN1, OUTPUT);
pinMode(MOTOR2_IN2, OUTPUT);
pinMode(MOTOR3_IN1, OUTPUT);
pinMode(MOTOR3_IN2, OUTPUT);
pinMode(MOTOR4_IN1, OUTPUT);
pinMode(MOTOR4_IN2, OUTPUT);
// Initialize ultrasonic sensor pins
pinMode(TRIG_PIN1, OUTPUT);
pinMode(ECHO_PIN1, INPUT);
pinMode(TRIG_PIN2, OUTPUT);
pinMode(ECHO_PIN2, INPUT);
pinMode(TRIG_PIN3, OUTPUT);
pinMode(ECHO_PIN3, INPUT);
Serial.begin(9600); // Initialize serial communication
// Run self-test on power-up
performSelfTest();
}
void loop() {
// Get distance readings from the ultrasonic sensors
distance1 = getUltrasonicDistance(TRIG_PIN1, ECHO_PIN1);
distance2 = getUltrasonicDistance(TRIG_PIN2, ECHO_PIN2);
distance3 = getUltrasonicDistance(TRIG_PIN3, ECHO_PIN3);
// Voltage compensation
float batteryVoltage = readBatteryVoltage();
adjustForVoltageDrop(batteryVoltage);
// Check for obstacles and surface change
if (distance1 > safeDistance && distance2 > safeDistance) {
// Use PID for smooth motor control
controlMotorsPID(200, 200, 200, 200);
} else if (distance3 < safeDistance) {
makeLeftTurn(); // Turn left if left-side sensor detects an obstacle
} else {
stopMotors(); // Stop if obstacles are detected in front
}
// Detect surface change using encoder feedback
detectSurfaceChange();
}
// Function to control motors using PID
void controlMotorsPID(int setpoint1, int setpoint2, int setpoint3, int setpoint4) {
long encoder1 = motor1Encoder.read();
long encoder2 = motor2Encoder.read();
long encoder3 = motor3Encoder.read();
long encoder4 = motor4Encoder.read();
// PID calculations for each motor
float error1 = setpoint1 - encoder1;
float error2 = setpoint2 - encoder2;
float error3 = setpoint3 - encoder3;
float error4 = setpoint4 - encoder4;
iTerm1 += ki * error1;
iTerm2 += ki * error2;
iTerm3 += ki * error3;
iTerm4 += ki * error4;
float dTerm1 = kd * (error1 - lastError1);
float dTerm2 = kd * (error2 - lastError2);
float dTerm3 = kd * (error3 - lastError3);
float dTerm4 = kd * (error4 - lastError4);
float output1 = kp * error1 + iTerm1 + dTerm1;
float output2 = kp * error2 + iTerm2 + dTerm2;
float output3 = kp * error3 + iTerm3 + dTerm3;
float output4 = kp * error4 + iTerm4 + dTerm4;
// Control motors based on PID output
setMotorSpeed(output1, MOTOR1_IN1, MOTOR1_IN2);
setMotorSpeed(output2, MOTOR2_IN1, MOTOR2_IN2);
setMotorSpeed(output3, MOTOR3_IN1, MOTOR3_IN2);
setMotorSpeed(output4, MOTOR4_IN1, MOTOR4_IN2);
lastError1 = error1;
lastError2 = error2;
lastError3 = error3;
lastError4 = error4;
}
// Function to set motor speed
void setMotorSpeed(float speed, int in1, int in2) {
if (speed > 0) {
analogWrite(in1, speed);
analogWrite(in2, 0);
} else {
analogWrite(in1, 0);
analogWrite(in2, -speed);
}
}
// Surface change detection function
void detectSurfaceChange() {
long encoder1 = motor1Encoder.read();
long encoder2 = motor2Encoder.read();
long encoder3 = motor3Encoder.read();
long encoder4 = motor4Encoder.read();
long change1 = abs(encoder1 - prevEncoder1);
long change2 = abs(encoder2 - prevEncoder2);
long change3 = abs(encoder3 - prevEncoder3);
long change4 = abs(encoder4 - prevEncoder4);
if (change1 > surfaceThreshold || change2 > surfaceThreshold || change3 > surfaceThreshold || change4 > surfaceThreshold) {
surfaceChanged = true;
Serial.println("Surface change detected!");
// Adjust motor behavior or speed for different surfaces here
}
prevEncoder1 = encoder1;
prevEncoder2 = encoder2;
prevEncoder3 = encoder3;
prevEncoder4 = encoder4;
}
// Advanced navigation function
void advancedNavigation() {
// Use ultrasonic sensors and encoder data to adjust speed or path dynamically
if (distance1 < safeDistance || distance2 < safeDistance) {
// If an obstacle is detected in front, make a decision
if (distance3 < safeDistance) {
makeLeftTurn(); // Turn left if there's space on the left
} else {
stopMotors();
// Implement a decision-making algorithm here for complex navigation
}
} else {
controlMotorsPID(200, 200, 200, 200); // Continue moving forward if no obstacles
}
}
// Self-test routine on power-up
void performSelfTest() {
Serial.println("Starting self-test...");
// Move forward
moveForward();
delay(500);
stopMotors();
// Turn left
makeLeftTurn();
delay(500);
stopMotors();
// Move reverse
moveBackward();
delay(500);
stopMotors();
// Turn right (opposite of left turn)
makeRightTurn();
delay(500);
stopMotors();
// Test ultrasonic sensors
distance1 = getUltrasonicDistance(TRIG_PIN1, ECHO_PIN1);
distance2 = getUltrasonicDistance(TRIG_PIN2, ECHO_PIN2);
distance3 = getUltrasonicDistance(TRIG_PIN3, ECHO_PIN3);
Serial.print("Ultrasonic Sensor 1: ");
Serial.println(distance1);
Serial.print("Ultrasonic Sensor 2: ");
Serial.println(distance2);
Serial.print("Ultrasonic Sensor 3: ");
Serial.println(distance3);
// Twitch forward
moveForward();
delay(200);
stopMotors();
// Twitch backward
moveBackward();
delay(200);
stopMotors();
Serial.println("Self-test complete.");
}
// Move functions
void moveForward() {
digitalWrite(MOTOR1_IN1, HIGH);
digitalWrite(MOTOR1_IN2, LOW);
digitalWrite(MOTOR2_IN1, HIGH);
digitalWrite(MOTOR2_IN2, LOW);
digitalWrite(MOTOR3_IN1, HIGH);
digitalWrite(MOTOR3_IN2, LOW);
digitalWrite(MOTOR4_IN1, HIGH);
digitalWrite(MOTOR4_IN2, LOW);
}
void moveBackward() {
digitalWrite(MOTOR1_IN1, LOW);
digitalWrite(MOTOR1_IN2, HIGH);
digitalWrite(MOTOR2_IN1, LOW);
digitalWrite(MOTOR2_IN2, HIGH);
digitalWrite(MOTOR3_IN1, LOW);
digitalWrite(MOTOR3_IN2, HIGH);
digitalWrite(MOTOR4_IN1, LOW);
digitalWrite(MOTOR4_IN2, HIGH);
}
void makeLeftTurn() {
digitalWrite(MOTOR1_IN1, LOW);
digitalWrite(MOTOR1_IN2, HIGH); // Left motors reverse
digitalWrite(MOTOR2_IN1, LOW);
digitalWrite(MOTOR2_IN2, HIGH);
digitalWrite(MOTOR3_IN1, HIGH); // Right motors forward
digitalWrite(MOTOR3_IN2, LOW);
digitalWrite(MOTOR4_IN1, HIGH);
digitalWrite(MOTOR4_IN2, LOW);
}
void makeRightTurn() {
digitalWrite(MOTOR1_IN1, HIGH);
digitalWrite(MOTOR1_IN2, LOW); // Left motors forward
digitalWrite(MOTOR2_IN1, HIGH);
digitalWrite(MOTOR2_IN2, LOW);
digitalWrite(MOTOR3_IN1, LOW); // Right motors reverse
digitalWrite(MOTOR3_IN2, HIGH);
digitalWrite(MOTOR4_IN1, LOW);
digitalWrite(MOTOR4_IN2, HIGH);
}
void stopMotors() {
digitalWrite(MOTOR1_IN1, LOW);
digitalWrite(MOTOR1_IN2, LOW);
digitalWrite(MOTOR2_IN1, LOW);
digitalWrite(MOTOR2_IN2, LOW);
digitalWrite(MOTOR3_IN1, LOW);
digitalWrite(MOTOR3_IN2, LOW);
digitalWrite(MOTOR4_IN1, LOW);
digitalWrite(MOTOR4_IN2, LOW);
}
// Function to read the battery voltage using a voltage divider
float readBatteryVoltage() {
int analogValue = analogRead(VOLTAGE_PIN); // Read analog value
float voltage = (analogValue / 1023.0) * 5.0 * voltageMultiplier; // Convert to actual voltage
Serial.print("Battery Voltage: ");
Serial.println(voltage);
return voltage;
}
// Function to adjust for voltage drop
void adjustForVoltageDrop(float batteryVoltage) {
if (batteryVoltage < nominalVoltage) {
float voltageRatio = batteryVoltage / nominalVoltage;
// Adjust motor power and PID values based on voltage ratio
kp = kp / voltageRatio;
ki = ki / voltageRatio;
kd = kd / voltageRatio;
Serial.print("Adjusted kp: ");
Serial.println(kp);
}
}
// Function to get distance from an ultrasonic sensor
long getUltrasonicDistance(int trigPin, int echoPin) {
digitalWrite(trigPin, LOW); // Clear the trigger
delayMicroseconds(2);
digitalWrite(trigPin, HIGH); // Set the trigger high
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
long duration = pulseIn(echoPin, HIGH); // Read the echo pin
long distance = duration * 0.034 / 2; // Calculate the distance in cm
return distance;
}