0

I'll try to keep this brief and add code/output at the bottom. I am using an Arduino Uno with a Pololu motor shield and two Pololu motors. I am driving speed control using the encoders at the ends of the motors for a small craft.

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.

I have attached a wiring diagram at the bottom for supplemental information. Does anyone know why this might happened?

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

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



Wiring Diagram

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.