58 lines
1.7 KiB
C
58 lines
1.7 KiB
C
/*
|
|
* 英雄履带控制
|
|
*/
|
|
|
|
|
|
/* Includes ----------------------------------------------------------------- */
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include "track.h"
|
|
#include "bsp/mm.h"
|
|
#include "bsp/time.h"
|
|
#include "component/filter.h"
|
|
#include "component/user_math.h"
|
|
/* Private typedef ---------------------------------------------------------- */
|
|
/* Private define ----------------------------------------------------------- */
|
|
|
|
/* Private macro ------------------------------------------------------------ */
|
|
/* Private variables -------------------------------------------------------- */
|
|
|
|
/* Private function -------------------------------------------------------- */
|
|
|
|
/* Exported functions ------------------------------------------------------- */
|
|
uint8_t TRACK_Init(Track_t *t, Track_Params_t *param, float target_freq)
|
|
{
|
|
if (t == NULL || param == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
t->param = param;
|
|
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
PID_Init(&t->pid[i], KPID_MODE_CALC_D, target_freq, ¶m->pid[i]);
|
|
}
|
|
return TRACK_OK;
|
|
}
|
|
|
|
uint8_t TRACK_Control(Track_t *t, Track_CMD_t *cmd)
|
|
{
|
|
if (t == NULL || cmd == NULL) {
|
|
return TRACK_ERR_NULL; // 参数错误
|
|
}
|
|
t->timer.now = BSP_TIME_Get_us()/1000000.0f;
|
|
t->timer.dt = t->timer.now - t->timer.lask_wakeup/1000000.0f;
|
|
t->timer.lask_wakeup = BSP_TIME_Get_us();
|
|
|
|
for (int i=0; i<TRACK_MOTOR_NUM; i++) {
|
|
t->output.iout[i] = PID_Calc(&t->pid[i], cmd->vel, t->motor.feedback.rotor_speed, 0.0f, t->timer.dt);
|
|
MOTOR_RM_SetOutput(&t->param->motor[i], t->output.iout[i]);
|
|
}
|
|
MOTOR_RM_Ctrl(&t->param->motor[0]);
|
|
return TRACK_OK;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|