How can I use two DC mortors with PID control.

int speed1 = 10;
int dir1 = 12;
int encoderPin1 = 2;
int encoderPin2 = 3;
int speed2 = 9;
int dir2 = 11;
int encoder1 = 4;
int encoder2 = 5;

volatile int lastEncoded = 0;
volatile long encoderValue = 0;
long lastencoderValue = 0;

int lastMSB = 0;
int lastLSB = 0;
int lastASB = 0;
int lastBSB = 0;

float Kp = 1;
float Ki = 0;
float Kd = 0;
float error=0;
float error_previous=0;

float desired_encoder=0;
float P_control, I_control, D_control;
float Time_previous=0;
float Time=0;
float PID_control;

void setup() {
Serial.begin(9600);
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
pinMode(encoder1, INPUT_PULLUP);
pinMode(encoder2, INPUT_PULLUP);
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
Serial.println(“INPUT desired encoder value”);
}
void loop() {
float DT;

if (Serial.available()) {
desired_encoder = Serial.parseInt();
Serial.print("desired_encoder : ");
Serial.println(desired_encoder);
}
//Kp=float(analogRead(A0)/100.0);
//Serial.print("Kp : ");
//Serial.println(Kp);

error = desired_encoder - encoderValue;
Time=millis();
DT =(Time-Time_previous)/1000;
Time_previous=Time;
P_control = Kp * error;
I_control += Ki * error * DT;
D_control = Kd * (error - error_previous) / DT;

PID_control = P_control + I_control + D_control;

if (PID_control>0){
digitalWrite(dir1,HIGH);
analogWrite(speed1, PID_control);
digitalWrite(dir2,HIGH);
analogWrite(speed2, PID_control);
}
else{
PID_control=PID_control*(-1);
digitalWrite(dir1,LOW);
analogWrite(speed1, PID_control);
digitalWrite(dir2,LOW);
analogWrite(speed2, PID_control);
}
error_previous = error;

Serial.print(“PID_control : “);
Serial.print(PID_control);
Serial.print(” encoderValue : “);
Serial.println(encoderValue);
Serial.println(”\n”);

}

void updateEncoder(){
int MSB = digitalRead(encoderPin1);
int LSB = digitalRead(encoderPin2);
int ASB = digitalRead(encoder1);
int BSB = digitalRead(encoder2);

int encoded = (MSB << 1) |LSB;
int sum = (lastEncoded << 2) | encoded;
int encoded1 = (ASB << 1) |BSB;
int sum1 = (lastEncoded << 2) | encoded;

if(sum == 0b1101 || sum == 0b0100 ||
sum == 0b0010 || sum == 0b1011)
encoderValue ++;
if(sum == 0b1110 || sum == 0b0111 ||
sum == 0b0001 || sum == 0b1000)
encoderValue --;

lastEncoded = encoded;
}

This is my sketch.
please find this sketch’s problem :slight_smile:

scd0208:
This is my sketch.
please find this sketch’s problem :slight_smile:

That’s going to be a problem …

You have not told us how the program in intended to work - what the different parts of the code do.
You have not told us what it actually does.
You have not told us what you want it to do that is different.

If you want to use PID control is there any reason not to use the PID library?

Also To make it easier for people to help you please modify your post and use the code button </> so your code looks like this and is easy to copy to a text editor. See How to use the Forum

…R

Well, you declare and initialize a couple of variables that you don’t use:

/Users/john/Documents/Arduino/sketch_nov03a/sketch_nov03a.ino:95:7: warning: unused variable 'encoded1' [-Wunused-variable]
   int encoded1 = (ASB << 1) | BSB;
       ^
/Users/john/Documents/Arduino/sketch_nov03a/sketch_nov03a.ino:96:7: warning: unused variable 'sum1' [-Wunused-variable]
   int sum1  = (lastEncoded << 2) | encoded;
       ^

That might point to a mistake.

Your choice of variable names leave a lot to be desired. This might get you a bit closer:

const byte  Encoder1Int0Pin = 2;
const byte  Encoder2Int1Pin = 3;


const byte  Encoder1OtherPin = 4;
const byte  Encoder2OtherPin = 5;


volatile long Encoder1Value = 0;
volatile long Encoder2Value = 0;


const byte  Speed2Pin = 9;
const byte  Speed1Pin = 10;
const byte  Direction2Pin = 11;
const byte  Direction1Pin = 12;


float Kp = 1;
float Ki = 0;
float Kd = 0;
float error = 0;
float error_previous = 0;


float desired_encoder = 0;
float P_control, I_control, D_control;
float Time_previous = 0;
float Time = 0;
float PID_control;


void setup() {
  Serial.begin(9600);
  pinMode(Encoder1Int0Pin, INPUT_PULLUP);
  attachInterrupt(0, updateEncoder1, CHANGE);
  pinMode(Encoder1OtherPin, INPUT_PULLUP);


  pinMode(Encoder2Int1Pin, INPUT_PULLUP);
  attachInterrupt(1, updateEncoder2, CHANGE);
  pinMode(Encoder2OtherPin, INPUT_PULLUP);


  Serial.println("INPUT desired encoder value");
}


void loop() {
  float DT;


  if (Serial.available()) {
    desired_encoder = Serial.parseInt();
    Serial.print("desired_encoder :  ");
    Serial.println(desired_encoder);
  }
  //Kp=float(analogRead(A0)/100.0);
  //Serial.print("Kp :  ");
  //Serial.println(Kp);


  error = desired_encoder - Encoder1Value;
  Time = millis();
  DT = (Time - Time_previous) / 1000;
  Time_previous = Time;
  P_control = Kp * error;
  I_control += Ki * error * DT;
  D_control = Kd * (error - error_previous) / DT;


  PID_control = P_control + I_control + D_control;


  if (PID_control > 0) {
    digitalWrite(Direction1Pin, HIGH);
    analogWrite(Speed1Pin, PID_control);
    digitalWrite(Direction2Pin, HIGH);
    analogWrite(Speed2Pin, PID_control);
  }
  else {
    PID_control = -PID_control;
    digitalWrite(Direction1Pin, LOW);
    analogWrite(Speed1Pin, PID_control);
    digitalWrite(Direction2Pin, LOW);
    analogWrite(Speed2Pin, PID_control);
  }
  error_previous = error;


  Serial.print("PID_control :  ");
  Serial.print(PID_control);
  Serial.print("         Encoder1Value :  ");
  Serial.println(Encoder1Value);
  Serial.println("\n");


}




void updateEncoder1() {
  int encoded, sum;
  static int lastEncoded1 = 0;


  // Check Encoder 1
  encoded = (digitalRead(Encoder1Int0Pin) << 1) | digitalRead(Encoder1OtherPin);
  sum  = (lastEncoded1 << 2) | encoded;
  lastEncoded1 = encoded;


  if (sum == 0b1101 || sum == 0b0100 ||
      sum == 0b0010 || sum == 0b1011)
    Encoder1Value ++;
  if (sum == 0b1110 || sum == 0b0111 ||
      sum == 0b0001 || sum == 0b1000)
    Encoder1Value --;
}


void updateEncoder2() {
  int encoded, sum;
  static int lastEncoded2 = 0;


  encoded = (digitalRead(Encoder2Int1Pin) << 1) | digitalRead(Encoder2OtherPin);
  sum  = (lastEncoded2 << 2) | encoded;
  lastEncoded2 = encoded;


  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011)
    Encoder2Value ++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000)
    Encoder2Value --;
}