419 lines
12 KiB
C
419 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 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[i] .rev ,u->param->go_param[i] .T ,u->param->go_param[i] .W ,0,u->param->go_param [i].K_P ,u->param->go_param[i] .K_W );
|
||
}
|
||
|
||
|
||
// 初始化上层状态机
|
||
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
|
||
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
|
||
u->DribbleContext .DribbleState = Dribble_PREPARE;
|
||
u->DribbleContext .is_initialized = 1;
|
||
u->DribbleContext .Dribble_run_num =0;//记录运行次数
|
||
}
|
||
|
||
if (!u->PitchContext .is_initialized) {
|
||
u->PitchContext .PitchConfig = u->param ->PitchConfig_Config ;//赋值
|
||
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
|
||
u->PitchContext .is_initialized = 1;
|
||
u->PitchContext .Pitch_run_num =0;//记录运行次数
|
||
|
||
}
|
||
|
||
|
||
}
|
||
|
||
/*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;
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
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 ;
|
||
return 0;
|
||
|
||
}
|
||
|
||
/*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[id] .rev ,u->param->go_param[id] .T ,u->param->go_param[id] .W ,AngleChange(RADIAN,pos),u->param->go_param [id].K_P ,u->param->go_param[id] .K_W );
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
|
||
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 ;
|
||
|
||
return 0;
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
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->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
|
||
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
|
||
/*运球*/
|
||
u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
|
||
u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ;
|
||
|
||
|
||
RELAY1_TOGGLE (1);//接球，1开0关
|
||
RELAY2_TOGGLE (1);//夹球，0关1开
|
||
|
||
|
||
break;
|
||
|
||
case Pitch :
|
||
Pitch_Process(u,out);
|
||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
|
||
|
||
break ;
|
||
|
||
case Dribble:
|
||
{
|
||
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
|
||
Dribble_Process(u,out);
|
||
u->DribbleContext .DribbleState =STATE_GRAB_BALL;//置标志位用于启动运球
|
||
|
||
}break ;
|
||
|
||
case PICK_Pitch_START :
|
||
|
||
Pitch_Process(u,out);
|
||
u->PitchContext .PitchState=PITCH_START;//置标志位用于启动视觉辅助投篮
|
||
|
||
break;
|
||
|
||
|
||
|
||
}
|
||
break;
|
||
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
|
||
|
||
return 0;
|
||
|
||
|
||
}
|
||
|
||
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
|
||
{
|
||
|
||
|
||
switch(u->PitchContext .PitchState){
|
||
|
||
case PITCH_START:
|
||
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
|
||
|
||
if(u->motorfeedback .GO_motor_info[0]->Pos < -35.0) //检测go位置到达最上面，这里的检测条件可以更改
|
||
u->motor_target .M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
|
||
u->PitchContext .PitchState =PITCH_PULL_TRIGGER; //更改标志位
|
||
break ;
|
||
case PITCH_PULL_TRIGGER:
|
||
if(u->motorfeedback .M2006 .total_angle <5) //当2006的总角度小于5，可以认为已经勾上
|
||
{
|
||
u->motor_target .go_shoot =u->PitchContext.PitchConfig .go1_init_position;//go下拉
|
||
u->PitchContext.PitchState =PITCH_LAUNCHING;//转移标志位
|
||
}
|
||
|
||
break ;
|
||
case PITCH_LAUNCHING:
|
||
if(u->motorfeedback .GO_motor_info [0]->Pos <2.0f)
|
||
{
|
||
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度发射
|
||
u->PitchContext .PitchState =PITCH_COMPLETE;
|
||
}
|
||
|
||
|
||
break ;
|
||
case PITCH_COMPLETE://完成运行动作后，恢复2006，-140度，go，pos0位置
|
||
u->motor_target .M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;//给-140角度
|
||
u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_init_position;
|
||
u->PitchContext .Pitch_run_num ++;//运行次数+1
|
||
|
||
break ;
|
||
|
||
|
||
|
||
}
|
||
|
||
return 0;
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
|
||
{
|
||
|
||
switch (u->DribbleContext.DribbleState) {
|
||
case STATE_GRAB_BALL://开始
|
||
|
||
RELAY2_TOGGLE (0);//夹球
|
||
|
||
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_high_angle;//3508升起
|
||
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_flip_angle;
|
||
|
||
|
||
if((u->motorfeedback .M3508 .total_angle >400)) {
|
||
RELAY1_TOGGLE (0);
|
||
|
||
|
||
if((u->motorfeedback.GO_motor_info[1]->angle) <-53){
|
||
|
||
u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
|
||
}
|
||
|
||
}
|
||
break;
|
||
|
||
|
||
case STATE_RELEASE_BALL:
|
||
RELAY2_TOGGLE (1);//松球
|
||
|
||
|
||
|
||
|
||
if((u->motorfeedback.GO_motor_info[1]->angle) <-60){
|
||
u->motor_target.go_spin =u->DribbleContext .DribbleConfig .go2_init_angle ; //恢复go2位置
|
||
u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
|
||
|
||
}
|
||
|
||
break;
|
||
|
||
case STATE_CATCH_PREP:
|
||
if((u->motorfeedback.GO_motor_info[1]->angle) > -0){
|
||
u->motor_target.M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; //当go2到初始位置，3508降
|
||
|
||
RELAY1_TOGGLE (1);//接球
|
||
}
|
||
if(u->motorfeedback .M3508 .total_angle <5){
|
||
u->DribbleContext .DribbleState = STATE_CATCH_DONE;
|
||
}
|
||
break;
|
||
|
||
case STATE_CATCH_DONE:
|
||
RELAY2_TOGGLE (1);//松球
|
||
RELAY1_TOGGLE (0);
|
||
u->DribbleContext .Dribble_run_num ++;//运行次数增加
|
||
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|