Hello, I've just finished coding my simple controllers and I'd like to know if there's anything I can do to improve it. I am still using arrays (because I don't want too many words e.g. previous_error, previous_output...), but inside the functions for each controller, rather than as global variables.
/***** Controllers: P, P+I, P+DFB, PID *****/
float rVel = 20.00; //reference signal, % of speed
int rPos = 150; //reference signal, degrees
// I think r would be inside the TWS variables function
int delta = 20; // Sample time, in milliseconds
float kp = 34.125;
float ki = 12.2;
float kd = 0.1;
int y_read;
float u;
int u_write;
const int velocityPin = A0;
const int positionPin = A2;
int inputPin; // I could use this to choose which pin to use as input
const int outputPin = 5;
void setup() {
pinMode(velocityPin, INPUT);
pinMode(positionPin, INPUT);
pinMode(outputPin, OUTPUT);
inputPin = velocityPin; // This would need to change
}
// Float version of map() - I don't think I will need this working with mV
//float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
//}
// Converting reference values
int rVel_mv = map(rVel, -100.00, 100.00, 0, 5000); // % to mV
int rPos_mv = map(rPos, -180, 180, 0, 5000); // degrees to mV
// Initialise reference value to be used
int r_mv;
// Controller functions
int err(int ref_mv, int output_mv) {
return ref_mv - output_mv;
}
float P(float gain, int ref_mv, int output_mv) {
int error = err(ref_mv, output_mv);
return gain*error; // = u
}
float PplusI(float Pgain, float Igain, int ref_mv, int output_mv, int SampleTime) {
float u[3]; // See below
int e[3]; // e[0] = current error, e[1] = previous error
e[0] = err(ref_mv, output_mv);
u[0] = u[1] + Pgain*e[0] - (Pgain - Igain*SampleTime)*e[1];
u[1] = u[0]; // previous u = current u
e[1] = e[0]; // previous error = current error
return u[0];
}
float PplusDFB(float Pgain, float Dgain, int ref_mv, int output_mv, int SampleTime) {
int e[3];
int y[3]; // y[0] = current output, y[1] = previous output
e[0] = err(ref_mv, output_mv);
y[0] = output_mv;
u = Pgain*e[0] - (Pgain*Dgain/SampleTime)*(y[0] - y[1]);
y[1] = y[0];
return u;
}
float PID(float Pgain, float Igain, float Dgain, int ref_mv, int output_mv, int SampleTime) {
int e[4]; // error: e[0] = e(k), e[1] = e(k-1), e[2] = e(k-2)
float u[3];
e[0] = err(ref_mv, output_mv);
u[0] = u[1] + Pgain*(e[0] - e[1]) + Igain*SampleTime*e[0] + (Dgain/SampleTime)*(e[0] - 2*e[1] + e[2]);
u[1] = u[0];
e[2] = e[1];
e[1] = e[0];
return u[0];
}
void loop() {
// Choose which variable to use as a reference value
if (inputPin == velocityPin) {
r_mv = rVel_mv;
}
else if (inputPin == positionPin) {
r_mv = rPos_mv;
}
// Read output
y_read = analogRead(inputPin);
int y_mv = map(y_read, 0, 1024, 0, 5000); // convert to mV
// Controllers: choose one
// u = P(kp, r_mv, y_mv);
// u = PplusI(kp, ki, r_mv, y_mv, delta);
// u = PDFB(kp, kd, r_mv, y_mv, delta);
// u = PID(kp, ki, kd, r_mv, y_mv, delta);
u_write = map(u, -5000, 5000, 0, 256); // convert mV to [0,255]
// Why 256? http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1266045058/22#22
// Limit output
if (u_write > 255) {
u_write = 255;
}
else if (u_write < 0) {
u_write = 0;
}
analogWrite(outputPin, u_write); // PWM output
}
+-
