Diff: PID.c

Revision:

2:fd1023193cc4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.c	Fri Dec 20 16:29:33 2019 +0000
@@ -0,0 +1,83 @@
+
+#include "PID.h"
+
+pid* pid_create(pid* pid, float* input, float* output, float* setpoint, float kp, float ki, float kd)
+{
+	pid->input = input;
+	pid->output = output;
+	pid->setpoint = setpoint;
+	pid->sumError = 0;
+	pid->lastInput = 0;
+	pid->lastError = 0;
+	pid_limits(pid,-254, 254);
+
+	pid_tune(pid, kp, ki, kd);
+
+	return pid;
+}
+
+void pid_compute(pid* pid)
+{
+
+	
+	float in = *(pid->input);
+	// Compute error
+	float error =(*(pid->setpoint)) - in;
+	// Compute integral
+	pid->sumError += (pid->Ki * error);
+	/*if ((error>0 && pid->lastError<0) || (error<0 && pid->lastError>0) ){
+		pid->sumError = 0;
+	}*/
+	if (pid->sumError > 254)
+		pid->sumError = 255;
+	else if (pid->sumError < -254)
+		pid->sumError = -255;
+	// Compute differential on input
+	float dinput = in - pid->lastInput;
+	// Compute PID output
+	float out = pid->Kp * error + pid->sumError - pid->Kd * dinput;
+	// Apply limit to output value
+	int absOut=abs((int)out);
+	if (out > pid->maxOutput){
+		out = 255;
+		//pid->sumError=0;
+	}
+	else if (out < pid->minOutput){
+		out = -255;
+		//pid->sumError=0;
+	}
+	// Output to pointed variable
+	(*pid->output) = out;
+	// Keep track of some variables for next execution
+	pid->lastInput = in;
+	pid->lastError = error;
+}
+
+void pid_tune(pid* pid, float kp, float ki, float kd)
+{
+	// Check for validity
+	pid->Kp = kp;
+	pid->Ki = ki ;
+	pid->Kd = kd ;
+
+}
+
+
+void pid_limits(pid* pid, float min, float max)
+{
+	if (min >= max) return;
+	pid->minOutput = min;
+	pid->maxOutput = max;
+	if (pid->sumError > pid->maxOutput)
+			pid->sumError = pid->maxOutput;
+	else if (pid->sumError < pid->minOutput)
+			pid->sumError = pid->minOutput;
+}
+
+
+
+
+
+
+
+