SkyLiCoder
ed6d509c72
- add DM_Feedforward_Init_Config_s with Kp and dir fields - replace plain DM_Feedforward_Kp in DM_Motor_Init_Config_s - apply direction flag in DMMotorControl to negate feedforward when reverse - set yaw feedforward: Kp=0.9, dir=REVERSE
122 lines
4.1 KiB
C
122 lines
4.1 KiB
C
#ifndef DMMOTOR_H
|
|
#define DMMOTOR_H
|
|
|
|
#include <stdint.h>
|
|
#include "bsp_can.h"
|
|
#include "controller.h"
|
|
#include "motor_def.h"
|
|
#include "daemon.h"
|
|
|
|
#define DM_MOTOR_CNT 4
|
|
|
|
/* DM motor parameter ranges (from reference DM_6220 driver) */
|
|
#define DM_P_MIN (-12.5f)
|
|
#define DM_P_MAX 12.5f
|
|
#define DM_V_MIN (-45.0f)
|
|
#define DM_V_MAX 45.0f
|
|
#define DM_T_MIN (-10.0f)
|
|
#define DM_T_MAX 10.0f
|
|
#define DM_KP_MIN 0.0f
|
|
#define DM_KP_MAX 500.0f
|
|
#define DM_KD_MIN 0.0f
|
|
#define DM_KD_MAX 5.0f
|
|
|
|
/* DM motor control modes */
|
|
/* DM motor command codes */
|
|
typedef enum
|
|
{
|
|
DM_CMD_ENABLE_MODE = 0xFC, // enable motor
|
|
DM_CMD_DISABLE_MODE = 0xFD, // disable motor
|
|
DM_CMD_ZERO_POSITION = 0xFE, // set current position as encoder zero
|
|
DM_CMD_CLEAR_ERROR = 0xFB, // clear motor error
|
|
} DM_Motor_Mode_e;
|
|
|
|
/* DM motor control modes */
|
|
typedef enum
|
|
{
|
|
DM_MODE_MIT = 0, // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
|
|
DM_MODE_POS_VEL = 1, // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
|
|
DM_MODE_VELOCITY = 2, // Velocity mode: CAN ID + 0x200, float velocity only
|
|
} DM_Motor_Ctrl_Mode_e;
|
|
|
|
/* DM motor measurement data (parsed from feedback CAN frame) */
|
|
typedef struct
|
|
{
|
|
uint8_t id; // motor ID from feedback byte[0:3]
|
|
uint8_t state; // motor state from feedback byte[7:4]: 0=reset, 1=run
|
|
float position; // current position (rad, range ±12.5)
|
|
float velocity; // current velocity (rad/s, range ±45)
|
|
float torque; // current torque (N·m, range ±10)
|
|
float T_Mos; // MOS temperature (Celsius)
|
|
float T_Rotor; // rotor temperature (Celsius)
|
|
float last_position; // previous position for multi-turn calculation
|
|
int32_t total_round; // accumulated full revolutions
|
|
float total_angle; // multi-turn angle (degrees)
|
|
} DM_Motor_Measure_s;
|
|
|
|
typedef struct{
|
|
float Kp;
|
|
Output_Reverse_Flag_e dir;
|
|
float feed_input;
|
|
} DM_Feedforward_Controller_s;
|
|
|
|
/* DM motor instance (runtime state) */
|
|
typedef struct
|
|
{
|
|
DM_Motor_Measure_s measure;
|
|
Motor_Controller_s motor_controller;
|
|
DM_Feedforward_Controller_s feedforward_controller;
|
|
|
|
CANInstance *motor_can_instance; // CAN communication instance
|
|
DaemonInstance *motor_daemon; // daemon watchdog
|
|
Output_Reverse_Flag_e output_reverse; // output reverse flag
|
|
DM_Motor_Ctrl_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
|
|
DM_Motor_Mode_e motor_mode; // last command mode (enable/disable/zero/clear error)
|
|
float mit_kp; // MIT mode KP (motor internal position gain, 0~500)
|
|
float mit_kd; // MIT mode KD (motor internal velocity gain, 0~5)
|
|
|
|
} DMMotorInstance;
|
|
|
|
/* Feedforward controller init config */
|
|
typedef struct
|
|
{
|
|
float Kp;
|
|
Output_Reverse_Flag_e dir;
|
|
} DM_Feedforward_Init_Config_s;
|
|
|
|
/* DM motor init config */
|
|
typedef struct
|
|
{
|
|
Motor_Init_Config_s motor_config; // generic motor PID and setting config
|
|
DM_Motor_Ctrl_Mode_e ctrl_mode; // default control mode
|
|
DM_Feedforward_Init_Config_s feedforward_config;
|
|
float mit_kp; // MIT mode KP (only for MIT mode)
|
|
float mit_kd; // MIT mode KD (only for MIT mode)
|
|
} DM_Motor_Init_Config_s;
|
|
|
|
/* ---- API ---- */
|
|
|
|
DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
|
|
|
|
void DMMotorSetRef(DMMotorInstance *motor, float ref);
|
|
void DMMotorSetMode(DM_Motor_Mode_e mode, DMMotorInstance *motor);
|
|
|
|
/**
|
|
* @brief Main control function called from Motor Task at 1kHz.
|
|
* Iterates all registered motors, runs PID cascade, sends CAN frames.
|
|
*/
|
|
void DMMotorControl(void);
|
|
|
|
/* ---- Low-level CAN send functions (all three modes) ---- */
|
|
|
|
/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */
|
|
void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq);
|
|
|
|
/* Position-Velocity mode: send float position + float velocity */
|
|
void DMMotorSendPosVel(CANInstance *can, float pos, float vel);
|
|
|
|
/* Velocity mode: send float velocity only */
|
|
void DMMotorSendVelocity(CANInstance *can, float vel);
|
|
|
|
#endif // DMMOTOR_H
|