Initial commit

Source/Control/Src/pid.c

@@ -0,0 +1,23 @@
+#include "pid.h"
+
+float pid_update(pid_t* pid, float error, float gyro_rate, float dt)
+{
+  float p = pid->kp * error;
+  
+  pid->integral += error * dt;
+  if (pid->integral > 100) pid->integral = 100;
+  if (pid->integral < -100) pid->integral = -100;
+  
+  float i = pid->ki * pid->integral;
+  
+  float d = - pid->kd * gyro_rate;
+  
+  pid->prev_error = error;
+  
+  float spid = p + i + d;
+  
+  if (spid > 300) spid = 300;
+  if (spid < -300) spid = -300;
+  
+  return spid;
+}