im having issues for driving it to 1 meter exact if anyone know can help here is my code
#include <Encoder.h>
// Motor pin definitions
#define PWMA 12 // Motor A speed control
#define DIRA1 34 // Motor A direction control 1
#define DIRA2 35 // Motor A direction control 2
#define PWMB 8 // Motor B speed control
#define DIRB1 37 // Motor B direction control 1
#define DIRB2 36 // Motor B direction control 2
#define PWMC 6 // Motor C speed control
#define DIRC1 43 // Motor C direction control 1
#define DIRC2 42 // Motor C direction control 2
#define PWMD 5 // Motor D speed control
#define DIRD1 A4 // Motor D direction control 1
#define DIRD2 A5 // Motor D direction control 2
// Encoder pin definitions
#define ENCODER_A1 18
#define ENCODER_A2 31
#define ENCODER_B1 19
#define ENCODER_B2 38
#define ENCODER_C1 3
#define ENCODER_C2 49
#define ENCODER_D1 2
#define ENCODER_D2 48
// Constants
const double wheelDiameter = 0.08; // meters
const double wheelCircumference = wheelDiameter * 3.14159;
const int pulsesPerRevolution = 330 * 4; // 1:30 reduction and quadrature encoder
const double distancePerPulse = wheelCircumference / pulsesPerRevolution;
const double targetDistance = 1.0; // meters
const int MAX_MOTOR_SPEED = 150; // Reduced max motor speed
const int MIN_MOTOR_SPEED = 50; // Min motor speed to ensure it moves
const double slowdownThreshold = 0.5; // Distance to start slowing down (meters)
// Encoder objects
Encoder encoderA(ENCODER_A1, ENCODER_A2);
Encoder encoderB(ENCODER_B1, ENCODER_B2);
Encoder encoderC(ENCODER_C1, ENCODER_C2);
Encoder encoderD(ENCODER_D1, ENCODER_D2);
// Time variables
unsigned long startTime;
unsigned long lastTime;
unsigned long currentTime;
// Function prototypes
void setupMotors();
void runMotors(int speed);
void stopMotors();
void emergencyStop();
void taskCompletionNotification();
void checkForEmergencyStop();
void resetEncoders();
void ISR_A();
void ISR_B();
void ISR_C();
void ISR_D();
// Variables to keep track of encoder counts using interrupts
volatile long countA = 0;
volatile long countB = 0;
volatile long countC = 0;
volatile long countD = 0;
void setup() {
Serial.begin(9600);
setupMotors();
// Attach interrupts for encoders
attachInterrupt(digitalPinToInterrupt(ENCODER_A1), ISR_A, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCODER_B1), ISR_B, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCODER_C1), ISR_C, CHANGE);
attachInterrupt(digitalPinToInterrupt(ENCODER_D1), ISR_D, CHANGE);
// Reset encoder counts
resetEncoders();
}
void loop() {
// Calculate the number of ticks required to travel the target distance
long targetTicks = targetDistance / distancePerPulse;
Serial.print("Target Ticks: ");
Serial.println(targetTicks);
// Record the start time
startTime = millis();
// Run motors until the required number of ticks is reached
while ((countA + countB + countC + countD) / 4 < targetTicks) {
double currentDistance = calculateDistance((countA + countB + countC + countD) / 4);
double remainingDistance = targetDistance - currentDistance;
int motorSpeed = MAX_MOTOR_SPEED;
if (remainingDistance <= slowdownThreshold) {
motorSpeed = (int)(MAX_MOTOR_SPEED * (remainingDistance / slowdownThreshold));
motorSpeed = constrain(motorSpeed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);
}
runMotors(motorSpeed);
checkForEmergencyStop();
// Debugging output
Serial.print("Current Ticks: ");
Serial.print((countA + countB + countC + countD) / 4);
Serial.print(" Distance: ");
Serial.println(currentDistance);
delay(20); // Reduce delay for higher control frequency
}
// Stop motors once the target distance is reached
stopMotors();
// Record the end time
unsigned long endTime = millis();
unsigned long elapsedTime = endTime - startTime;
Serial.print("Time taken: ");
Serial.print(elapsedTime);
Serial.println(" ms");
taskCompletionNotification();
while (true); // Halt execution
}
void setupMotors() {
pinMode(PWMA, OUTPUT);
pinMode(DIRA1, OUTPUT);
pinMode(DIRA2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(DIRB1, OUTPUT);
pinMode(DIRB2, OUTPUT);
pinMode(PWMC, OUTPUT);
pinMode(DIRC1, OUTPUT);
pinMode(DIRC2, OUTPUT);
pinMode(PWMD, OUTPUT);
pinMode(DIRD1, OUTPUT);
pinMode(DIRD2, OUTPUT);
}
void runMotors(int speed) {
speed = constrain(speed, MIN_MOTOR_SPEED, MAX_MOTOR_SPEED);
// Corrected direction for all motors
analogWrite(PWMA, speed);
digitalWrite(DIRA1, LOW);
digitalWrite(DIRA2, HIGH);
analogWrite(PWMB, speed);
digitalWrite(DIRB1, HIGH);
digitalWrite(DIRB2, LOW);
analogWrite(PWMC, speed);
digitalWrite(DIRC1, HIGH);
digitalWrite(DIRC2, LOW);
analogWrite(PWMD, speed);
digitalWrite(DIRD1, LOW);
digitalWrite(DIRD2, HIGH);
}
void stopMotors() {
analogWrite(PWMA, 0);
digitalWrite(DIRA1, LOW);
digitalWrite(DIRA2, LOW);
analogWrite(PWMB, 0);
digitalWrite(DIRB1, LOW);
digitalWrite(DIRB2, LOW);
analogWrite(PWMC, 0);
digitalWrite(DIRC1, LOW);
digitalWrite(DIRC2, LOW);
analogWrite(PWMD, 0);
digitalWrite(DIRD1, LOW);
digitalWrite(DIRD2, LOW);
}
double calculateDistance(long count) {
return count * distancePerPulse;
}
void emergencyStop() {
stopMotors();
Serial.println("Emergency Stop Triggered!");
while (true); // Halt execution
}
void taskCompletionNotification() {
Serial.println("Task Completed: Robot has driven exactly 1 meter.");
}
void checkForEmergencyStop() {
if (Serial.available()) {
char ch = Serial.read();
if (ch == 'e' || ch == 'E') {
emergencyStop();
}
}
}
void resetEncoders() {
countA = 0;
countB = 0;
countC = 0;
countD = 0;
}
void ISR_A() {
countA++;
}
void ISR_B() {
countB++;
}
void ISR_C() {
countC++;
}
void ISR_D() {
countD++;
}
