I am following a tutorial(that has proven very helpful thus far) but I have modified it to control two motors on my Arduino mega instead of one on an arduino uno, but I am using the same motor driver. I have gotten it to drive the motors and measure the position correctly, however when I try to implement the control algorithm it breaks and starts the motors running infinitely in the negative direction. If anybody can figure out why that is, I would be grateful. Code below:
#include <Arduino.h>
// Encoder pins:
#define ENC1A 3
#define ENC1B 2
// encoder 1 ^ encoder 2:
#define ENC2A 20
#define ENC2B 21
// motor driver pins
#define PWMA 5
#define INA1 7
#define INA2 6
#define INB1 8
#define INB2 9
#define PWMB 10
volatile int apos = 0;
volatile int bpos = 0;
long prevT = 0;
float aeprev = 0;
float beprev = 0;
float aeintegral = 0;
float beintegral = 0;
void readEncoderA()
{
int a = digitalRead(ENC1B);
if(a>0){
apos++;
}
else {
apos--;
}
}
void readEncoderB()
{
int b = digitalRead(ENC2B);
if(b>0){
bpos++;
}
else {
bpos--;
}
}
void setMotor(int dir, int pwmVal, int pwm, int in1, int in2)
{
analogWrite(pwm, pwmVal);
if(dir==1) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
else if(dir == -1){
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
}
}
void setup()
{
Serial.begin(9600);
pinMode(ENC2A, INPUT);
pinMode(ENC2B, INPUT);
pinMode(ENC1A, INPUT);
pinMode(ENC1B, INPUT);
attachInterrupt(digitalPinToInterrupt(ENC1A), readEncoderA, RISING);
attachInterrupt(digitalPinToInterrupt(ENC2A), readEncoderB, RISING);
}
void loop()
{
// set target position
int target = 1200;
// pid constants
float kp = 1;
float kd = 0;
float ki = 0;
// time difference
long currT = micros();
float deltaT = ((float)(currT -prevT))/1.0e6;
prevT = currT;
// calculate error
int ae = target-apos;
int be = target-bpos;
// derivative
float adedt = (ae-aeprev)/(deltaT);
float bdedt = (be-beprev)/(deltaT);
// integral
float aeintegral = aeintegral + ae*deltaT;
float beintegral = beintegral + be*deltaT;
// control signal
float au = kp*ae + kd*adedt + ki*aeintegral;
float bu = kp*be + kd*bdedt + ki*beintegral;
// motor power
float apwr = fabs(au);
if(apwr > 255){
apwr = 255;
}
float bpwr = fabs(bu);
if(bpwr > 255){
bpwr = 255;
}
// motor direction
int adir = 1;
if(au<0){
adir =-1;
}
int bdir = 1;
if(bu<0){
bdir = -1;
}
// signal the motor
setMotor(adir, apwr, PWMA, INA1, INA2);
setMotor(bdir, bpwr, PWMB, INB1, INB2);
// store previous errors
aeprev = ae;
beprev = be;
Serial.print(target);
Serial.print(" ");
Serial.print(apos);
Serial.print(" ");
Serial.print(bpos);
Serial.println();
}
Update:
I have solved the issue of it moving in the negative direction forever by changing the order of calculated error, instead of apos-target it is target-apos. However I am now having an issue where whichever function I drive second, comes up about 100 units short of the target and won't move independently once the first motor comes up to the goal. If anybody knows why they wouldn't move independently of one another, as I can't see a reason in the code that one couldn't move while the other stays in place the help would be appreciated. I have updated the code sample to reflect this change.