429 lines
12 KiB
C
429 lines
12 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 CAN2_G3508_ID (0x200)
|
||
|
||
|
||
// 定义继电器控制
|
||
#define RELAY1_TOGGLE(state) HAL_GPIO_WritePin(GPIOE, RELAY_Pin, (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_Angle_M2006_speed ,PID_POSITION ,&(u->param->Pitch_Angle_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 ;
|
||
LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
|
||
|
||
|
||
if (!u->PitchContext .is_init) {
|
||
u->PitchContext .PitchConfig = u->param ->PitchCfg ;//赋值
|
||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
|
||
u->PitchContext .is_init = 1;
|
||
}
|
||
if(!u->PassContext.is_init) {
|
||
u->PassContext .PassCfg = u->param ->PassCfg ;//赋值
|
||
u->PassContext .PassState = PASS_STOP; //状态更新
|
||
u->PassContext .is_init = 1;
|
||
}
|
||
u->LaunchContext.LaunchCfg = u->param->LaunchCfg;
|
||
|
||
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.VESC_5065_M1_rpm =can ->motor .chassis5065 .as_array [0].rotor_speed ;
|
||
u->motorfeedback .VESC.VESC_5065_M2_rpm =can ->motor .chassis5065 .as_array [1].rotor_speed ;
|
||
|
||
u->motorfeedback .Encoder .angle = can ->Oid.angle ;
|
||
u->motorfeedback .Encoder .ecd = can ->Oid.ecd ;
|
||
|
||
for(int i=0;i<4;i++){ //范围0x201--0x204
|
||
u->motorfeedback .DJmotor_feedback[i].id =i+CAN2_G3508_ID+1;
|
||
u->motorfeedback .DJmotor_feedback[i].rpm =can ->motor .up_dribble_motor3508 .as_array [i].rotor_speed ;
|
||
u->motorfeedback .DJmotor_feedback[i].ecd =can ->motor .up_dribble_motor3508.as_array [i].rotor_ecd ;
|
||
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i], MOTOR3508_ECD_TO_ANGLE);
|
||
}
|
||
for(int i=0;i<2;i++){ //范围0x205--0x206
|
||
u->motorfeedback .DJmotor_feedback[i+4].id =i+CAN2_G3508_ID+4+1;
|
||
u->motorfeedback .DJmotor_feedback[i+4].rpm =can ->motor .up_shoot_motor3508 .as_array [i].rotor_speed ;
|
||
u->motorfeedback .DJmotor_feedback[i+4].ecd =can ->motor .up_shoot_motor3508.as_array [i].rotor_ecd ;
|
||
DJ_processdata(&u->motorfeedback .DJmotor_feedback [i+4], MOTOR2006_ECD_TO_ANGLE);
|
||
}
|
||
|
||
u->cmd =c;
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
/*
|
||
这里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-CAN2_G3508_ID-1]=delta_speed;
|
||
return 0;
|
||
}
|
||
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed)
|
||
{
|
||
|
||
fp32 delta_speed;
|
||
delta_speed = PID_calc(Speed_pid , f->rpm , target_speed);
|
||
u->final_out .DJfinal_out [id-CAN2_G3508_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;
|
||
|
||
}
|
||
|
||
|
||
|
||
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
|
||
{
|
||
//电机控制 ，传进can
|
||
DJ_Angle_Control(u,0x205,&u->motorfeedback .DJmotor_feedback[4] ,
|
||
&u->pid .Shoot_M2006angle ,
|
||
&u->pid .Shoot_M2006speed ,
|
||
u->motor_target .Shoot_M2006_angle );
|
||
|
||
/*俯仰2006，双环，内使用oid角度环，外电机速度环
|
||
|
||
*/
|
||
|
||
DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
|
||
(PID_calc (&(u->pid .Pitch_M2006_angle ),
|
||
u->motorfeedback .Encoder.angle,u->motor_target .Pitch_angle)+0.14)
|
||
);
|
||
|
||
GO_SendData(
|
||
&u->motorfeedback.go_data,
|
||
&u->go_cmd,u->motor_target .go_shoot,
|
||
u->motor_target.go_pull_speed
|
||
);
|
||
|
||
for(int i=0;i<4;i++){
|
||
out ->motor_UP3508_Dribble.as_array[i]=u->final_out.DJfinal_out [i] ;
|
||
}
|
||
for(int i=0;i<2;i++){
|
||
out ->motor_UP3508_shoot.as_array[i]=u->final_out.DJfinal_out [i+4] ;
|
||
}
|
||
out ->chassis5065 .erpm [0]= u->final_out .final_VESC_5065_M1out ;
|
||
out ->chassis5065 .erpm [1]= -u->final_out .final_VESC_5065_M2out ;
|
||
|
||
return 0;
|
||
|
||
|
||
}
|
||
|
||
|
||
fp32 posss=0;
|
||
int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||
{
|
||
|
||
if(u ==NULL) return 0;
|
||
if(out ==NULL) return 0;
|
||
if(c ==NULL) return 0;
|
||
|
||
/*指针简化*/
|
||
PitchCfg_t *pitch_cfg = &u->PitchContext.PitchConfig;
|
||
LaunchCfg_t *LaunchCfg =&u->LaunchContext.LaunchCfg;
|
||
up_motor_target_t *target=&u->motor_target ;
|
||
|
||
/*config值限位*/
|
||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.go_release_pos ,-215.0f,0.0f);
|
||
abs_limit_min_max_fp(&u->PitchContext.PitchConfig.Pitch_angle ,48.0f,67.0f);
|
||
|
||
/*部分数据更新*/
|
||
static int is_pitch=1;
|
||
posss=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||
// if (u->PitchContext.Curve == CURVE_58) {
|
||
// target->Pitch_angle = 58;
|
||
// } else {
|
||
// target->Pitch_angle = 66;
|
||
// }
|
||
|
||
switch (c->CMD_CtrlType )
|
||
{
|
||
case RCcontrol: //在手动模式下
|
||
|
||
|
||
switch (c-> CMD_mode )
|
||
{
|
||
|
||
case Normal : //复位，go位置和俯仰角保持LaunchCfg最后修改后的位置，扳机2006角度复位用于发射
|
||
/*投篮*/
|
||
if(is_pitch){
|
||
target->go_shoot = LaunchCfg->go_init ;
|
||
target->Pitch_angle = LaunchCfg->Pitch_angle;
|
||
is_pitch=0;
|
||
} //初始上电
|
||
LaunchCfg->go_up_speed=15;
|
||
target->Pitch_angle =LaunchCfg->Pitch_angle;
|
||
|
||
|
||
target->Shoot_M2006_angle=LaunchCfg->m2006_init;
|
||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新
|
||
u->LaunchContext.LaunchState = Launch_Stop;
|
||
break;
|
||
|
||
case Pitch :
|
||
if (u->PitchContext .PitchState ==PITCH_PREPARE)
|
||
{
|
||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||
}
|
||
|
||
Pitch_Process(u,out,c);
|
||
|
||
|
||
break ;
|
||
case UP_Adjust: //测试用,手动使用pitch下的cfg
|
||
LaunchCfg->go_up_speed=5;
|
||
pitch_cfg ->go_release_pos += c->Vx ;
|
||
LaunchCfg->Pitch_angle += c->Vy/100;
|
||
|
||
target->go_shoot=pitch_cfg ->go_release_pos;
|
||
target->Pitch_angle=LaunchCfg->Pitch_angle;
|
||
break ;
|
||
default:
|
||
break;
|
||
}
|
||
break;
|
||
case AUTO:
|
||
/*自动下数据更新，根据距离切换俯仰角,如需自动调整，放在*/
|
||
// if (u->PitchContext.Curve == CURVE_58) {
|
||
// target->Pitch_angle = 58;
|
||
// } else {
|
||
// target->Pitch_angle = 66;
|
||
// }
|
||
|
||
switch(c-> CMD_mode)
|
||
{
|
||
case AUTO_MID360_Pitch:
|
||
|
||
if (u->PitchContext .PitchState ==PITCH_PREPARE || u->PitchContext .PitchState ==PITCH_COMPLETE)
|
||
{
|
||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||
}
|
||
/*根据距离实时计算所需pos*/
|
||
if(c->pos){
|
||
pitch_cfg ->go_release_pos=
|
||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||
}
|
||
else pitch_cfg ->go_release_pos=0 ;
|
||
Pitch_Process(u,out,c);
|
||
break ;
|
||
|
||
case AUTO_MID360:
|
||
target->Shoot_M2006_angle =LaunchCfg ->m2006_init ;
|
||
|
||
|
||
|
||
break ;
|
||
default:
|
||
break;
|
||
|
||
}
|
||
|
||
break ;
|
||
case PASS_BALL:
|
||
// if (u->PitchContext.Curve == CURVE_58) {
|
||
// target->Pitch_angle = 58;
|
||
// } else {
|
||
// target->Pitch_angle = 66;
|
||
// }
|
||
Pass_Process(u,out,c);
|
||
break;
|
||
case RELAXED:
|
||
// target->go_shoot= pitch_cfg .
|
||
target->Pitch_angle = 58;
|
||
target->go_shoot = -210.0f;
|
||
// target->Pitch_angle = 58;
|
||
|
||
|
||
break ;
|
||
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
//复用发射，
|
||
int8_t Pitch_Launch_Sequence(UP_t *u, LaunchContext_t *LaunchContext,fp32 StartPos,fp32 EndPos,CAN_Output_t *out){
|
||
|
||
switch(LaunchContext->LaunchState){
|
||
case Launch_Stop: break;
|
||
case Launch_PREPARE:
|
||
u->motor_target.go_shoot = StartPos;
|
||
if(is_reached(u->motorfeedback.go_data.Pos,StartPos,1.0f)&&
|
||
is_reached(u->motorfeedback.go_data.W,0,1.0f)){
|
||
//根据位置和速度判断是否到达初始位置
|
||
LaunchContext->LaunchState = Launch_START;
|
||
|
||
}break;
|
||
|
||
case Launch_START:
|
||
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_up_speed;
|
||
u->motor_target.go_shoot = u->LaunchContext.LaunchCfg.go_pull_pos;
|
||
if(u->motorfeedback .go_data .Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
|
||
u->motor_target.Shoot_M2006_angle = u->LaunchContext.LaunchCfg.m2006_trig ;//设置2006角度，关闭
|
||
|
||
LaunchContext->LaunchState = Launch_TRIGGER;
|
||
}break;
|
||
|
||
case Launch_TRIGGER:
|
||
|
||
if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1){ //当2006的总角度小于1，可以认为已经勾上,误差为1
|
||
u->motor_target.go_pull_speed=LaunchContext->LaunchCfg.go_down_speed;
|
||
u->motor_target.go_shoot = EndPos ;
|
||
if(is_reached(u->motorfeedback.go_data.Pos,EndPos,1.0f))
|
||
LaunchContext->LaunchState = Launch_SHOOT_WAIT;
|
||
} break;
|
||
|
||
case Launch_SHOOT_WAIT:
|
||
if(u->motorfeedback.DJmotor_feedback[4].total_angle<-1) //认为发射
|
||
LaunchContext->LaunchState = Launch_DONE;
|
||
break;
|
||
case Launch_DONE:
|
||
break ;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
|
||
{
|
||
/*简化*/
|
||
PitchCfg_t *cfg = &u->PitchContext.PitchConfig;
|
||
PitchState_t *state =&u->PitchContext .PitchState;
|
||
up_motor_target_t *target=&u->motor_target ;
|
||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||
// 可根据实际需要传入不同的起始和末位置,起始：当前位置
|
||
Pitch_Launch_Sequence(u, LaunchContext, u->motorfeedback.go_data.Pos, cfg->go_release_pos, out);
|
||
|
||
|
||
switch(*state){
|
||
|
||
case PITCH_START:
|
||
LaunchContext->LaunchState = Launch_PREPARE;
|
||
|
||
*state=PITCH_WAIT;
|
||
break;
|
||
|
||
case PITCH_WAIT:
|
||
|
||
if(LaunchContext->LaunchState==Launch_DONE)
|
||
*state=PITCH_COMPLETE;
|
||
break;
|
||
case PITCH_COMPLETE:
|
||
|
||
break;
|
||
default:
|
||
break;
|
||
|
||
}
|
||
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
|
||
int8_t Pass_Process(UP_t *u,CAN_Output_t *out,CMD_t *c)
|
||
{
|
||
|
||
PassCfg_t *PassCfg = &u->PassContext.PassCfg;
|
||
PassState_t *state = &u->PassContext.PassState;
|
||
up_motor_target_t *target=&u->motor_target ;
|
||
LaunchContext_t *LaunchContext = &u->LaunchContext;
|
||
|
||
Pass_Sequence_Check(u,c);
|
||
|
||
if(c->pos) //
|
||
{
|
||
PassCfg ->go_release_pos =
|
||
CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos),3,4,&u->PitchContext.Curve);
|
||
|
||
}
|
||
switch (*state) { //遥控器按键进行状态切换
|
||
case PASS_STOP:
|
||
|
||
break;
|
||
case PASS_IDLE:
|
||
LaunchContext->LaunchState = Launch_PREPARE;
|
||
break;
|
||
case PASS_PREPARE:
|
||
target->go_pull_speed=PassCfg->go_up_speed;
|
||
|
||
Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,PassCfg->go_wait,out);
|
||
|
||
|
||
break;
|
||
case PASS_START:
|
||
if(LaunchContext->LaunchState == Launch_SHOOT_WAIT){
|
||
target->go_pull_speed=PassCfg->go_down_speed;
|
||
target->go_shoot = PassCfg->go_release_pos ;
|
||
}
|
||
break ;
|
||
case PASS_POS_PREPARE:
|
||
target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init;//发射
|
||
break;
|
||
case PASS_COMPLETE:
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
return 0;
|
||
}
|
||
|