254 lines
6.7 KiB
C
254 lines
6.7 KiB
C
#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 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; /*初始化参数 */
|
||
|
||
/*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 .Shoot_M2006angle ,PID_POSITION ,&(u->param->Shoot_M2006_angle_param ));
|
||
PID_init (&u->pid .Shoot_M2006speed ,PID_POSITION ,&(u->param->Shoot_M2006_speed_param ));
|
||
|
||
PID_init (&u->pid .Pitch_M2006_angle ,PID_POSITION ,&(u->param->Pitch_M2006_angle_param ));
|
||
PID_init (&u->pid .Pitch_M2006_speed ,PID_POSITION ,&(u->param->Pitch_M2006_speed_param ));
|
||
|
||
|
||
u->go_cmd =u->param ->go_cmd ;
|
||
|
||
// 初始化上层状态机
|
||
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
|
||
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
|
||
u->DribbleContext .DribbleState = Dribble_PREPARE;
|
||
u->DribbleContext .is_initialized = 1;
|
||
|
||
}
|
||
|
||
if (!u->PitchContext .is_initialized) {
|
||
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
|
||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||
u->PitchContext .is_initialized = 1;
|
||
}
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
|
||
|
||
}
|
||
|
||
/*can，上层状态更新*/
|
||
int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
|
||
|
||
|
||
|
||
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 ;
|
||
|
||
|
||
for(int i=0;i<4;i++){
|
||
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .motor3508 .as_array [i].rotor_speed ;
|
||
u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .motor3508 .as_array [i].rotor_ecd ;
|
||
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR2006_ECD_TO_ANGLE);
|
||
}
|
||
|
||
u->cmd =c;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle)
|
||
{
|
||
int8_t cnt=0;
|
||
fp32 angle ,delta;
|
||
|
||
angle = f->ecd;
|
||
if (f->init_cnt < 50) {
|
||
f->orig_angle= angle;
|
||
f->last_angle = angle;
|
||
f->init_cnt++;
|
||
return 0;
|
||
}
|
||
|
||
|
||
delta = angle - f->last_angle;
|
||
if (delta > 4096) {
|
||
f->round_cnt--;
|
||
} else if (delta < -4096) {
|
||
f->round_cnt++;
|
||
}
|
||
f->last_angle = angle;
|
||
f->total_angle=(f->round_cnt*8191+(angle -f->orig_angle ))*ecd_to_angle;
|
||
|
||
}
|
||
/*
|
||
这里id范围为1-4，
|
||
*/
|
||
int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle_pid,pid_type_def *Speed_pid,fp32 target_angle)
|
||
{
|
||
fp32 delta_angle,delta_speed;
|
||
delta_angle = PID_calc(Angle_pid , f->total_angle , target_angle);
|
||
delta_speed = PID_calc(Speed_pid , f->rpm , delta_angle);
|
||
|
||
u->final_out .DJfinal_out [id-1]=delta_speed;
|
||
return 0;
|
||
}
|
||
|
||
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;
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
/*go电机控制*/
|
||
|
||
int8_t GO_SendData(UP_t *u, float pos, float limit)
|
||
{
|
||
static int is_calibration=0;
|
||
static fp32 error=0; //误差量
|
||
|
||
GO_MotorData_t *GO_calibration;//校准前，原始数据
|
||
GO_calibration = get_GO_measure_point() ;
|
||
if(is_calibration==0)
|
||
{
|
||
is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
|
||
u->go_cmd.Pos = -50; //上电之后跑
|
||
error= GO_calibration->Pos ;
|
||
}
|
||
u->motorfeedback .go_data = GO_calibration;
|
||
u->motorfeedback .go_data ->Pos= error ;
|
||
u->go_cmd.Pos = pos;
|
||
|
||
// 读取参数
|
||
float tff = u->go_cmd.T; // 前馈力矩
|
||
float kp = u->go_cmd.K_P; // 位置刚度
|
||
float kd = u->go_cmd.K_W; // 速度阻尼
|
||
float q_desired = u->go_cmd.Pos; // 期望位置（rad）
|
||
float q_current = u->motorfeedback.go_data->Pos; // 当前角度位置（rad）
|
||
float dq_desired = u->go_cmd.W; // 期望角速度（rad/s）
|
||
float dq_current = u->motorfeedback.go_data->W; // 当前角速度（rad/s）
|
||
|
||
// 计算输出力矩 tau
|
||
float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
|
||
|
||
/*限制最大输入来限制最大输出*/
|
||
if (pos - q_current > limit) {
|
||
u->go_cmd.Pos = q_current + limit; // 限制位置
|
||
}else if (pos - q_current < -limit) {
|
||
u->go_cmd.Pos = q_current - limit; // 限制位置
|
||
}else {
|
||
u->go_cmd.Pos = pos; // 允许位置
|
||
}
|
||
|
||
// 发送数据
|
||
GO_M8010_send_data(&u->go_cmd);
|
||
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||
{
|
||
//电机控制 ，传进can
|
||
DJ_Angle_Control(u,1,&u->motorfeedback .DJmotor_feedback[0] ,
|
||
&u->pid .Shoot_M2006angle ,
|
||
&u->pid .Shoot_M2006speed ,
|
||
u->motor_target .Shoot_M2006_angle );
|
||
DJ_Angle_Control(u,2,&u->motorfeedback .DJmotor_feedback[1] ,
|
||
&u->pid .Pitch_M2006_angle ,
|
||
&u->pid .Pitch_M2006_speed ,
|
||
u->motor_target .Pitch_M2006_angle );
|
||
GO_SendData(u,u->motor_target .go_shoot,1 );
|
||
|
||
|
||
for(int i=0;i<4;i++){
|
||
out ->motor_UP3508.as_array[i]=u->final_out.DJfinal_out [i] ;
|
||
}
|
||
|
||
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
|
||
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
|
||
|
||
return 0;
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||
{
|
||
|
||
if(u ==NULL) return 0;
|
||
static int is_pitch=1;
|
||
switch (c->CMD_CtrlType )
|
||
{
|
||
case RCcontrol: //在手动模式下
|
||
|
||
switch (c-> CMD_mode )
|
||
{
|
||
|
||
case Normal :
|
||
/*投篮*/
|
||
if(is_pitch){
|
||
u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
|
||
is_pitch=0;
|
||
}
|
||
|
||
//
|
||
|
||
break;
|
||
|
||
case Pitch :
|
||
if (u->PitchContext .PitchState ==PITCH_PREPARE) //首次启动
|
||
{
|
||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||
}
|
||
|
||
|
||
|
||
break ;
|
||
|
||
|
||
|
||
return 0;
|
||
|
||
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|