168 lines
5.2 KiB
C
168 lines
5.2 KiB
C
#include "up.h"
|
||
#include "gpio.h"
|
||
#include "user_math.h"
|
||
#include "bsp_buzzer.h"
|
||
#include "bsp_delay.h"
|
||
|
||
#define GEAR_RATIO (36) // 2006减速比
|
||
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
|
||
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO)) //2006编码值转轴角度
|
||
|
||
|
||
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
|
||
{
|
||
u->param = param; /*初始化参数 */
|
||
|
||
GO_M8010_init();
|
||
PID_init (&u->pid.VESC_5065_M1 ,PID_POSITION ,&(u->param ->VESC_5065_M1_param ));
|
||
PID_init (&u->pid.VESC_5065_M2 ,PID_POSITION ,&(u->param ->VESC_5065_M2_param ));
|
||
|
||
PID_init (&u->pid .M2006_angle ,PID_POSITION ,&(u->param->M2006_angle_param ));
|
||
PID_init (&u->pid .M2006_speed ,PID_POSITION ,&(u->param->M2006_speed_param ));
|
||
|
||
PID_init (&u->pid .GM6020_speed,PID_POSITION ,&(u->param ->UP_GM6020_speed_param ));
|
||
PID_init (&u->pid .GM6020_angle,PID_POSITION ,&(u->param ->UP_GM6020_angle_param ));
|
||
|
||
u->M2006 .motor =M2006 ;
|
||
u->M3508 .motor =M3508 ;
|
||
|
||
for(int i=0;i<3;i++){
|
||
PID_init (&u->pid .M3508_speed[i] ,PID_POSITION ,&(u->param ->M3508_speed_param ));
|
||
}
|
||
|
||
for(int i=0;i<2;i++){ //go初始位置设置为0
|
||
u->GO_motor_info[i] = getGoPoint(i);
|
||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), i,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,0,u->param->go_param .K_P ,u->param->go_param .K_W );
|
||
}
|
||
}
|
||
|
||
|
||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can) {
|
||
|
||
u->motorfeedback .M2006_angle=can ->motor .motor3508 .as_array [0].rotor_ecd ;
|
||
u->motorfeedback .M2006_rpm =can ->motor .motor3508 . as_array [0].rotor_speed ;
|
||
|
||
u->motorfeedback .VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
|
||
u->motorfeedback .VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
|
||
|
||
u->motorfeedback .rotor_pit6020ecd =can ->motor .chassis6020.as_array [2].rotor_ecd ;
|
||
u->motorfeedback .rotor_pit6020rpm =can ->motor .chassis6020.as_array [2].rotor_speed ;
|
||
for(int i=0;i<3;i++){
|
||
u->motorfeedback .M3508_speed[i] =can ->motor .motor3508 .as_array [i+1].rotor_speed ;
|
||
}
|
||
}
|
||
|
||
int8_t cnt=0;
|
||
|
||
/*上层电机控制*/
|
||
int8_t UP_angle(UP_t *u, fp32 target_angle) {
|
||
// 获取当前编码器角度
|
||
|
||
// switch (u->)
|
||
float angle = u->motorfeedback.M2006_angle;
|
||
// 初始化阶段：前50次循环记录初始值
|
||
if (u->M2006.init_cnt < 50) {
|
||
u->M2006.orig_angle = angle; // 记录初始编码器值
|
||
u->M2006.last_angle = angle;
|
||
|
||
u->M2006.init_cnt++; // 初始化计数器递增
|
||
return 0;
|
||
}
|
||
|
||
|
||
float delta = angle - u->M2006.last_angle;
|
||
if (delta > 4096) {
|
||
u->M2006.round_cnt--; // 逆时针跨圈
|
||
} else if (delta < -4096) {
|
||
u->M2006.round_cnt++; // 顺时针跨圈
|
||
}
|
||
|
||
u->M2006.last_angle = angle;
|
||
|
||
// 计算总角度
|
||
float total_angle = (u->M2006.round_cnt * 8191 + (angle - u->M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
|
||
u->M2006 .total_angle =total_angle;
|
||
|
||
float delta_angle = PID_calc(&u->pid.M2006_angle, total_angle, target_angle);
|
||
float delta_speed = PID_calc(&u->pid.M2006_speed, u->motorfeedback.M2006_rpm, delta_angle);
|
||
u->motor_target.M2006_angle = target_angle;
|
||
u->final_out .final_2006out =delta_speed;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t UP_M3508_speed(UP_t *u,fp32 speed)
|
||
{
|
||
u->motor_target .M3508_speed =speed;
|
||
for(int i=0;i<3;i++){
|
||
u->final_out .final_3508out [i] =
|
||
PID_calc (&(u->pid .M3508_speed[i] ),u->motorfeedback .M3508_speed [i],speed );
|
||
}
|
||
|
||
}
|
||
|
||
|
||
int8_t VESC_M5065_Control(UP_t *u,fp32 speed)
|
||
{
|
||
u->motor_target .VESC_5065_M1_rpm =speed;
|
||
u->motor_target .VESC_5065_M2_rpm =speed;
|
||
|
||
u->final_out .final_VESC_5065_M1out =u->motor_target .VESC_5065_M1_rpm;
|
||
u->final_out .final_VESC_5065_M2out =-u->motor_target .VESC_5065_M2_rpm;
|
||
|
||
|
||
}
|
||
|
||
int8_t GM6020_control(UP_t *u,fp32 angle)
|
||
{
|
||
fp32 delat_speed;
|
||
Clip(&angle,90,270);
|
||
|
||
delat_speed =
|
||
PID_calc (&(u->pid .GM6020_angle ),u->motorfeedback .rotor_pit6020ecd ,(angle /360*8191));
|
||
u->final_out .final_pitchout =
|
||
PID_calc (&(u->pid .GM6020_speed ),u->motorfeedback.rotor_pit6020rpm ,delat_speed);
|
||
u->motor_target .rotor_pit6020angle =angle ;
|
||
}
|
||
/*go电机控制*/
|
||
int8_t GO_SendData(UP_t *u,int id,float pos)
|
||
{
|
||
|
||
GO_M8010_send_data(BSP_UART_GetHandle(BSP_UART_RS485), id,u->param->go_param .rev ,u->param->go_param .T ,u->param->go_param .W ,AngleChange(RADIAN,pos),u->param->go_param .K_P ,u->param->go_param .K_W );
|
||
|
||
}
|
||
|
||
|
||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||
{
|
||
//电机控制 ，传进can
|
||
out ->motor3508 .as_array [0]=u->final_out .final_2006out ;
|
||
|
||
for(int i=1;i<4;i++){
|
||
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i-1] ;
|
||
}
|
||
|
||
out ->chassis5065 .erpm [0]=u->final_out .final_VESC_5065_M1out ;
|
||
out ->chassis5065 .erpm [1]=u->final_out .final_VESC_5065_M2out ;
|
||
|
||
out ->chassis6020 .as_array [2]=u->final_out .final_pitchout ;
|
||
|
||
}
|
||
|
||
int8_t UP_control(UP_t *u,CAN_Output_t *out)
|
||
{
|
||
// if(u ==NULL) return 0;
|
||
// if(u ==NULL) return 0;
|
||
// UP_M2006_angle(u,2500);
|
||
switch (u->ctrl)
|
||
{
|
||
case STATE_IDLE : break ;
|
||
case STATE_PRE_DRIBBLE : break ;
|
||
case STATE_POST_DRIBBLE : break ;
|
||
case STATE_PRE_LAUNCH : break ;
|
||
case STATE_POST_LAUNCH : break ;
|
||
}
|
||
}
|
||
|
||
|