How exact is same exact speed? Variation in load, wheel diameter, and surface would likely need some adjustment. With an analogWrite()'s 0-255, PWM has a granularity/resolution of 0.4%, so it wouldn't be the PWM or encoder alone to keep things straight.
If you need to map a +/- speed to direction and PWM, that could be a function:
void motorA(int speed){
digitalWrite(dirPinA,speed>=0);
analogWrite(pwmPinA,abs(speed)*CALIBRATION_CONSTANT);
}
How many pulses per rev do the motors produce? What diameter are the wheels?
Most PID libraries I've seen take one input measurement and produce one output measurement--the measurement is not normally integrated into the PID library.
I'd start with some code to excercise the motors and and see the pulses/time you get:
#include <PID_v1.h> // https://playground.arduino.cc/Code/PIDLibrary/
const byte dirPinA = 8 ;
const byte dirPinB = 9 ;
const byte pwmPinA = 10 ;
const byte pwmPinB = 11;
const byte pulseA = 2; // interrupt pin
const byte pulseB = 3; // interrupt pin
const byte controlPin = A0; //
volatile unsigned int countA, countB; // 16 bit counters
//Define Variables we'll be connecting to
double SetpointA, InputA, OutputA;
double SetpointB, InputB, OutputB;
//Specify the links and initial tuning parameters
PID PID_A(&InputA, &OutputA, &SetpointA, 2, 0, 0, DIRECT);
PID PID_B(&InputB, &OutputB, &SetpointB, 2, 0, 0, DIRECT);
int speedA(void) {
static unsigned long last = 0;
static unsigned int lastCount = 0;
static int speed = 0;
unsigned int delta;
if (millis() - last >= 100) {
// magic to turn pulseA into speed;
noInterrupts();
delta = (countA - lastCount); // pulses/100ms
lastCount = countA;
interrupts();
speed = delta * 10; // pulses/sec
last += 100;
}
return speed;
}
int speedB(void) {
static unsigned long last = 0;
static unsigned int lastCount = 0;
static int speed = 0;
unsigned int delta;
if (millis() - last > 100) {
// magic to turn pulseB into speed;
noInterrupts();
delta = (countA - lastCount) * 10; // pulses/100ms;
lastCount = countA;
interrupts();
speed = delta * 10; // pulsess/sec
last += 100;
}
return speed;
}
void ISR_A(void) { // count rising pulses
countA++;
}
void ISR_B(void) { // count rising pulses
countB++;
}
void report(void) {
static unsigned long nextReport = 0;
if ((signed long)(millis() - nextReport) < 0)
return;
Serial.print("Counts A/B: ");
Serial.print(countA);
Serial.print('/');
Serial.print(countA);
Serial.print(' Speed A/B');
Serial.print(speedA());
Serial.print('/');
Serial.print(speedB());
Serial.println(" counts/sec");
}
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
pinMode(dirPinA, OUTPUT);
pinMode(dirPinB, OUTPUT);
pinMode(pwmPinA, OUTPUT);
pinMode(pwmPinB, OUTPUT);
pinMode(pulseA, INPUT);
pinMode(pulseA, INPUT);
pinMode(controlPin,INPUT);
attachInterrupt(digitalPinToInterrupt(pulseA), ISR_A, RISING);
attachInterrupt(digitalPinToInterrupt(pulseB), ISR_B, RISING);
SetpointA = 0;
SetpointB = 0;
PID_A.SetOutputLimits(-255,255);
PID_B.SetOutputLimits(-255,255);
PID_A.SetMode(AUTOMATIC);
PID_B.SetMode(AUTOMATIC);
}
void motorA(int speed){
const int CALIBRATION_CONSTANT=1;
digitalWrite(dirPinA,speed>=0);
analogWrite(pwmPinA,abs(speed)*CALIBRATION_CONSTANT);
}
void motorB(int speed){
const int CALIBRATION_CONSTANT=1;
digitalWrite(dirPinB,speed>=0);
analogWrite(pwmPinB,abs(speed)*CALIBRATION_CONSTANT);
}
void handleMotors(void) {
// measure
InputA = speedA();
InputB = speedB();
// update
PID_A.Compute();
PID_B.Compute();
// do
motorA(OutputA);
motorB(OutputB);
}
void loop() {
// put your main code here, to run repeatedly:
// plan
SetpointA = SetpointB = map(analogRead(controlPin),0,1024,-1000,1000);
handleMotors();
report();
}