Hi, I have wrote an code with timerOne library, and my PID is base on Brett Beauregard's library.
#include <TimerOne.h>
#define MotorR_I1 8
#define MotorR_I2 9
#define MotorL_I3 10
#define MotorL_I4 11
#define MotorR_ENA 5
#define MotorL_ENB 6
#define initial_speedr 100
#define initial_speedl 100
#define encoder0PinA 2
#define encoder0PinB 3
volatile double rpmcount=0.0;
bool inAuto = false;
#define manual 0
#define automatic 1
int speed_left;
double Output, Setpoint=48.0, lastInput=48.0,kp=1.0,ki=0.01,kd=0.01,outMin=45,outMax=100,ITerm;
void setup() {
Serial.begin(9600);
pinMode(2,INPUT);
pinMode(3,INPUT);
attachInterrupt(0,rpm_fun,CHANGE);
attachInterrupt(1,rpm_fun2 ,CHANGE);
pinMode(MotorR_I1,OUTPUT);
pinMode(MotorR_I2,OUTPUT);
pinMode(MotorL_I3,OUTPUT);
pinMode(MotorL_I4,OUTPUT);
pinMode(MotorR_ENA,OUTPUT);
pinMode(MotorL_ENB,OUTPUT);
analogWrite(MotorR_ENA,0);
analogWrite(MotorL_ENB,initial_speedl);
Timer1.initialize(1000);
Timer1.attachInterrupt( Compute_pid );
}
void Compute_pid()
{
if(!inAuto) return;
double error = Setpoint - rpmcount;
ITerm+= (ki*error*0.001);
if(ITerm> outMax) ITerm= outMax;
else if(ITerm< outMin) ITerm= outMin;
double dInput = (rpmcount - lastInput)/ 0.001;
if(error>=0)
Output =( kp * error + ITerm - kd * dInput)+45;
else Output =- ( kp * error + ITerm - kd * dInput)+45;
if(Output> outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
lastInput=rpmcount;
}
void advance(int a,int speed_left)
{
analogWrite(MotorL_ENB,speed_left);
digitalWrite(MotorR_I1,LOW);
digitalWrite(MotorR_I2,HIGH);
digitalWrite(MotorL_I3,LOW);
digitalWrite(MotorL_I4,HIGH);
delay(a * 100);
}
void stopRL(int d)
{
digitalWrite(MotorR_I1,HIGH);
digitalWrite(MotorR_I2,HIGH);
digitalWrite(MotorL_I3,HIGH);
digitalWrite(MotorL_I4,HIGH);
delay(d * 100);
}
void back(int d,int speed_left)
{
analogWrite(MotorL_ENB,speed_left);
digitalWrite(MotorR_I1,HIGH);
digitalWrite(MotorR_I2,LOW);
digitalWrite(MotorL_I3,HIGH);
digitalWrite(MotorL_I4,LOW);
delay(d * 100);
}
void moveForward( )
{
speed_left=(int)Output;
if((Setpoint-rpmcount)>0)
{setMode(1);
advance(0,speed_left);
}
else if(Setpoint-rpmcount==0)
{
Output=0;
setMode(0);
}
else
{ setMode(1);
back(0,speed_left);
}
}
void setMode(int Mode)
{
bool newAuto = (Mode == automatic);
if(newAuto && !inAuto)
{
Initialize();
}
inAuto = newAuto;
}
void Initialize()
{
lastInput = rpmcount;
ITerm = Output;
if(ITerm> 250) ITerm=outMax;
else if(ITerm< 0) ITerm= outMin;
}
void loop() {
moveForward();
}
void rpm_fun()
{
// look for a low-to-high on channel A
if (digitalRead(encoder0PinA) == HIGH) {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == LOW) {
rpmcount = rpmcount- 1; // CW
}
else {
rpmcount =rpmcount + 1; // CCW
}
}
else // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == HIGH) {
rpmcount = rpmcount - 1; // CW
}
else {
rpmcount = rpmcount + 1; // CCW
}
}
// Serial.println (rpmcount, DEC);
}
void rpm_fun2()
{ // look for a low-to-high on channel B
if (digitalRead(encoder0PinB) == HIGH) {
// check channel A to see which way encoder is turning
if (digitalRead(encoder0PinA) == HIGH) {
rpmcount = rpmcount-1; // CW
}
else {
rpmcount = rpmcount+1; // CCW
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinA) == LOW) {
rpmcount = rpmcount -1; // CW
}
else {
rpmcount = rpmcount + 1; // CCW
}
}
//Serial.println ( rpmcount, DEC);
}
I have a huge problem when error from 1 to -1 .
First, my motor barely can move when the PWM decrease to 45,so I set the output from 100 to 45 ,and zero when it reach error=0,but I don't know how to set the output when the error goes to negative, the
motor'speed should act like error>0 except the moving direction is reverse(but the ki term also has accumulated ), should all terms (-1)or only kp term(1-)or something else?