Merge pull request #17 from RoboMaster-Club/main

dm motor driver

.vscode/c_cpp_properties.json

@@ -11,93 +11,19 @@
                 "${workspaceFolder}\\lib\\typec-board-base\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
                 "${workspaceFolder}\\lib\\typec-board-base\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
                 "${workspaceFolder}\\lib\\typec-board-base\\Drivers\\CMSIS\\Include",
+                "${workspaceFolder}\\lib\\CMSIS-DSP\\Include",
                 "${workspaceFolder}\\src\\devices\\inc",
                 "${workspaceFolder}\\src\\algo\\inc",
                 "${workspaceFolder}\\src\\bsp\\inc",
                 "${workspaceFolder}\\src\\app\\inc"
             ],
             "defines": [
-                "_DEBUG",
-                "UNICODE",
-                "_UNICODE",
                 "USE_HAL_DRIVER",
-                "STM32F407xx",
-                "__CC_ARM",
-                "__arm__",
-                "__align(x)=",
-                "__ALIGNOF__(x)=",
-                "__alignof__(x)=",
-                "__asm(x)=",
-                "__forceinline=",
-                "__restrict=",
-                "__global_reg(n)=",
-                "__inline=",
-                "__int64=long long",
-                "__INTADDR__(expr)=0",
-                "__irq=",
-                "__packed=",
-                "__pure=",
-                "__smc(n)=",
-                "__svc(n)=",
-                "__svc_indirect(n)=",
-                "__svc_indirect_r7(n)=",
-                "__value_in_regs=",
-                "__weak=",
-                "__writeonly=",
-                "__declspec(x)=",
-                "__attribute__(x)=",
-                "__nonnull__(x)=",
-                "__register=",
-                "__breakpoint(x)=",
-                "__cdp(x,y,z)=",
-                "__clrex()=",
-                "__clz(x)=0U",
-                "__current_pc()=0U",
-                "__current_sp()=0U",
-                "__disable_fiq()=",
-                "__disable_irq()=",
-                "__dmb(x)=",
-                "__dsb(x)=",
-                "__enable_fiq()=",
-                "__enable_irq()=",
-                "__fabs(x)=0.0",
-                "__fabsf(x)=0.0f",
-                "__force_loads()=",
-                "__force_stores()=",
-                "__isb(x)=",
-                "__ldrex(x)=0U",
-                "__ldrexd(x)=0U",
-                "__ldrt(x)=0U",
-                "__memory_changed()=",
-                "__nop()=",
-                "__pld(...)=",
-                "__pli(...)=",
-                "__qadd(x,y)=0",
-                "__qdbl(x)=0",
-                "__qsub(x,y)=0",
-                "__rbit(x)=0U",
-                "__rev(x)=0U",
-                "__return_address()=0U",
-                "__ror(x,y)=0U",
-                "__schedule_barrier()=",
-                "__semihost(x,y)=0",
-                "__sev()=",
-                "__sqrt(x)=0.0",
-                "__sqrtf(x)=0.0f",
-                "__ssat(x,y)=0",
-                "__strex(x,y)=0U",
-                "__strexd(x,y)=0",
-                "__strt(x,y)=",
-                "__swp(x,y)=0U",
-                "__usat(x,y)=0U",
-                "__wfe()=",
-                "__wfi()=",
-                "__yield()=",
-                "__vfp_status(x,y)=0"
+                "STM32F407xx"
             ],
             "macFrameworkPath": [
                 "/Library/Developer/CommandLineTools/SDKs/MacOSX.sdk/System/Library/Frameworks"
-            ],
+            ]
         }
     ],
     "version": 4

.vscode/launch.json

@@ -12,7 +12,7 @@
             "configFiles": [
                 "${workspaceRoot}/config/openocd_cmsis_dap.cfg" // config
             ],
-            //"runToEntryPoint": "main", // stop at main
+            "runToEntryPoint": "main", // stop at main
             "rtos": "FreeRTOS",
             "liveWatch": { 
                 "enabled": true,

src/algo/inc/leg.h

@@ -0,0 +1,44 @@
+#ifndef THIGH_H
+#define THIGH_H
+
+#include "can.h"
+#include "math.h"
+#include "pid.h"
+#include "user_math.h"
+
+typedef struct
+{
+    float phi1;
+    float phi4;
+} Leg_Info_t;
+
+typedef struct leg
+{
+    float phi1;
+    float phi2;
+    float phi3;
+    float phi4;
+
+    float phi1_dot;
+    float phi2_dot;
+    float phi3_dot;
+    float phi4_dot;
+
+    float phi0;
+    float last_phi0;
+    float phi0_dot;
+    float length;
+    float length_dot;
+
+    float xe1, xe2, ye1, ye2;
+    float torq1, torq4;
+
+    float current_disp, current_vel, current_theta, current_theta_dot, last_theta;
+    float target_leg_virtual_torq, target_wheel_torq;
+    uint32_t current_tick, last_tick;
+} Leg_t;
+
+void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi2, float phi1_dot, float phi2_dot);
+void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2);
+void Leg_VMC(Leg_t *leg, float force, float torq);
+#endif

src/algo/inc/wheel_legged_locomotion.h

@@ -0,0 +1,26 @@
+#ifndef WHEEL_LEGGED_LOCOMOTION_H
+#define WHEEL_LEGGED_LOCOMOTION_H
+
+#include <stdint.h>
+#include "arm_math.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// #define THREE_D_MODEL
+#define TWO_D_MODEL
+
+#ifdef TWO_D_MODEL
+
+typedef struct _Leg {
+    float state_vector[6];
+} Wheel_Leg_t;
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // WHEEL_LEGGED_LOCOMOTION_H

src/algo/src/leg.c

@@ -0,0 +1,100 @@
+#include "leg.h"
+#include "robot_param.h"
+
+void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi4, float phi1_dot, float phi4_dot)
+{
+    leg->current_tick = SysTick->VAL;
+    leg->phi1 = phi1;
+    leg->phi1_dot = phi1_dot;
+    leg->phi4 = phi4;
+    leg->phi4_dot = phi4_dot;
+
+    float x_B = -HALF_THIGH_DISTANCE + THIGH_LENGTH * cos(leg->phi1);
+    float y_B = THIGH_LENGTH * sin(leg->phi1);
+    float x_D = HALF_THIGH_DISTANCE + THIGH_LENGTH * cos(leg->phi4);
+    float y_D = THIGH_LENGTH * sin(leg->phi4);
+
+    float xD_minus_xB = x_D - x_B;
+    float yD_minus_yB = y_D - y_B;
+
+    float A = 2 * CALF_LENGTH * xD_minus_xB;
+    float B = 2 * CALF_LENGTH * yD_minus_yB;
+    float C = pow(xD_minus_xB, 2) + pow(yD_minus_yB, 2);
+
+    leg->phi2 = 2 * atan2(B + sqrt(pow(A, 2) + pow(B, 2) - pow(C, 2)), A + C);
+
+    float x_C = x_B + CALF_LENGTH * cos(leg->phi2);
+    float y_C = y_B + CALF_LENGTH * sin(leg->phi2);
+
+    leg->phi3 = atan2(y_C - y_D, x_C - x_D);
+
+    leg->xe1 = xD_minus_xB;
+    leg->ye1 = yD_minus_yB;
+    leg->xe2 = x_D;
+    leg->ye2 = y_D;
+    leg->length = sqrt(pow(x_C, 2) + pow(y_C, 2));
+    leg->phi0 = atan2(y_C, x_C);
+
+    leg->phi0_dot = (leg->phi0 - leg->last_phi0) / (0.004f);
+
+    leg->last_phi0 = leg->phi0;
+    leg->last_tick = leg->current_tick;
+}
+
+void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2)
+{
+    float x_toe = height / (tan(leg_angle) == 0 ? 0.000001f : tan(leg_angle));
+    float y_toe = height;
+
+    float a_1 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH;
+    float b = -2 * y_toe * THIGH_LENGTH;
+    float c_1 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2);
+
+    x_toe = -x_toe;
+    float a_2 = 2 * (-HALF_THIGH_DISTANCE - x_toe) * THIGH_LENGTH;
+    float c_2 = pow(-HALF_THIGH_DISTANCE - x_toe, 2) + pow(y_toe, 2) + pow(THIGH_LENGTH, 2) - pow(CALF_LENGTH, 2);
+
+    *leg_1 = 2 * atan2(-b + sqrt(pow(a_1, 2) + pow(b, 2) - pow(c_1, 2)), c_1 - a_1);
+    *leg_2 = -(2 * atan2(-b + sqrt(pow(a_2, 2) + pow(b, 2) - pow(c_2, 2)), c_2 - a_2) - PI);
+}
+
+void Leg_VMC(Leg_t *leg, float force, float torq)
+{
+    float leg_length = leg->length;
+    float theta = leg->phi0 - PI / 2;
+    float phi1 = leg->phi1;
+    float phi4 = leg->phi4;
+
+    float sin_phi1 = sin(phi1);
+    float cos_phi1 = cos(phi1);
+    float sin_phi4 = sin(phi4);
+    float cos_phi4 = cos(phi4);
+    float sin_theta = sin(theta);
+    float cos_theta = cos(theta);
+
+    float xC_minus_xB = (-leg_length * sin_theta) - (-HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi1);
+    float yC_minus_yB = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi1;
+    float xC_minus_xD = (-leg_length * sin_theta) - (HALF_THIGH_DISTANCE + THIGH_LENGTH * cos_phi4);
+
+    float yC_minus_yD = (leg_length * cos_theta) - THIGH_LENGTH * sin_phi4;
+
+    float M11 = ((-sin_theta * xC_minus_xB + cos_theta * yC_minus_yB) /
+                 (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB)) /
+                THIGH_LENGTH;
+    float M12 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xB - sin_theta * yC_minus_yB) /
+                                               (-sin_phi1 * xC_minus_xB + cos_phi1 * yC_minus_yB));
+    float M21 = ((-sin_theta * xC_minus_xD + cos_theta * yC_minus_yD) /
+                 (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD)) /
+                THIGH_LENGTH;
+    float M22 = (leg_length / THIGH_LENGTH) * ((-cos_theta * xC_minus_xD - sin_theta * yC_minus_yD) /
+                                               (-sin_phi4 * xC_minus_xD + cos_phi4 * yC_minus_yD));
+
+    float one_over_deter = 1 / (M11 * M22 - M12 * M21);
+    float J11 = one_over_deter * M22;
+    float J12 = -one_over_deter * M12;
+    float J21 = -one_over_deter * M21;
+    float J22 = one_over_deter * M11;
+
+    leg->torq1 = J11 * force + J21 * torq;
+    leg->torq4 = J12 * force + J22 * torq;
+}

src/algo/src/wheel_legged_locomotion.c

@@ -0,0 +1,35 @@
+#include "wheel_legged_locomotion.h"
+
+Wheel_Leg_t wheel_leg[2];
+
+void Wheel_Legged_Locomotion_Init()
+{
+    wheel_leg[0].state_vector[0] = 0.0f;
+    wheel_leg[0].state_vector[1] = 0.0f;
+    wheel_leg[0].state_vector[2] = 0.0f;
+    wheel_leg[0].state_vector[3] = 0.0f;
+    wheel_leg[0].state_vector[4] = 0.0f;
+    wheel_leg[0].state_vector[5] = 0.0f;
+
+    wheel_leg[1].state_vector[0] = 0.0f;
+    wheel_leg[1].state_vector[1] = 0.0f;
+    wheel_leg[1].state_vector[1] = 0.0f;
+    wheel_leg[1].state_vector[2] = 0.0f;
+    wheel_leg[1].state_vector[3] = 0.0f;
+    wheel_leg[1].state_vector[4] = 0.0f;
+    wheel_leg[1].state_vector[5] = 0.0f;
+}
+
+void Wheel_Legged_Update_State()
+{
+    /* Forward Kinematics */
+}
+
+void Wheel_Legged_Locomotion()
+{
+    /* Linear Quadratic Regulator */
+
+    /* Torque Mapping */
+
+    /* Output */
+}

src/app/inc/robot_param.h

@@ -14,7 +14,6 @@
 #define LEFT_REAR_OFFSET -2.11f
 #define RIGHT_FRONT_OFFSET -3.70f
 #define RIGHT_REAR_OFFSET 0.45f
-#define PI 3.14159f
 
 #define MAX_FORWARD_VEL 5.0f
 #define MAX_YAW_VEL PI

src/devices/inc/dm_motor.h

@@ -22,12 +22,14 @@
 typedef struct
 {
     uint8_t id;
+    uint8_t state;
     uint16_t pos_int;
     uint16_t vel_int;
     uint16_t torq_int;
 
-    uint8_t state;
     float pos;
+    float pos_raw;
+    float pos_offset;
     float vel;
     float torq;
     uint16_t t_mos;
@@ -37,10 +39,11 @@ typedef struct
 
 typedef struct _DM_Motor_Config {
     uint8_t can_bus;
+    uint8_t control_mode;
     uint32_t rx_id;
     uint32_t tx_id;
-    uint8_t control_mode;
-
+
+    float pos_offset;
     float kp;
     float kd;
 } DM_Motor_Config_t;
@@ -52,6 +55,7 @@ typedef struct _DM_Motor {
     uint16_t tx_id;
     uint16_t rx_id;
     CAN_Instance_t *can_instance;
+
     /* Motor Target */
     float target_pos;
     float target_vel;

src/devices/src/dm_motor.c

@@ -44,11 +44,13 @@ void DM_Motor_Decode(CAN_Instance_t *motor_can_instance)
     data_frame->pos_int = (data[1] << 8) | data[2];
     data_frame->vel_int = (data[3] << 4) | (data[4] >> 4);
     data_frame->torq_int = ((data[4] & 0xF) << 8) | data[5];
-    data_frame->pos = uint_to_float(data_frame->pos_int, P_MIN, P_MAX, 16);   // (-12.5,12.5)
+    data_frame->pos_raw = uint_to_float(data_frame->pos_int, P_MIN, P_MAX, 16);   // (-12.5,12.5)
     data_frame->vel = uint_to_float(data_frame->vel_int, V_MIN, V_MAX, 12);   // (-45.0,45.0)
     data_frame->torq = uint_to_float(data_frame->torq_int, T_MIN, T_MAX, 12); // (-18.0,18.0)
     data_frame->t_mos = (float)(data[6]);
     data_frame->t_rotor = (float)(data[7]);
+
+    data_frame->pos = data_frame->pos_raw - data_frame->pos_offset;
 }
 
 void DM_Motor_Enable_Motor(DM_Motor_t *motor)
@@ -86,7 +88,7 @@ void DM_Motor_Ctrl_MIT(DM_Motor_t *motor, float target_pos, float target_vel, fl
     uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp;
     CAN_Instance_t *motor_can_instance = motor->can_instance;
     uint8_t *data = motor_can_instance->tx_buffer;
-    motor->target_pos = target_pos;
+    motor->target_pos = target_pos + motor->stats->pos_offset;
     motor->target_vel = target_vel;
     motor->torq = torq;
     pos_temp = float_to_uint(motor->target_pos, P_MIN, P_MAX, 16);
@@ -126,6 +128,7 @@ DM_Motor_t* DM_Motor_Init(DM_Motor_Config_t *config)
     motor->kd = config->kd;
     motor->torq = 0.0f;
     motor->stats = calloc(sizeof(DM_Motor_Stats_t), 1);
+    motor->stats->pos_offset = config->pos_offset;
 
     motor->can_instance = CAN_Device_Register(motor->can_bus, motor->tx_id, motor->rx_id, DM_Motor_Decode);
     motor->can_instance->binding_motor_stats =(void*) motor->stats;