I am trying to use the PID library to apply feedback control to a motor (with encoder). I am setting an exponential velocity curve for my motor. However, when I attempted to use PID, I am getting a strange square output from the function (see plot below). It will output a single value for a time, then go abruptly to zero. Currently, kp = 0.5, ki = 0, kd = 0.1. Same issue if kd = 0.
(Disregard time in the plot, I did not calculate it for this visual)
#include <Encoder.h> //include encoder library
#include <SoftwareSerial.h> //include serial library
#include <avr-libc.h> //include arduino trig and exponential functions
#include <PID_v1.h> //include PID library
/*
Encoder myEnc(2, 3); //setting up interrupt pins 2 & 3 for encoder
SoftwareSerial mySerial(7,8); //setting up serial pins 7 & 8 for Jrk communication
//VARIABLES FOR ENCODER
long oldPosition = -999;
long newtime;
long oldtime;
double enc_vel;
//VARIABLES FOR Jrk COMMUNICATION
int MyTarget = 0; //target position, 0-4095 is the range of the JRK controller
double lstart = 0.005; // starting distance of platforms (m)
double lfinal = lstart; //lfinal used for "while" loop
double t=0; //time for exponential rate
double edot = 3.3; //shear rate
double Rpin = 0.012; //pinion radius (m)
double req_vel;
double new_vel;
double kp = 0.5;
double ki = 0;
double kd = 0.1;
long starttime;
//SPECIFY LINKS AND INITIAL TUNING PARAMETERS FOR PID
//PID(&Input,&Output,&Setpoint, Kp, Ki, Kd, Direction)
PID myPID(&enc_vel, &new_vel, &req_vel, kp, ki, kd, DIRECT);
//sets the motor speed for the JRK21V3 controller, this uses pololu high resulution protocol
void Move(int x) {
word target = x; //only pass this ints
mySerial.write(0xAA); //Pololu protocol: tells the controller we're starting to send it commands
mySerial.write(0x0B); //This is the pololu device # in config utility (converted to hex). Typically #11
mySerial.write(0x40 + (target & 0x1F)); //first half of the target (High resolution)
mySerial.write((target >> 5) & 0x7F); //second half of the target (High resolution)
}
void setup() {
mySerial.begin(9600);//setting initial baud rate
Serial.begin(9600);
Serial.println("Basic Motor Drive Test");
Serial.print("Req vel");
Serial.print("\t");
Serial.print("Enc vel");
Serial.print("\t");
Serial.println("New vel");
myPID.SetMode(AUTOMATIC); //turning PID on
starttime=millis();
oldtime = starttime;
}
void loop() {
while (lfinal <= .165) {
newtime=millis(); //finds current time for loop
req_vel= lstart*edot*exp(edot*(double(newtime-starttime)/1000)); //calculating plate velocity required (m/s)
long newPosition = myEnc.read(); //reading current encoder position
if (newPosition == 0) {
enc_vel = 0; }
else {
enc_vel = ((newPosition-oldPosition)*5.236*Rpin)/(newtime-oldtime);} //finding current velocity (m/s) (based on 1200 ct/rev)
oldPosition = newPosition;
myPID.Compute(); //compute PID
MyTarget = int(round (1998 - (815.287 * new_vel))); //finding value to send to Jrk controller (btwn 1486 (max) and 1998 (no speed))
Move(MyTarget);
lfinal = lfinal+ req_vel*(double(millis()-oldtime)/1000); //gives us total length plate will travel with this new rp
delay (10); //delay program 50 ms
Serial.print(req_vel,3);
Serial.print("\t");
Serial.print(enc_vel,3);
Serial.print("\t");
Serial.println(new_vel,3);
oldtime = newtime;
}
Move (2048); // keep motor stationary
}
