140 lines
5.0 KiB
C
140 lines
5.0 KiB
C
/*
|
|
此module是负责武林秘籍组装代码模块
|
|
*/
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include "module/assemble.h"
|
|
#include "bsp/can.h"
|
|
#include "component/pid.h"
|
|
#include "device/motor.h"
|
|
#include <stdint.h>
|
|
#include "bsp/time.h"
|
|
|
|
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/* Private macro ------------------------------------------------------------ */
|
|
|
|
/* Private variables -------------------------------------------------------- */
|
|
|
|
/* Private function prototypes ---------------------------------------------- */
|
|
|
|
/*Export function
|
|
* --------------------------------------------------------------*/
|
|
|
|
int8_t Assemble_Init(Assemble_t *a, Assemble_param_t *param,float target_freq) {
|
|
if (a == NULL || param == NULL) {
|
|
return ASSEMBLE_ERR; // 参数错误
|
|
}
|
|
|
|
a->param = param;
|
|
/*CAN初始化*/
|
|
BSP_CAN_Init();
|
|
/*注册电机*/
|
|
for (int i = 0; i < 3; i++) {
|
|
MOTOR_DM_Register(&a->param->motor_param.motor_4310_param[i]);
|
|
}
|
|
/*初始化pid*/
|
|
for (int i = 0; i < 3; i++) {
|
|
PID_Init(&a->pid.assemble_4310_Angle_pid[i], KPID_MODE_CALC_D,
|
|
target_freq,&a->param->pid_param.assemble_4310_Angle_param[i]);
|
|
PID_Init(&a->pid.assemble_4310_Omega_pid[i], KPID_MODE_CALC_D,
|
|
target_freq,&a->param->pid_param.assemble_4310_Omega_param[i]);
|
|
}
|
|
for (int i = 0; i < 3; i++) {
|
|
MOTOR_DM_Enable(&a->param->motor_param.motor_4310_param[i]);
|
|
}
|
|
return ASSEMBLE_OK;
|
|
}
|
|
|
|
int8_t Assemble_Update(Assemble_t *a) {
|
|
if (a == NULL) {
|
|
return ASSEMBLE_ERR; // 参数错误
|
|
}
|
|
|
|
/*更新所有电机数据*/
|
|
MOTOR_DM_UpdateAll();
|
|
|
|
/*更新电机反馈*/
|
|
MOTOR_DM_t *motor_4310_1 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[0]);
|
|
if (motor_4310_1 != NULL) {
|
|
a->feedback.motor_4310_angle[0] =
|
|
MOTOR_GetRotorAbsAngle(&motor_4310_1->motor);
|
|
a->feedback.motor_4310_speed[0] = MOTOR_GetRotorSpeed(&motor_4310_1->motor);
|
|
}
|
|
|
|
MOTOR_DM_t *motor_4310_2 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[1]);
|
|
if (motor_4310_2 != NULL) {
|
|
a->feedback.motor_4310_angle[1] =
|
|
MOTOR_GetRotorAbsAngle(&motor_4310_2->motor);
|
|
a->feedback.motor_4310_speed[1] = MOTOR_GetRotorSpeed(&motor_4310_2->motor);
|
|
}
|
|
|
|
MOTOR_DM_t *motor_4310_3 = MOTOR_DM_GetMotor(&a->param->motor_param.motor_4310_param[2]);
|
|
if (motor_4310_3 != NULL) {
|
|
a->feedback.motor_4310_angle[2] =
|
|
MOTOR_GetRotorAbsAngle(&motor_4310_3->motor);
|
|
a->feedback.motor_4310_speed[2] = MOTOR_GetRotorSpeed(&motor_4310_3->motor);
|
|
}
|
|
|
|
return ASSEMBLE_OK;
|
|
}
|
|
|
|
int8_t Assemble_Calc(Assemble_t *a,Assemble_CMD_t cmd_assemble)
|
|
{
|
|
if (a == NULL) {
|
|
return ASSEMBLE_ERR; // 参数错误
|
|
}
|
|
|
|
a->dt = (BSP_TIME_Get_us() - a->last_wakeup) / 1000000.0f;
|
|
a->last_wakeup = BSP_TIME_Get_us();
|
|
|
|
a->setpoint.motor_4310_setpoint[0] = cmd_assemble.setpoint.motor_4310_setpoint[0];
|
|
a->setpoint.motor_4310_setpoint[1] = cmd_assemble.setpoint.motor_4310_setpoint[1];
|
|
a->setpoint.motor_4310_setpoint[2] = cmd_assemble.setpoint.motor_4310_setpoint[2];
|
|
|
|
float angle_output_1 = PID_Calc(&a->pid.assemble_4310_Angle_pid[0],
|
|
a->setpoint.motor_4310_setpoint[0],
|
|
a->feedback.motor_4310_angle[0], 0.0f, a->dt);
|
|
a->assemble_out.motor_4310_out[0] =
|
|
PID_Calc(&a->pid.assemble_4310_Omega_pid[0], angle_output_1,
|
|
a->feedback.motor_4310_speed[0], 0.0f, a->dt);
|
|
|
|
float angle_output_2 = PID_Calc(&a->pid.assemble_4310_Angle_pid[1],
|
|
a->setpoint.motor_4310_setpoint[1],
|
|
a->feedback.motor_4310_angle[1], 0.0f, a->dt);
|
|
a->assemble_out.motor_4310_out[1] =
|
|
PID_Calc(&a->pid.assemble_4310_Omega_pid[1], angle_output_2,
|
|
a->feedback.motor_4310_speed[1], 0.0f, a->dt);
|
|
|
|
float angle_output_3 = PID_Calc(&a->pid.assemble_4310_Angle_pid[2],
|
|
a->setpoint.motor_4310_setpoint[2],
|
|
a->feedback.motor_4310_angle[2], 0.0f, a->dt);
|
|
a->assemble_out.motor_4310_out[2] =
|
|
PID_Calc(&a->pid.assemble_4310_Omega_pid[2], angle_output_3,
|
|
a->feedback.motor_4310_speed[2], 0.0f, a->dt);
|
|
|
|
return ASSEMBLE_OK;
|
|
}
|
|
|
|
int8_t Assemble_SetOutput(Assemble_t *a) {
|
|
MOTOR_MIT_Output_t MOTOR_4310_output_1 = {0};
|
|
MOTOR_4310_output_1.torque = a->assemble_out.motor_4310_out[0] * 3.0f;
|
|
MOTOR_4310_output_1.kd=0.1f;
|
|
|
|
MOTOR_MIT_Output_t MOTOR_4310_output_2 = {0};
|
|
MOTOR_4310_output_2.torque = a->assemble_out.motor_4310_out[1] * 9.0f;
|
|
MOTOR_4310_output_2.kd=0.1f;
|
|
|
|
MOTOR_MIT_Output_t MOTOR_4310_output_3 = {0};
|
|
MOTOR_4310_output_3.torque = a->assemble_out.motor_4310_out[2] * 3.0f;
|
|
MOTOR_4310_output_3.kd=0.1f;
|
|
|
|
MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[0], &MOTOR_4310_output_1);
|
|
MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[1], &MOTOR_4310_output_2);
|
|
MOTOR_DM_MITCtrl(&a->param->motor_param.motor_4310_param[2], &MOTOR_4310_output_3);
|
|
|
|
return ASSEMBLE_OK;
|
|
} |