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.)
