PID control with two motors

[kclprectech]

Hi!

I am trying to control two DC motors with PID control. I have the code for one motor with is pretty accurate. Howevevr, I am not sure how to about having it controlling two motors to reach two different angles?

This is the code for 1 motor and I need it to implement it to 2:

 
#include <MatrixMath.h>
#define pi 3.1415926535897932384626433832795

int gearRatio = 298;
int ppr = 3;
//motor 1 
const int enablePin1 = 8;
const int pin1 = 10;
const int pin2 = 12;


//encoder 
const byte interruptPin = 3;
const int pinEncoder = 9; //interrupt pin 

//PID constants 

double Kp1 = 10;
double Kd1 = 0.7;
double Ki1 = 1;

double demandPosition = 0;
volatile float currentPosition = 0;
double errorPosition = 2;
double errorPosition_diff = 0;
double errorPosition_sum = 0;
double errorPosition_prev = 0;
double Output = 0;
double integral;

volatile long int Position = 0; //count ticks from encoder 

float degPosition = 0.0; 
float radPosition = 0.0;

int flag = 0;

//  Initialize timer
long currentTime = 0;
long previousTime = millis(); 
double Time;
 
const int dt = 10; //ms


void setup () {
  
  Serial.begin(115200);
  pinMode(enablePin1,OUTPUT);
  digitalWrite(enablePin1, HIGH);
  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);


  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(pinEncoder, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin), encoder, RISING);
}

void loop () {
  if (flag==0) {
  currentTime = millis();
  Time = float(currentTime - previousTime); 
  if (Time>dt) {
        Time/=1000;
        if(degPosition>=360.0) {
        Position = 0; 
        }
        if (degPosition<=-360.0) {
        Position = 0; 
        }
        degPosition = (Position*360.0/(ppr*gearRatio));
        radPosition = degPosition*0.0174532925;

        demandPosition = 90.0;
        currentPosition = degPosition;//update current encoder reading
        errorPosition = demandPosition-currentPosition;

        
        Serial.print(demandPosition, 3);
        Serial.print("\t");
        Serial.print(currentPosition, 3);
        Serial.print("\t");
        //Serial.print(errorPosition, 3);
        //Serial.print("\t");
     
          errorPosition_diff = (errorPosition-errorPosition_prev)/Time;
          errorPosition_sum += errorPosition*Ki1*Time;
          if (errorPosition_sum>255) {errorPosition_sum = 255;}
          if (errorPosition_sum <-255) {errorPosition_sum = -255;}
       
       Output = errorPosition*Kp1 + errorPosition_diff*Kd1 + errorPosition_sum;

       Serial.print("Output: ");
       Serial.println(Output);
  
       errorPosition_prev = errorPosition;
       previousTime = currentTime; 

        if (Output >= 0) {
        if(Output>255) {Output = 255;}
        else if(Output<70) {Output = Output+70;}
        }
        else {
        if(Output <-255) {Output = -255;}
        else if(Output>-70) {Output = Output-70;}
        }
        
        if (Output >= 0) {
          analogWrite(pin1, round(Output));
          analogWrite(pin2, 0);
  
        } else {
          analogWrite(pin2, round(-Output));
          analogWrite(pin1, 0); 
       
        }
     
   }
 
  }
}

void encoder() {
  if(digitalRead(pinEncoder)==LOW){
    Position++;
  }else {
    Position--;
  }
}

Thank you

[jremington]

Two independent loops.

[gcjr]

here's where a c++ class could be useful (or C struct with functions)

most of the variables and constants in your code would be members of the class. your routine a method. the current position value, an argument to that method.

you may then want other methods to possibly specify the values of constants, pins, timing values, ... or they could be specified in a constructor for the class.

looks like you would need two interrupt service routines, one for the encoder on each motor. they could be part of the classs.

you would instantiate two instances of the class, one for each motor along with the differences (e.g. pins, consts) for each motor.

you may want to deal with timing, outside of the class, or maybe the current time (msec) is passed as an argument along with the feedback value.

when thinking about how to re-organize you code, you may understand why you see code written by others in certain ways that make it more modular so that it can easily be used multiple times