R2_UP/User/Module/up.c
2025-03-31 16:17:10 +08:00

422 lines
11 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "up.h"
#include "gpio.h"
#include "user_math.h"
#include "bsp_buzzer.h"
#include "bsp_delay.h"
#define GEAR_RATIO_2006 (36) // 2006减速比
#define GEAR_RATIO_3508 (19)
#define CAN_MOTOR_ENC_RES 8191 // 编码器分辨率
#define MOTOR2006_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_2006)) //2006编码值转轴角度
#define MOTOR3508_ECD_TO_ANGLE (360.0 / 8191.0 / (GEAR_RATIO_3508)) //3508编码值转轴角度
/*投球*/
#define M2006_INIT_ANGLE (-120) //初始和发射
#define GO1_INIT_POSITION (0) //go初始
#define M2006_TRIGGER (0) //扳机
#define GO_POSITION_TRIGGER (-300) //go发射控制值
#define GO_POSITION_PITCH_FD (-4.8f) //反馈检测
/*运球*/
#define M3508_INIT_ANGLE (0) //3508
#define M3508_HIGH_ANGLE (1000) //3508升起角度
#define GO2_Flip_timing (200) // go的翻转时机以3508角度反馈值为准
#define GO2_Flip_ANGLE (150) //go2翻转角度
#define BALL_REL_TIME (1.76) //球放开时机以go的反馈值为准
// 定义继电器控制
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define RELAY2_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, (state) ? GPIO_PIN_SET : GPIO_PIN_RESET)
int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
{
u->param = param; /*初始化参数 */
/*go电机初始化*/
GO_M8010_init();
/*pid初始化*/
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 .M3508_angle ,PID_POSITION ,&(u->param->M3508_angle_param ));
PID_init (&u->pid .M3508_speed ,PID_POSITION ,&(u->param->M3508_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 ));
for(int i=0;i<2;i++){ //go初始位置设置为0
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 );
}
RELAY1_TOGGLE (0);
RELAY2_TOGGLE (0);
/**/
u->state .Dribble_flag =Not_started_dri;
u->state. Pitch_flag=Not_started_Pit;
u->state .last_state = Not_started_dri;
}
/*can上层状态更新*/
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
for(int i=0;i<2;i++){ //go初始位置设置为0
u->motorfeedback .GO_motor_info [i] = getGoPoint(i);
}
u->motorfeedback .M2006 .motor =M2006;
u->motorfeedback .M3508 .motor =M3508;
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<4;i++){
u->motorfeedback .M3508_rpm[i] =can ->motor .motor3508 .as_array [i].rotor_speed ;
u->motorfeedback .M3508_angle [i]=can ->motor .motor3508 .as_array [i].rotor_ecd ;
}
u->cmd =c;
return 0;
}
/*上层电机控制,使用can1的id1和2*/
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
// 获取当前编码器角度
int8_t cnt=0;
fp32 angle ,delta;
switch(motor)
{
case M2006 :
angle = u->motorfeedback.M3508_angle[0];
if (u->motorfeedback .M2006 .init_cnt < 50) {
u->motorfeedback .M2006.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M2006.last_angle = angle;
u->motorfeedback .M2006.init_cnt++; // 初始化计数器递增
return 0;
}
delta = angle - u->motorfeedback .M2006.last_angle;
delta = angle - u->motorfeedback .M2006.last_angle;
if (delta > 4096) {
u->motorfeedback .M2006.round_cnt--; // 逆时针跨圈
} else if (delta < -4096) {
u->motorfeedback .M2006.round_cnt++; // 顺时针跨圈
}
u->motorfeedback.M2006.last_angle = angle;
// 计算总角度
float total_angle = (u->motorfeedback.M2006 .round_cnt * 8191 + (angle - u->motorfeedback.M2006.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.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.M3508_rpm [0], delta_angle);
u->motor_target.M2006_angle = target_angle;
u->final_out .final_3508out [0] =delta_speed;
break ;
case M3508 :
angle = u->motorfeedback.M3508_angle[1];
if (u->motorfeedback .M3508 .init_cnt < 50) {
u->motorfeedback .M3508.orig_angle = angle; // 记录初始编码器值
u->motorfeedback .M3508.last_angle = angle;
u->motorfeedback .M3508.init_cnt++; // 初始化计数器递增
return 0;
}
delta = angle - u->motorfeedback .M3508.last_angle;
delta = angle - u->motorfeedback .M3508.last_angle;
if (delta > 4096) {
u->motorfeedback .M3508.round_cnt--; // 逆时针跨圈
} else if (delta < -4096) {
u->motorfeedback .M3508.round_cnt++; // 顺时针跨圈
}
u->motorfeedback.M3508.last_angle = angle;
// 计算总角度
total_angle = (u->motorfeedback.M3508 .round_cnt * 8191 + (angle - u->motorfeedback.M3508.orig_angle)) * MOTOR2006_ECD_TO_ANGLE;
u->motorfeedback.M3508.total_angle =total_angle;
delta_angle = PID_calc(&u->pid.M3508_angle , total_angle, target_angle);
delta_speed = PID_calc(&u->pid.M3508_speed , u->motorfeedback.M3508_rpm [1], delta_angle);
u->motor_target.M3508_angle = target_angle;
u->final_out .final_3508out[1] =delta_speed;
break ;
}
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
UP_angle_control(u,u->motor_target .M2006_angle ,M2006);
UP_angle_control(u,u->motor_target .M3508_angle ,M3508 );
GO_SendData(u,0 ,u->motor_target .go_shoot );
GO_SendData(u,1 ,u->motor_target .go_spin);
for(int i=0;i<4;i++){
out ->motor3508 .as_array[i]=u->final_out.final_3508out [i] ;
}
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,CMD_t *c)
{
if(u ==NULL) return 0;
switch (c->CMD_CtrlType )
{
case UP_RCcontrol: //在手动模式下
switch (c->CMD_UP_mode )
{
case Normal :
u->state .Pitch_flag =Not_started_Pit;
u->state .last_state = Not_started_Pit;
u->motor_target .go_shoot =0;
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
break;
case Pitch_pull :
if(u->state .last_state == Not_started_Pit)
{
<<<<<<< Updated upstream
u->motor_target .go_shoot =-2050;
u->motor_target .M2006_angle =-140;
if(u->motorfeedback .GO_motor_info[0]->Pos < (-35.20)) //到达位置后再扣扳机
=======
u->motor_target .go_shoot =GO_POSITION_TRIGGER;
if(u->motorfeedback .GO_motor_info[0]->Pos < (GO_POSITION_PITCH_FD)) //到达位置后再扣扳机
>>>>>>> Stashed changes
{
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
if(u->motorfeedback .M2006.total_angle>-5)
{//避免没勾上就拉
u->motor_target .go_shoot =GO1_INIT_POSITION;
u->state .Pitch_flag = Launch_Ready ;
u->state .last_state = Launch_Ready;
}
}
}
break ;
case Pitch_shoot :
if (u->state .last_state == Launch_Ready)
{
u->motor_target .M2006_angle =M2006_INIT_ANGLE;
u->state .Pitch_flag = Done_Pit ;
u->state .last_state = Done_Pit;
}
break;
case Dribble:
{
// u->motor_target.M3508_angle = M3508_HIGH_ANGLE;
// if(u->motorfeedback .M3508 .total_angle >GO2_Flip_timing){
// u->motor_target .go_spin = GO2_Flip_ANGLE ;
//
// if(u->motorfeedback .GO_motor_info[2]->Pos > BALL_REL_TIME )
// {
// RELAY1_TOGGLE (0);
// RELAY2_TOGGLE (1);
//
//
// }
//
// }
//
// RELAY1_TOGGLE (0);
// RELAY2_TOGGLE (0);
}
break ;
}
break;
default:
break;
}
}
int8_t Dribble_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
{
if(u ==NULL) return 0;
switch (c->CMD_UP_mode)
case Dribble:
{
RELAY1_TOGGLE (1);
RELAY2_TOGGLE (0);
u->motor_target.M3508_angle = M3508_HIGH_ANGLE;
if(u->motorfeedback .M3508 .total_angle >GO2_Flip_timing){
u->motor_target .go_spin = GO2_Flip_ANGLE ;
if((u->motorfeedback .GO_motor_info[2]->Pos) > BALL_REL_TIME )
{
RELAY1_TOGGLE (0);
osDelay (10);
RELAY2_TOGGLE (1);
}
}
}
}