💯 Adds initial IK and servo functionality

include/ik_task.h

@@ -0,0 +1,27 @@
+#ifndef IK_TASK_H
+#define IK_TASK_H
+
+#include <Arduino.h>
+#include <servo.h>
+#include <IK_config.h>
+#include <spot_ik.h>
+
+#define DEGREES2RAD 0.017453292519943
+
+typedef  struct {
+    float omega;
+    float phi;
+    float psi;
+    float xm;
+    float ym;
+    float zm;
+    bool set;
+} position_t;
+
+void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm);
+
+void reset_position();
+
+void task_ik(void *ignore);
+
+#endif