hi ,
I want to adjust the two DC motors speed dynamically to correct for the differences , I use the difference between left ticks and right ticks to tell in what way the bot is moving ,so to go straight I want there to be no difference between left and right tick counts. the code I wrote increases the left motor and decreases the right motor if the error is negative, and vice verse if positive.
I just multiply the error by some factor to give us a better adjustment value. This factor P represents the P (for Proportional) in PID .
But when run the code , at first the difference is small enough but suddenly increases dramatically , I guess there is may be wrong in data types matching or wrong in choice the right value for adjustment , need some help/advice to how choose it correctly ?
#include "Arduino.h"
#include <digitalWriteFast.h>
int ENA=8; // SpeedPinA
int ENB=9; // SpeedPinB
int IN1=48; // RightMotorWire1 connected to Arduino's port 48
int IN2=49; // RightMotorWire2 connected to Arduino's port 49
int IN3=50; // LeftMotorWire1 connected to Arduino's port 50
int IN4=51; // LeftMotorWire2 connected to Arduino's port 51
// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 0
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 4
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks = 0;
// Right encoder
#define c_RightEncoderInterrupt 1
#define c_RightEncoderPinA 3
#define c_RightEncoderPinB 5
volatile bool _RightEncoderBSet;
volatile long _RightEncoderTicks = 0;
void setup()
{
Serial.begin(115200);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
digitalWrite(ENA,HIGH); //enable motorA
digitalWrite(ENB,HIGH); //enable motorB
SetupEncoders();
}
int WL=50;
int WR=50;
void SetupEncoders()
{
// Quadrature encoders
// Left encoder
pinMode(c_LeftEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_LeftEncoderPinA, LOW); // turn on pullup resistors
pinMode(c_LeftEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_LeftEncoderPinB, LOW); // turn on pullup resistors
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
// Right encoder
pinMode(c_RightEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_RightEncoderPinA, LOW); // turn on pullup resistors
pinMode(c_RightEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_RightEncoderPinB, LOW); // turn on pullup resistors
attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
}
void loop()
{
// PID controller to adjust the motor speeds dynamically to correct for the differences
float P= 0.2;
long difference =long (_LeftEncoderTicks - _RightEncoderTicks);
if (difference > 0)
{WL -= int(P * difference);
WR += int(P * difference);}
else
{WL += int(P * difference);
WR -= int(P * difference);}
int rightPWM, leftPWM;
// Right Motor
if (WR > 0) {
//forward
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
} else if (WR < 0){
//reverse
digitalWrite(IN1,HIGH);
digitalWrite(IN2,LOW);
}
if (WR == 0) {
rightPWM = 0;
analogWrite(ENA, rightPWM);
} else {
rightPWM = map(abs(WR), 1, 100, 1, 255);
analogWrite(ENA, rightPWM);
}
// Left Motor
if (WL > 0) {
//forward
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
} else if (WL < 0) {
//reverse
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);}
if (WL == 0) {
leftPWM = 0;
analogWrite(ENB, leftPWM);
} else {
leftPWM = map(abs(WL), 1, 100, 1, 255);
analogWrite(ENB, leftPWM);
}
Serial.print(_LeftEncoderTicks);
Serial.print("\t");
Serial.print(_RightEncoderTicks);
Serial.print("\t");
Serial.print(_LeftEncoderTicks - _RightEncoderTicks);
Serial.println("\t");
}
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
_LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
#else
_LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
#endif
}
// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_RightEncoderBSet = digitalReadFast(c_RightEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef RightEncoderIsReversed
_RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
#else
_RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
#endif
}
I 'll try that