Howdy ya’ll,
I have attached links to my parts and a wiring diagram as well as code and sample serial monitor output. I’m using an Arduino to power two 25D HP Pololu motors through the MC33926 motor shield. Several days ago, the serial monitor readout started giving me irregular readings from the encoders. This was strange, because our code has been working perfectly for months, but given that we’ve been rough with the craft and possibly wires, I thought maybe there was a disconnect or damaged part. The thing is… I’ve replaced literally every hardware component for a benchtop test and the readings are still irregular. A code which was giving 5 readings per second smoothly is now unable to give me a consistent output, regardless if its at 200 milliseconds, 500, or 1000. Specifically, the following has been tried:
-Replacing laptop (I have Windows, my undergrad has Mac, same issues)
-Replacing USB cable (no change)
-Replacing Arduino (no change, tried three new ones)
-Replacing MC33926 Motor Shield (no change)
-Replacing all wires, testing continuity and shorts with voltmeter (all passed and no changes)
-Replacing motors with two new ones, fresh out of the box, first individually and then both (no changes)
-Pulling the yellow encoder wire (feedback goes haywire without this reading but same problem persists) from Arduino
-Pulling white encoder wires (the serial moniter prints remain identical with white wires attached or unattached. This is the only behavior which could prove useful.) from Arduino
-Pulling encoder power (simply stops prints to serial monitor as encoders are not running)
-The sample output I tested both with and without the timestamp. It did not seem to have an impact.
-Testing different read/write rates. No change.
In short, I’m stuck, I’ve spent two full days troubleshooting this in a meticulous fashion with no headway, and I am now getting increasingly frustrated with no apparent cause in sight. I’m hoping someone encountered this issue. It could be Arduino code, but perhaps it is faulty hardware or a coincidental bad connection. Again, let me emphasize: this code was performing perfectly at 200ms sample rate for months! A few weeks ago this code was working flawlessly. Then the encoder readings started sampling irregularly.
Example output:
Dual MC33926 Motor Shield
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 89 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 89 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 89 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 89 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -59 RPM2: -63 k1 V: 89 k2 V: 90 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 88 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 88 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 88 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 88 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -59 k1 V: 90 k2 V: 88 M1 current: 108 M2 current: 108
RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 89 M1 current: 108 M2 current: 108
RPM1: -59 RPM2: -63 k1 V: 88 k2 V: 89 M1 current: 108 M2 current: 108
16:26:37.849 -> RPM1: 0 RPM2: 0 k1 V: 89 k2 V: 87 M1 current: 144 M2 current: 144
16:26:38.324 -> RPM1: -75 RPM2: -63 k1 V: 126 k2 V: 99 M1 current: 108 M2 current: 108
16:26:38.904 -> RPM1: -67 RPM2: -75 k1 V: 93 k2 V: 103 M1 current: 108 M2 current: 108
16:26:40.196 -> RPM1: -63 RPM2: -63 k1 V: 94 k2 V: 91 M1 current: 108 M2 current: 108
16:26:41.583 -> RPM1: -63 RPM2: -63 k1 V: 92 k2 V: 89 M1 current: 108 M2 current: 108
16:26:42.462 -> RPM1: -67 RPM2: -63 k1 V: 93 k2 V: 89 M1 current: 108 M2 current: 108
16:26:43.238 -> RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
16:26:44.905 -> RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
16:26:48.059 -> RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
16:26:48.703 -> RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
16:26:49.991 -> RPM1: -63 RPM2: -63 k1 V: 90 k2 V: 90 M1 current: 108 M2 current: 108
Arduino Uno: Arduino Uno Rev3 — Arduino Online Shop
Motor Shield: Pololu Dual MC33926 Motor Driver Shield for Arduino
Motors: Pololu - 75:1 Metal Gearmotor 25Dx66L mm HP 12V with 48 CPR Encoder (No End Cap)
Code:
//*************************************************************/
//Numbers in this code are for 28V input to motor controller/shield
#include <Wire.h>
#include <SPI.h>
#include <DualMC33926MotorShield.h>
#define encodPinA1 2 // encoder A pin motor 1
#define encodPinB1 6 // encoder B pin motor 1
#define encodPinA2 3 // encoder A pin motor 2
#define encodPinB2 5 // encoder A pin motor 2
//These working controller gains are for CounterRotating B150_20RPM
float Kp = 0.2; // PID proportional control Gain
float Kd = 0.4; // PID Derivitave control gain
float Ki = 0;
int speed_req = 60;
int error_sum = 0;
float pidTerm = 0;
int error = 0;
int last_error = 0;
int k = 50;
int k_new = 0;
int k1 = k;
int k2 = k;
int t1 = 0;
int t2 = 0;
int t3 = 0;
int t4 = 0;
volatile long count1 = 0; // rev counter
volatile long count2 = 0;
int LOOPTIME = 50;
int LOOPTIME2 = 200;
int speed_act1 = 0; // speed (actual value)
int speed_act2 = 0;
int current = 0; // in mA
int dt = 0;
DualMC33926MotorShield md;
void setup () {
//Set up Motor Controller
Serial.begin(115200);
Serial.println("Dual MC33926 Motor Shield");
md.init();
t1 = millis();
//Set up Encoders
pinMode(encodPinA1, INPUT);
pinMode(encodPinB1, INPUT);
digitalWrite(encodPinA1, HIGH); // turn on pullup resistor
digitalWrite(encodPinB1, HIGH);
attachInterrupt(0, rencoder1, FALLING);
pinMode(encodPinA2, INPUT);
pinMode(encodPinB2, INPUT);
digitalWrite(encodPinA2, HIGH); // turn on pullup resistor
digitalWrite(encodPinB2, HIGH);
attachInterrupt(1, rencoder2, FALLING);
initial_k();
}
//Gains for B150 and Counter rotating
void initial_k()
{
if (speed_req == 20)
{ k1 = 50;
k2 = 50;
}
if (speed_req == 40)
{ k1 = 80;
k2 = 80;
}
if (speed_req == 60)
{ k1 = 90;
k2 = 90;
}
if (speed_req == 80)
{ k1 = 170;
k2 = 170;
}
if (speed_req == 100)
{ k1 = 199;
k2 = 199;
}
}
void loop () {
//Motor A forward
k1 = constrain(k1, 0, 229);
k2 = constrain(k2, 0, 229);
md.setM1Speed(k1);
// put k1 negative for counter rotating and positive for crabwalking. This moves to the right for crabwalking if craft is facing forward.
//Motor B backward
md.setM2Speed(k2);
t2 = millis() ;
t4 = millis();
if (t2 - t1 >= LOOPTIME) {
getMotorData1();
getMotorData2();
if (current > 100) {
displayData();
k1 = updatePid(k1, speed_req, abs(speed_act1));
k2 = updatePid(k2, speed_req, abs(speed_act2));
}
t1 = t2;
}
}
void rencoder1() { // pulse and direction, direct port reading to save cycles
if (digitalRead(encodPinB1) == HIGH) count1 ++; // if(digitalRead(encodPinB1)==HIGH) count ++;
else count1--; // if (digitalRead(encodPinB1)==LOW) count --;
}
void rencoder2() { // pulse and direction, direct port reading to save cycles
if (digitalRead(encodPinB2) == HIGH) count2 ++; // if(digitalRead(encodPinB1)==HIGH) count ++;
else count2--; // if (digitalRead(encodPinB1)==LOW) count --;
}
void getMotorData1() { // calculate speed, volts and Amps
static long countAnt1 = 0; // last count
t2 = millis();
dt = t2 - t1;
speed_act1 = ((count1 - countAnt1) * (60 * (1000 / dt))) / (48 * 75) * 4.22222; // 48 pulses X 75 gear ratio = 3600 counts per output shaft rev
current = md.getM1CurrentMilliamps(); // motor curren
count1 = 0;
}
void getMotorData2() { // calculate speed, volts and Amps
static long countAnt2 = 0; // last count
t2 = millis();
dt = t2 - t1;
speed_act2 = ((count2 - countAnt2) * (60 * (1000 / dt))) / (48 * 75) * 4.22222; // 48 pulses X 75 gear ratio = 3600 counts per output shaft rev
current = md.getM2CurrentMilliamps(); // motor current
count2 = 0;
}
void displayData() {
if (t4 - t3 >= LOOPTIME2) {
Serial.print("RPM1: ");
Serial.print(speed_act1);
Serial.print("\t");
Serial.print("RPM2: ");
Serial.print(speed_act2);
// Serial.print("\t");
// Serial.print("k1 V: ");
// Serial.print(k1);
// Serial.print("\t");
// Serial.print("k2 V: ");
// Serial.print(k2);
// Serial.print("\t");
// Serial.print("M1 current: ");
// Serial.print(current);
// Serial.print("\t");
// Serial.print("M2 current: ");
// Serial.print(current);
Serial.println();
t3 = t4;
}
}
int updatePid(int number_k, int targetValue, int currentValue) { // compute PWM value
error = abs(targetValue) - abs(currentValue);
pidTerm = (Kp * error) + (Kd * (error - last_error)) + error_sum * Ki;
last_error = error;
error_sum += error;
return constrain(number_k + int(pidTerm), 0, 399);
}