Changing A Motor Code To Work For Two Motors

Hi everyone, I am pretty new here. This is my first post. I have a code that works for one motor. I did some changes to make it work for two motors. However, second motors position reading returns always 0. I suspect it is because there are two interrupts. As I said, I'm pretty new. I don't know how interrupts work exactly. Also, this code's job is to basically stop the motor at the target position.

Code For a Single Motor:

// This alternate version of the code does not require
// atomic.h. Instead, interrupts() and noInterrupts() 
// are used. Please use this code if your 
// platform does not support ATOMIC_BLOCK.

#define ENCA 2 // YELLOW
#define ENCB 3// WHITE
#define PWM 11
#define IN2 10
#define IN1 9

volatile int posi = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
float eprev = 0;
float eintegral = 0;

void setup() {
  Serial.begin(9600);
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
  
  pinMode(PWM,OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  
  Serial.println("target pos");
}

void loop() {

  // set target position
  //int target = 1200;
  int target = 300;

  // PID constants
  float kp = 5.6;
  float kd = 0.36;
  float ki = 0.6;

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position
  int pos = 0; 
  noInterrupts(); // disable interrupts temporarily while reading
  pos = posi;
  interrupts(); // turn interrupts back on
  
  // error
  int e = pos - target;

  // derivative
  float dedt = (e-eprev)/(deltaT);

  // integral
  eintegral = eintegral + e*deltaT;

  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;

  // motor power
  float pwr = fabs(u);
  if( pwr > 255 ){
    pwr = 255;
  }

  // motor direction
  int dir = 1;
  if(u<0){
    dir = -1;
  }

  // signal the motor
  setMotor(dir,pwr,PWM,IN1,IN2);


  // store previous error
  eprev = e;

  Serial.print(target);
  Serial.print(" ");
  Serial.print(pos);
  Serial.println();
}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}

void readEncoder(){
  int b = digitalRead(ENCB);
  if(b > 0){
    posi++;
  }
  else{
    posi--;
  }
}

Code For Two Motors:

// This alternate version of the code does not require
// atomic.h. Instead, interrupts() and noInterrupts() 
// are used. Please use this code if your 
// platform does not support ATOMIC_BLOCK.

#define L_ENCA 2 // YELLOW
#define L_ENCB 3// GREEN
#define L_PWM 10
#define L_IN2 9
#define L_IN1 8

#define R_ENCA 4 // YELLOW
#define R_ENCB 5// GREEN
#define R_PWM 13
#define R_IN2 12
#define R_IN1 11

volatile int posiL = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
volatile int posiR = 0;
long prevT = 0;
float eprevL = 0;
float eprevR = 0;
float eintegralL = 0;
float eintegralR = 0;

void setup() {
  Serial.begin(9600);
  pinMode(L_ENCA,INPUT);
  pinMode(L_ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(L_ENCA),readEncoderL,RISING);
  
  pinMode(L_PWM,OUTPUT);
  pinMode(L_IN1,OUTPUT);
  pinMode(L_IN2,OUTPUT);
  
  Serial.println("target pos");

  pinMode(R_ENCA,INPUT);
  pinMode(R_ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(R_ENCA),readEncoderR,RISING);
  
  pinMode(R_PWM,OUTPUT);
  pinMode(R_IN1,OUTPUT);
  pinMode(R_IN2,OUTPUT);
  
  Serial.println("target pos");
}

void loop() {

  // set target position
  //int target = 1200;
  int target = 600;

  // PID constants
  float kp = 5.6;
  float kd = 0.36;
  float ki = 0.6;

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position
  int posL = 0; 
  int posR = 0;
  noInterrupts(); // disable interrupts temporarily while reading
  posL = posiL;
  interrupts(); // turn interrupts back on
  noInterrupts(); // disable interrupts temporarily while reading
  posR = posiR;
  interrupts(); // turn interrupts back on
  
  // error
  int eL = posL - target;
  int eR = posR - target;

  // derivative
  float dedtL = (eL-eprevL)/(deltaT);
  float dedtR = (eR-eprevR)/(deltaT);

  // integral
  eintegralL = eintegralL + eL*deltaT;
  eintegralR = eintegralR + eR*deltaT;

  // control signal
  float uL = kp*eL + kd*dedtL + ki*eintegralL;
  float uR = kp*eR + kd*dedtR + ki*eintegralR;

  // motor power
  float pwrL = fabs(uL);
  if( pwrL > 255 ){
    pwrL = 255;
  }
  float pwrR = fabs(uR);
  if( pwrR > 255 ){
    pwrR = 255;
  }
  // motor direction
  int dirL = 1;
  if(uL<0){
    dirL = -1;
  }
  int dirR = 1;
  if(uR<0){
    dirR = -1;
  }

  // signal the motor
  setMotor(dirL,pwrL,L_PWM,L_IN1,L_IN2);
  setMotor(dirR,pwrR,R_PWM,R_IN1,R_IN2);


  // store previous error
  eprevL = eL;
  eprevR = eR;

  Serial.print(target);
  Serial.print(" ");
  Serial.print(posL);
  Serial.print(" ");
  Serial.print(posR);
  Serial.println();
}

void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
  analogWrite(pwm,pwmVal);
  if(dir == 1){
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }
  else if(dir == -1){
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else{
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
  }  
}

void readEncoderL(){
  int bL = digitalRead(L_ENCB);
  if(bL > 0){
    posiL++;
  }
  else{
    posiL--;
  }
}
void readEncoderR(){
  int bR = digitalRead(R_ENCB);
  if(bR > 0){
    posiR++;
  }
  else{
    posiR--;
  }
}

Output: (Position value of first motor also changes even though it stopped and position value of second motor is always 0.)
image_2022-06-01_093528290

Where is the interrupt call for the other encoder?

here?

Guys, I found something. It says only 2 and 3 pins are for interrupt.

Only 2 and 3 pins work with interrupts. So, I am going to set INCAs to 2 and 3 and INCBs to 4 and 5. I hope it works.

:+1:

It worked. Thanks for the help anyways.

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.