ADD: dummy foc

SIMPLE

CLEAN: cleanup code and add printf handler

FIX: counters

ADD: motor

ENH: temporary disable can

ADD: dummy foc, but creepy work

firmware/Lib/pid/pid.c

@@ -0,0 +1,82 @@
+#include "pid.h"
+#include <stdint.h>
+#include <stddef.h>
+
+#define LimitMax(input, max) \
+  {                          \
+    if (input > max)         \
+    {                        \
+      input = max;           \
+    }                        \
+    else if (input < -max)   \
+    {                        \
+      input = -max;          \
+    }                        \
+  }
+
+void pid_init(pid_type_t *pid, uint8_t mode, float Kp, float Ki, float Kd, float max_out, float max_iout)
+{
+  if (pid == NULL)
+  {
+    return;
+  }
+  pid->mode = mode;
+  pid->Kp = Kp;
+  pid->Ki = Ki;
+  pid->Kd = Kd;
+  pid->max_out = max_out;
+  pid->max_iout = max_iout;
+  pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
+  pid->error[0] = pid->error[1] = pid->error[2] = pid->Pout = pid->Iout = pid->Dout = pid->out = 0.0f;
+}
+
+float pid_calc(pid_type_t *pid, float ref, float set)
+{
+  if (pid == NULL)
+  {
+    return 0.0f;
+  }
+
+  pid->error[2] = pid->error[1];
+  pid->error[1] = pid->error[0];
+  pid->set = set;
+  pid->fdb = ref;
+  pid->error[0] = set - ref;
+  if (pid->mode == PID_POSITION)
+  {
+    pid->Pout = pid->Kp * pid->error[0];
+    pid->Iout += pid->Ki * pid->error[0];
+    pid->Dbuf[2] = pid->Dbuf[1];
+    pid->Dbuf[1] = pid->Dbuf[0];
+    pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
+    pid->Dout = pid->Kd * pid->Dbuf[0];
+    LimitMax(pid->Iout, pid->max_iout);
+    pid->out = pid->Pout + pid->Iout + pid->Dout;
+    LimitMax(pid->out, pid->max_out);
+  }
+  else if (pid->mode == PID_DELTA)
+  {
+    pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
+    pid->Iout = pid->Ki * pid->error[0];
+    pid->Dbuf[2] = pid->Dbuf[1];
+    pid->Dbuf[1] = pid->Dbuf[0];
+    pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
+    pid->Dout = pid->Kd * pid->Dbuf[0];
+    pid->out += pid->Pout + pid->Iout + pid->Dout;
+    LimitMax(pid->out, pid->max_out);
+  }
+  return pid->out;
+}
+
+void pid_clear(pid_type_t *pid)
+{
+  if (pid == NULL)
+  {
+    return;
+  }
+
+  pid->error[0] = pid->error[1] = pid->error[2] = 0.0f;
+  pid->Dbuf[0] = pid->Dbuf[1] = pid->Dbuf[2] = 0.0f;
+  pid->out = pid->Pout = pid->Iout = pid->Dout = 0.0f;
+  pid->fdb = pid->set = 0.0f;
+}

firmware/Lib/pid/pid.h

@@ -0,0 +1,34 @@
+#pragma once
+#include "stdint.h"
+
+enum PID_MODE
+{
+  PID_POSITION = 0,
+  PID_DELTA
+};
+
+typedef struct pid_type
+{
+  uint8_t mode;
+  // PID parameters
+  float Kp;
+  float Ki;
+  float Kd;
+
+  float max_out;  // max output
+  float max_iout; // max integral output
+
+  float set;
+  float fdb;
+
+  float out;
+  float Pout;
+  float Iout;
+  float Dout;
+  float Dbuf[3];  // diff buffer 0 save now 1 save last 2 save last last
+  float error[3]; // 
+
+} pid_type_t;
+extern void pid_init(pid_type_t *pid, uint8_t mode, float Kp, float Ki, float Kd, float max_out, float max_iout);
+extern float pid_calc(pid_type_t *pid, float ref, float set);
+extern void pid_clear(pid_type_t *pid);