Code at bottom. Hoping for some insight from the fine people here.
Currently using the following in a small vehicular craft:
Pololu - 75:1 Metal Gearmotor 25Dx66L mm HP 12V with 48 CPR Encoder (No End Cap) x2
Pololu Dual MC33926 Motor Driver Shield for Arduino x1
https://store.arduino.cc/usa/arduino-uno-rev3 x1
We developed our quadrature encoder reading code based off of previous feedback and it is working excellently. Our feedback control is also working, and can drive the motor towards the desired speed at the lower end of the range. However, as soon as we start nearing 60 RPM (130 RPM is max) we cannot draw any more current to get past this and drive higher speeds. This would make sense if the load was higher, but the power supply is indicating that we are only drawing about 2.5, 2.6A max @12V instead of the 3A apiece that the motor controller should allow.
Here's my conclusions and please, feel absolutely free to correct me. We instruct the craft to obtain a low speed such as 20 RPM. It does so flawlessly. This is because we give the motors 20RPM/120RPM * 12V of voltage, or 2V (we initialize at 12V but lets ignore that for now). It attemps to spin against a constant load. It cannot, and deals with the torque by increasing voltage to unknown levels. Since "R" is constant, a bump in V is a bump in I. It does the same thing up to about 60 RPM, gaining about 2.4A. But it is now apparent that at 60 RPM, it's already pullng the full 12V for correction. Thus telling it to go at 120 RPM is meaningless, because it's already pulling the 12V. Every speed given above 55 RPM or so shows identical results because it is pulling the same voltage after feedback. It appears that at the rated voltage, our motors are midway on the torque curve.
Except that absolutely should not happen. We know there's something wrong, because the stall rating for these motors is 5.6A and the motor controller is supposed to let 3A per motor through. We are receiving roughly 1.2A per motor, or reading 16% of stall torque. We ought to be pulling 110 RPMs under current load. That should be our terminal speed based on max power. Maybe less. And yet we're stalling, repeatedly. We know it's not a mechanical power transfer issue. It is therefore an electrical power transfer issue. Which is happening one of three places:
Power supply to motor controller: I'll have to check the exact model of power supply. It shouldn't be limited to 2.5A but... who knows.
Motor controller to motors: I'm really suspicious that the motor controller is artificially limiting the current to the motors somehow.
Arduino to motor controller: There could be something embedded in the code which is limiting current. What, I'm not sure. Could be in the libraries.
Bonus, wacky conspiracy theory: this behavior is easily explained if we received a low power motor instead of high power. At 12.3V, stalling at 2.3A is right on the money. May explain why all of a sudden we got poor results when we switched to brand new motors too.
Code:
//*************************************************************/
#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
//Max K=400
//float Kp = 0.4; // PID proportional control Gain
//float Kd = 1.5; // PID Derivitave control gain
float Kp = 0.8; // PID proportional control Gain
float Kd = 1.5; // PID Derivitave control gain
int speed_req = 120;
int e_speed_sum;
float pidTerm = 0;
int error = 0;
int last_error = 0;
int k = 200;
int k_new = 0;
int k1 = k;
int k2 = k;
int t1 = 0;
int t2 = 0;
volatile long count1 = 0; // rev counter
volatile long count2 = 0;
int LOOPTIME = 50;
int speed_act1 = 0; // speed (actual value)
int speed_act2 = 0;
int current = 0; // in mA
int dt = 0;
///About 76 RPM for 1 minute at 300///18
///About 49 ROM for 1 minute at 200///13
///200 45RPM
///175
//125-30RPM
//150-37RPM
DualMC33926MotorShield md;
void stopIfFault()
{
if (md.getFault())
{
Serial.println("fault");
while (1);
}
}
void initial_k()
{
if (speed_req == 30)
{ k1 = 160;
k2 = 160;
}
if (speed_req == 45)
{ k1 = 190;
k2 = 190;
}
if (speed_req == 60)
{ k1 = 240;
k2 = 240;
}
if (speed_req == 75)
{ k1 = 300;
k2 = 300;
}
if (speed_req == 90)
{ k1 = 335;
k2 = 335;
}
if (speed_req == 105)
{ k1 = 370;
k2 = 370;
}
if (speed_req == 120)
{ k1 = 399;
k2 = 399;
}
}
void setup () {
initial_k();
//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);
}
void loop () {
//Motor A forward
k1 = constrain(k1, 0, 399);
k2 = constrain(k2, 0, 399);
md.setM1Speed(-k1);
//Motor B backward
md.setM2Speed(k2);
t2 = millis() ;
if (t2 - t1 >= LOOPTIME) {
getMotorData1();
getMotorData2();
k1 = updatePid(k1, speed_req, abs(speed_act1));
k2 = updatePid(k2, speed_req, abs(speed_act2));
t1 = t2;
// Serial.print("k1: ");
// Serial.println(k1);
}
}
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 current
Serial.print("RPM1: ");
Serial.println(speed_act1);
// Serial.print("k1 ");
// Serial.println(k1);
//
// Serial.print ("M1: ");
// Serial.println(current);
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
Serial.print("RPM2: ");
Serial.println(speed_act2);
// Serial.print("k2 ");
// Serial.println(k2);
//
// Serial.print ("M2: ");
// Serial.println(current);
count2 = 0;
}
int updatePid(int number_k, int targetValue, int currentValue) { // compute PWM value
float pidTerm = 0; // PID correction
int error = 0;
static int last_error = 0;
error = abs(targetValue) - abs(currentValue);
pidTerm = (Kp * error) + (Kd * (error - last_error));
last_error = error;
return constrain(number_k + int(pidTerm), 0, 399);
}