Hello, I'm trying to control the position of a EMG 30 DC motor using a PID control loop and the feedback given by the optical encoder. What I want to do as a finality is to make the motor oscillate between to positions with a sinusoidal trajectory (that will recquire speed control too I believe). For that I use an Arduino Uno and a motor shield. Unfortunately after having implemented a PID control loop for position that seems correct to me, I'm not able to make the thing work. Actually, my motor starts spinning and then never stops... I use the attachInterrupt function in order to make the motor move in combination with the optical encoder as I have seen it in several other topics on the forum.
I'm very new to Arduino and accordingly I'm not very comfortable with it at the moment. So if anyone has an idea, it would be great. I post my code in order to illustrate my problem. If you need anything else, tell me.
#include "Arduino.h"
int _DIRA = 12;
int _PWMA = 3;
int _BRKA = 9;
#define TRUNCATE(value, mi, ma) min(max(value, mi), ma)
// Maximum error integral
#define SERROR_MAX 32768
// Maximum PWM value
#define PWM_MAX 255
// Gain scaling factor (yes, we want possibly non-integer gains)
#define Kscaling 0.2
// Proportional gain (response speed)
// Ku = 37
#define Kp (37*Kscaling/3)
// Integral gain (speed at which the "drift" is handled)
#define Ki (2*Kp/100)
// Derivative gain (rate of error change handling, probably useless)
#define Kd Kp*10/3
int tick=0;//optical encoder
long Serreur=0;//integral correction
long lerreur=0;//last error
long Derreur;//derivative correction
long sp,dir,cmd,erreur;
void inc() //move forwards
{
tick++;
}
void dec() //move backwards
{
tick--;
}
void test(int cmd)
{
// Compute current error
erreur = tick - cmd;
// Error integral, with a maximum value (avoid blowing stuff up in case of drift)
Serreur = TRUNCATE(Serreur + erreur, -SERROR_MAX, SERROR_MAX);
// Error derivative
Derreur = erreur - lerreur;
// Keep track of the last error value
lerreur = erreur;
// Ye olde PID formula
sp = TRUNCATE((Kp*erreur + Ki*Serreur + Kd*Derreur)/Kscaling, -255, 255);
// Handle speed sign
if (sp>=0){
dir = HIGH;
} else {
dir = LOW;
sp = -sp;
}
}
void encoder() //choose direction to move according to what says the optical encoder
{
if (digitalRead(2)==digitalRead(4)) //2 and 4 are the pins where the optical encoder is plugged in
{
inc();
}
else
{
dec();
}
}
// Output the PWM command and direction
void action()
{
analogWrite(_PWMA, sp);
digitalWrite(_DIRA, dir);
}
void setup()
{
pinMode(_DIRA,OUTPUT);
pinMode(_BRKA,OUTPUT);
pinMode(2,INPUT);
pinMode(4,INPUT);
digitalWrite(2,HIGH);
digitalWrite(4,HIGH);
digitalWrite(_BRKA, LOW);
attachInterrupt(0,encoder,CHANGE);
}
void loop()
{
cmd=5;
test(cmd);
action();
}
!. Now, this is an efficiente code to track position, it will give you double or quadruple encoder resolution:
