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.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);