One motor with Encoder comes up about 100 units short of other

Projects Programming
1

Hi, I am having an issue with my encoder motor driving algorithm 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 am using an arduino mega

#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.025;
	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-apos;

	// 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;

	// Serial.println("au: " + String(au) + "    ");
	// Serial.println("bu: " + String(bu) + "    ");

	// 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;
	}


	setMotor(bdir, bpwr, PWMB, INB1, INB2);
	setMotor(adir, apwr, PWMA, INA1, INA2);

	// store previous errors
	aeprev = ae;
	beprev = be;

	Serial.print(target);
	Serial.print("  ");
	Serial.print(apos);
	Serial.print("  ");
	Serial.print(bpos);
	Serial.println();

}
2

The way that you read the encoders could use improvement.

How to read a rotary encoder.

The Encoder library makes reading encoders easier, adds debounce , direction detection.

3

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.