💯 Adds initial IK and servo functionality

include/IK_config.h

@@ -0,0 +1,42 @@
+#ifndef IK_CONFIG_H
+#define IK_CONFIG_H
+
+#include <Arduino.h>
+
+// SERVOS
+#define Servo_Foot     0
+#define Servo_Leg      1
+#define Servo_Shoulder 2
+
+#define LEG_LF  0 // 0, 1, 2
+#define LEG_RF  1 // 3, 4, 5
+#define LEG_LB  2 // 6, 7, 8
+#define LEG_RB  3 // 9, 10, 11
+
+#define L1  60.5  // y Distance between Shoulder Servo and Leg
+#define L2  10    // z Distance between Shoulder Servo and Leg
+#define L3  100.7 // Length of upper leg
+#define L4  118.5 // Length of lower leg
+#define L   207.5 // Distance between front and back servos
+#define W   78    // Distance between left and right shoulder
+
+// predefined calculations
+#define L1L1  3660.25  //L1*L1
+// #define L1L2  3760.25  //L1*L1+L2*L2
+// #define LL12  1210     //2*L1*L2
+#define L3L3  10140.49 //L3*L3
+#define L4L4  14042.25 //L4*L4
+#define LL34  23865.9  //2*L3*L4
+
+#define SERVO_STEP_ANGLE 2
+#define MOTION_STEP_ANGLE 5
+#define MOTION_STEP_MOVEMENT 5
+#define MOTION_STEP_ALFA 0.20
+
+extern const int16_t servo_min[12] ;
+extern const float servo_conversion[12] ;
+extern const float theta_range[3][2];
+extern const int8_t servo_invert[12];
+extern int16_t servo_angles[4][3];
+
+#endif

include/config.h

@@ -33,6 +33,7 @@
  */
 #define SCREEN_WIDTH 128
 #define SCREEN_HEIGHT 64
+#define SCREEN_RESET -1
 
 
 /*
@@ -54,4 +55,16 @@
  */
 #define USS_LEFT 12
 #define USS_RIGHT 13
-#define USS_MAX_DISTANCE 200
+#define USS_MAX_DISTANCE 200
+
+/*
+ * Button settings
+ */
+#define BUTTON 16
+#define BUTTON_LED 2
+
+/*
+ * PWM controller settings
+ */
+#define SERVO_OSCILLATOR_FREQUENCY 27000000
+#define SERVO_FREQ 50

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

include/servo.h

@@ -0,0 +1,16 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include <esp_err.h>
+
+typedef struct {
+  uint16_t pulse_0; 
+  uint16_t pulse_180; 
+  int8_t invert;
+} servo_settings_t;
+
+esp_err_t disable_servos();
+esp_err_t setup_pwm_controller();
+esp_err_t set_servo(uint8_t id, uint16_t angle);
+
+#endif

include/servo_config.h

@@ -0,0 +1,93 @@
+#ifndef SERVO_CONFIG_H
+#define SERVO_CONFIG_H
+
+//#include "servo.h"
+#include "spot_ik.h"
+
+// RF - Right Front Leg
+// lower leg
+#define RF_LOWER_SERVO_CHANNEL 8
+#define RF_LOWER_SERVO_CENTER 306
+#define RF_LOWER_SERVO_RANGE 385
+#define RF_LOWER_SERVO_DIRECTION 1
+#define RF_LOWER_SERVO_CENTER_ANG_DEG 99.86f
+// upper leg
+#define RF_UPPER_SERVO_CHANNEL 7
+#define RF_UPPER_SERVO_CENTER 306
+#define RF_UPPER_SERVO_RANGE 407
+#define RF_UPPER_SERVO_DIRECTION 1
+#define RF_UPPER_SERVO_CENTER_ANG_DEG -31.62f
+// shoulder joint
+#define RF_HIP_SERVO_CHANNEL 6
+#define RF_HIP_SERVO_CENTER 306
+#define RF_HIP_SERVO_RANGE 396
+#define RF_HIP_SERVO_DIRECTION -1
+#define RF_HIP_SERVO_CENTER_ANG_DEG 1.67f
+
+// RB - Right Back Leg
+// lower leg
+#define RB_LOWER_SERVO_CHANNEL 2
+#define RB_LOWER_SERVO_CENTER 306
+#define RB_LOWER_SERVO_RANGE 369
+#define RB_LOWER_SERVO_DIRECTION 1
+#define RB_LOWER_SERVO_CENTER_ANG_DEG 95.37f
+// upper leg
+#define RB_UPPER_SERVO_CHANNEL 1
+#define RB_UPPER_SERVO_CENTER 306
+#define RB_UPPER_SERVO_RANGE 381
+#define RB_UPPER_SERVO_DIRECTION 1
+#define RB_UPPER_SERVO_CENTER_ANG_DEG -37.21f
+// shoulder joint
+#define RB_HIP_SERVO_CHANNEL 0
+#define RB_HIP_SERVO_CENTER 306
+#define RB_HIP_SERVO_RANGE 403
+#define RB_HIP_SERVO_DIRECTION 1
+#define RB_HIP_SERVO_CENTER_ANG_DEG -3.27f
+
+// LB - Left Back Leg
+// lower leg
+#define LB_LOWER_SERVO_CHANNEL 5
+#define LB_LOWER_SERVO_CENTER 306
+#define LB_LOWER_SERVO_RANGE 374
+#define LB_LOWER_SERVO_DIRECTION 1
+#define LB_LOWER_SERVO_CENTER_ANG_DEG -92.65f
+// upper leg
+#define LB_UPPER_SERVO_CHANNEL 4
+#define LB_UPPER_SERVO_CENTER 306
+#define LB_UPPER_SERVO_RANGE 403
+#define LB_UPPER_SERVO_DIRECTION 1
+#define LB_UPPER_SERVO_CENTER_ANG_DEG 91.23f
+// shoulder joint
+#define LB_HIP_SERVO_CHANNEL 3
+#define LB_HIP_SERVO_CENTER 306
+#define LB_HIP_SERVO_RANGE 367
+#define LB_HIP_SERVO_DIRECTION -1
+#define LB_HIP_SERVO_CENTER_ANG_DEG -7.20f
+
+// Lf - Left fRONT Leg
+// lower leg
+#define LF_LOWER_SERVO_CHANNEL 11
+#define LF_LOWER_SERVO_CENTER 306
+#define LF_LOWER_SERVO_RANGE 385
+#define LF_LOWER_SERVO_DIRECTION 1
+#define LF_LOWER_SERVO_CENTER_ANG_DEG -87.43f
+// upper leg
+#define LF_UPPER_SERVO_CHANNEL 10
+#define LF_UPPER_SERVO_CENTER 306
+#define LF_UPPER_SERVO_RANGE 388
+#define LF_UPPER_SERVO_DIRECTION 1
+#define LF_UPPER_SERVO_CENTER_ANG_DEG 38.21f
+// shoulder joint
+#define LF_HIP_SERVO_CHANNEL 9
+#define LF_HIP_SERVO_CENTER 306
+#define LF_HIP_SERVO_RANGE 388
+#define LF_HIP_SERVO_DIRECTION 1
+#define LF_HIP_SERVO_CENTER_ANG_DEG 4.67f
+
+extern const int16_t servo_min[12] ;
+extern const float servo_conversion[12] ;
+extern const float theta_range[3][2];
+extern const int8_t servo_invert[12];
+extern int16_t servo_angles[4][3];
+
+#endif

include/spot_ik.h

@@ -0,0 +1,19 @@
+#ifndef SPOT_IK_H
+#define SPOT_IK_H
+
+#include "esp_err.h"
+
+typedef struct {
+    float x;
+    float y;
+    float z;
+} point;
+
+esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t servo_angles[3]);
+esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm);
+esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]);
+
+void print_matrix(float * matrix, int n, int m, char* name) ;
+void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines);
+
+#endif