Hi guys,
I am trying to move a car straight using encoders, but the issue I'm having is that the car deviates towards right. The codes that I've used and modified is from Sparksfun in the link below:
My version of the codes are as follows.
#include <AFMotor.h>
// parameters
const int drive_distance = 100; // 100 cm
const int motor_power = 150; // 0-255
const int motor_offset = 5; // difference when driving straight
const int wheel_d = 60; // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel Circumference (mm)
const int counts_per_rev = 6; // (3 pairs N-S) * (48:1 gearbox) * (2 falling/rising edges) = 288,
// but in my case there are 6 counts per revolution as I've mounted the magnet
// onto the output shaft instead of the motor shaft.
// Globals
volatile unsigned long enc_l = 0;
volatile unsigned long enc_r = 0;
// Pins
const int enc_l_pin = 18; // Motor A
const int enc_r_pin = 19; // Motor B
// Motors
//AF_DCMotor motor1(1, MOTOR12_64KHZ); // create motor #1, 64KHz pwm (Left Back wheel)
AF_DCMotor motor2(2, MOTOR12_64KHZ); // create motor #2, 64KHz pwm (Left Front wheel)
//AF_DCMotor motor3(3, MOTOR12_64KHZ); // create motor #3, 64KHz pwm (Right Back wheel)
AF_DCMotor motor4(4, MOTOR12_64KHZ); // create motor #4, 64KHz pwm (Right Front wheel)
void setup() {
// Set up pins
pinMode(enc_l_pin, INPUT);
pinMode(enc_r_pin, INPUT);
// Set up interrupts
attachInterrupt(digitalPinToInterrupt(enc_l_pin), countLeft, CHANGE);
attachInterrupt(digitalPinToInterrupt(enc_r_pin), countRight, CHANGE);
}
void loop() {
bluetooth(); // Calling the driveStraight() function through bluetooth
}
void driveStraight(float drive_distance, int motor_power)
{
unsigned long num_ticks_l;
unsigned long num_ticks_r;
// Set initial motor power
int power_l = motor_power;
int power_r = motor_power;
// Used to determine which way to turn to adjust
unsigned long diff_l;
unsigned long diff_r;
// Reset encoder counts
enc_l = 0;
enc_r = 0;
// Remember previous encoder counts
unsigned long enc_1_prev = enc_l;
unsigned long enc_r_prev = enc_r;
// Calculate targer number of ticks
float num_rev = (drive_distance * 10) / wheel_c; // Convert to mm
unsigned long target_count = num_rev * counts_per_rev;
// Debug
Serial.print("Driving for ");
Serial.print(drive_distance);
Serial.print(" cm(");
Serial.print(target_count);
Serial.print(" ticks) at ");
Serial.print(motor_power);
Serial.println(" Motor Power");
//Drive until on of the encoders reaches desired count
while ((enc_l < target_count) && (enc_r < target_count))
{
num_ticks_l = enc_l;
num_ticks_r = enc_r;
Serial.println("enc_l = \t enc_r = ");
Serial.print(enc_1_prev);
Serial.print("\t");
Serial.println(enc_r_prev);
// Sample out current number of ticks
Serial.print(num_ticks_l);
Serial.print("\t");
Serial.println(num_ticks_r);
// Drive
drive(power_l, power_r);
// Number of ticsk counted since last time
diff_l = num_ticks_l - enc_1_prev;
diff_r = num_ticks_r - enc_r_prev;
// Store current ticks counter for next time
unsigned long enc_l_prev = num_ticks_l;
unsigned long enc_r_prev = num_ticks_r;
// If left is faster, slow it down and speed up right
if ( diff_l > diff_r) {
power_l -= motor_offset;
power_r += motor_offset;
}
// If right is faster, slow it down and speed up right
if ( diff_l < diff_r) {
power_l += motor_offset;
power_r -= motor_offset;
}
// Brif pause to let motors respond
delay(20);
}
StopCar();
}
void drive(int power_a, int power_b)
{
// Constrain power to between -255 and 255
power_a = constrain(power_a, -255, 255);
power_b = constrain(power_b, -255, 255);
// Left motor direction
if ( power_a < 0)
{
motor2.setSpeed(power_a); // Motor is the front left wheel
motor2.run(BACKWARD);
}
else
{
motor2.setSpeed(power_a); // Motor is the front left wheel
motor2.run(FORWARD);
}
// Right Motor direction
if ( power_b < 0)
{
motor4.setSpeed(power_b); // Motor is the front left wheel
motor4.run(BACKWARD);
}
else
{
motor4.setSpeed(power_b); // Motor is the front left wheel
motor4.run(FORWARD);
}
}
void StopCar()
{
//motor1.run(RELEASE);
motor2.run(RELEASE);
//motor3.run(RELEASE);
motor4.run(RELEASE);
}
And here is a picture of my robot:
