#include <SharpIR.h>
#include <Encoder.h>
// Define motor pins
#define M1R_DIR 5 // RIGHT motor direction
#define M1R_PWM 4 // RIGHT motor PWM
#define M2L_DIR 6 // LEFT motor direction
#define M2L_PWM 9 // LEFT motor PWM
// Define range sensor pins (IR sensors)
#define IR_FRONT A2 // Front IR sensor
#define IR_RIGHT A3 // Right IR sensor
#define IR_LEFT A4 // Left IR sensor
// Define encoder pins
#define ENC_M1R_A 18 // RIGHT encoder channel A
#define ENC_M1R_B 19 // RIGHT encoder channel B
#define ENC_M2L_A 3 // LEFT encoder channel A
#define ENC_M2L_B 2 // LEFT encoder channel B
// Define the SharpIR model
#define MODEL 1
// Setup SharpIR sensors
SharpIR leftSensor(MODEL, IR_LEFT);
SharpIR frontSensor(MODEL, IR_FRONT);
SharpIR rightSensor(MODEL, IR_RIGHT);
// Setup encoders
Encoder encL(ENC_M1R_A, ENC_M1R_B);
Encoder encR(ENC_M2L_A, ENC_M2L_B);
void setup() {
Serial.begin(9600);
// Define pin modes for sensors
pinMode(IR_FRONT, INPUT);
pinMode(IR_RIGHT, INPUT);
pinMode(IR_LEFT, INPUT);
// Define pin modes for motors
pinMode(M1R_DIR, OUTPUT);
pinMode(M1R_PWM, OUTPUT);
pinMode(M2L_DIR, OUTPUT);
pinMode(M2L_PWM, OUTPUT);
// Define pin modes for encoders
pinMode(ENC_M1R_A, INPUT);
pinMode(ENC_M1R_B, INPUT);
pinMode(ENC_M2L_A, INPUT);
pinMode(ENC_M2L_B, INPUT);
}
void loop() {
// Uncomment to test sensor values
/*
sensLval = sensL();
sensRval = sensR();
sensFval = sensF();
Serial.print("Left Value: ");
Serial.print(sensLval);
Serial.print(" mm");
Serial.print(", Front Value: ");
Serial.print(sensFval);
Serial.print(" mm");
Serial.print(", Right Value: ");
Serial.print(sensRval);
Serial.print(" mm");
Serial.println();
delay(dt);
*/
// Right motor control
analogWrite(M1R_PWM, 255);
digitalWrite(M1R_DIR, LOW);
Serial.print("Encoder Position Right: ");
Serial.print(encR.read());
// Left motor control
analogWrite(M2L_PWM, 255);
digitalWrite(M2L_DIR, LOW);
Serial.print(", Encoder Position Left: ");
Serial.print(encL.read());
Serial.println();
delay(1000);
}
int sensL() {
return leftSensor.getDistance() * 10; // Convert to millimeters if needed
}
int sensR() {
return rightSensor.getDistance() * 10; // Convert to millimeters if needed
}
int sensF() {
return frontSensor.getDistance() * 10; // Convert to millimeters if needed
}
Output:
Encoder Position Right: 828, Encoder Position Left: 1334
Encoder Position Right: 1673, Encoder Position Left: 2767
Encoder Position Right: 2535, Encoder Position Left: 4259
Encoder Position Right: 3456, Encoder Position Left: 5927
Encoder Position Right: 4420, Encoder Position Left: 7644
Encoder Position Right: 5472, Encoder Position Left: 9543
Encoder Position Right: 6604, Encoder Position Left: 11403
Encoder Position Right: 7769, Encoder Position Left: 13367
Encoder Position Right: 8928, Encoder Position Left: 15295
Basically my Left encoder gives me almost 2x more data than Right encoder there is no issue in mechanics but there is in code.
I couldn't figure it out where is the issue and how can I solve it.
Any help welcome
Thanks in advance.