Differential drive. Feedback control problem

Projects General Guidance
1

Hi guys/gals.

So a little background, I'm working on a project where I am controlling 2 motors and I am wanting to use some encoders i've just installed as feedback signal to sync the speeds of each motors. I'm hoping this will help me in instances where the robot must drive straight.

Anyway what I'm trying to do is attach interrupts to two pins that have one signal from each encoder plugged into it. Each interrupt function should calculate the period of the corresponding waveform. I want to feed the difference of the periods into the arduino's PID algorithm to speed up/slow down the motors to match each others speed.

Obviously though, this approach does not seem to work and after spending all day on it, I'm reaching out to ya'll. Any help would be appreciated :slight_smile:

This is just a simple test sketch that uses code from Simon Tushev's blog on a post about measuring frequency with arduinos.
http://tushev.org/articles/arduino/item/51-measuring-frequency-with-arduino

P.S: I've tested my encoders so at least I know they are working. Kinda a newbie still so be gentle...

#include <PID_v1.h>

#define SAMPLES 1024

int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;     //M1 Direction Control
int M2 = 7;     //M1 Direction Control
int pinA = 2;
int pinB = 3;
int i = 0;
long PerA, PerB;
double Setpoint=0, phaseDiff=0, forwardPWM=50;

PID myPID(&phaseDiff, &forwardPWM, &Setpoint,2,5,1, DIRECT);

void setup() 
{
  pinMode(2,INPUT);
  pinMode(3,INPUT);
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);
  digitalWrite(E1,LOW);   
  digitalWrite(E2,LOW);
  digitalWrite(2,HIGH); // Turn on pullup resisitors
  digitalWrite(3,HIGH);
  
  myPID.SetMode(AUTOMATIC); //Create an instance of PID
  
  attachInterrupt(0, getPeriodA, CHANGE); //Attach an interrupt to encoder 1 signal A
  attachInterrupt(1, getPeriodB, CHANGE); //Attach an interrupt to encoder 2 signal A
  Serial.begin(115200);
  Serial.print("Start...");
} 

void stop(void)                    //Stop
{
  digitalWrite(E1,0); 
  digitalWrite(M1,LOW);    
  digitalWrite(E2,0);   
  digitalWrite(M2,LOW);    
}   
void advance(char a,char b)          //Move forward
{
  analogWrite (E1,a);      //PWM Speed Control
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}  

void loop(void) 
{
  advance (forwardPWM,forwardPWM);
  myPID.Compute();
  calcPhaseDiff();
  
}

void getPeriodA() {
  long measA = 0;
  for(unsigned int j=0; j<SAMPLES; j++) measA+= 500000/pulseIn(2, CHANGE, 250000); //Grab 1024 samples of the period
  PerA = measA / SAMPLES; //Calcualte the average period of A
  Serial.println(PerA);
}

void getPeriodB() {
  long measB = 0;
  for(unsigned int j=0; j<SAMPLES; j++) measB+= 500000/pulseIn(3, CHANGE, 250000);
  PerB = measB / SAMPLES;
  Serial.println(PerB);
}
void calcPhaseDiff(){
  phaseDiff = abs(PerA-PerB);
}
2

A number of things:

  • It is not clear what you are trying to accomplish. You read two encoders and calculate the speed diffrence between them and then regulate on that difference. Then you feed the sameregulator signal to both motors?? That would not regulate anything at all. If you want the motors to run in sync you should use one as a master with a fixed signal and feed the regulator output to the other.
  • Encoders normally have two outputs inn order to measure both angle and direction. In your case you have only connected one wich means that you can measure speed only, not direction
  • You should not use uneccessary delays like serial commands inside your interrupt handlers.
  • This also applies to the measurement method. You cannot spend an undeterminded time insida an interrupt handler, That simply does not work. What you can do is increment a counter on each interrupt and then calculate frequency at regular time intervalls
  • waiting for 1024 pulses will take a long time even if not in a interruopt handler, It will be very difficult to regulate on such a long time period

Here is some code to measure the frequency on two inputs without interrupt. It is based on my statemachine library found here: Arduino Playground - SMlib

100Hz works fine, 1kHz is a little glitchy. Please observe that all code must be non-blocking for this to work.

#include <SM.h>

const int Samples = 32;
SM M1(M1Reset, M1Count);
int M1PulseCount;
int M1Freq;
int M1EncState;


SM M2(M2Reset, M2Count);
int M2PulseCount;
int M2Freq;
int M2EncState;

SM EachSec(ES, ESwait);

void setup(){
  Serial.begin(115200);
  Serial.println("ready..");
}//setup()

void loop(){
  EXEC(M1);
  EXEC(M2);
  EXEC(EachSec);
  //your code for PID calc etc goes here, must be non-blocking
}//loop()

State M1Reset(){
  M1PulseCount=0;
}//M1Reset()

State M1Count(){
  if(RE(digitalRead(2), M1EncState)) M1PulseCount++;
  if(M1PulseCount == Samples){
    M1Freq = 1000*Samples/M1.Statetime();
    M1.Set(M1Reset, M1Count);
  }//if(M1Pulsecount)
}//M1Count()

State M2Reset(){
  M2PulseCount=0;
}//M2Reset()

State M2Count(){
  if(RE(digitalRead(3), M2EncState)) M2PulseCount++;
  if(M2PulseCount == Samples){
    M2Freq = 1000*Samples/M2.Statetime();
    M2.Set(M2Reset, M2Count);
  }//if(M1Pulsecount)
}//M2Count()


State ES(){
  Serial.print("M1: ");
  Serial.print(M1Freq);
  Serial.print("\tM2: ");
  Serial.println(M2Freq);
}//ES()

State ESwait(){
  if(EachSec.Timeout(1000)) EachSec.Set(ES, ESwait);
}//ESwait()
3

Ok, thanks for the info. What does non-blocking code mean?

4

Non blocking code means not introducing uneccessary delays that is time to pass like the delay() function or waiting for some i/o state. You should check it and come back later if neccessary

5

There are several types of control involved here but what I think you're trying to do is use feedback for each motor to make that motor run at the speed you have selected.

In that case I'd use a separate PID instance for each motor. The input would be the difference between the desired and actual speed of that motor. The output would be the power to be applied to that motor. It would probably be sensible to represent that as a signed int where +255 means full power forwards and -255 means full power in reverse and 0 means no power.

If you're using differential steering then it would probably also make sense to manage the average speed of the two motors and the differential speed separately. The average speed is obviously how fast you want the vehicle to move. The differential speed gives the yaw rate. On one side the target speed is the average plus the differential speed; on the other side, it's the average minus the differential speed.