LYL/R2_NEW
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

未改完

Browse Source
This commit is contained in:
ZHAISHUI04 2025-05-07 14:04:51 +08:00
parent 410d62198f
commit 52668517bd
12 changed files with 117 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/R2.uvoptx
View File

@ -205,6 +205,11 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>can_out</ItemText> <ItemText>can_out</ItemText>
</Ww> </Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>\\R2\../User/task/up_task.c\UP.motorfeedback.DJmotor_feedback</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>

BIN
MDK-ARM/R2/R2.axf
View File

Binary file not shown.

11
User/Algorithm/user_math.c
View File

@ -321,3 +321,14 @@ int abs_value(int num) {
return -num; // 如果数是负的，返回它的相反数 return -num; // 如果数是负的，返回它的相反数
} }
} }
/**
* @brief 判断是否到达目标值
* @param current 当前值
* @param target 目标值
* @param mistake 阈值
* @return 如果到达目标值返回 true，否则返回 false
*/
bool is_reached(float current, float target, float mistake) {
return fabs(current - target) < mistake;
}

2
User/Algorithm/user_math.h
View File

@ -160,4 +160,6 @@ uint8_t average(uint8_t arr[], uint8_t n);
int abs_value(int num); int abs_value(int num);
float expo_map(float value, float expo_factor) ; float expo_map(float value, float expo_factor) ;
bool is_reached(float current, float target, float mistake) ;
#endif #endif

27
User/Module/Chassis.c
View File

@ -22,7 +22,6 @@
static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){ static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */ if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/ if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
//此处源代码处做了pid的reset 待添加
c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ; c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
c->chassis_ctrl.mode =ctrl->CMD_mode ; c->chassis_ctrl.mode =ctrl->CMD_mode ;
@ -85,20 +84,15 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算 void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算
{ {
c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前 c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw + c->pos088 .bmi088.gyro.z;//左前
} }
//bool isArrive(fp32 real_pos,fp32 target_pos,fp32 mistake)
//{
// uint16_t xArrive = 0, yArrive = 0;
// xArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// yArrive = abs_float_double(real_pos,target_pos) < mistake ? 1:0;
// if(xArrive && yArrive) return true;
// else return false;
//}
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out) int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
{ {
@ -215,11 +209,12 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
//电机速度限幅 //电机速度限幅
abs_limit_fp(&c->move_vec.Vx,8000.0f); abs_limit_fp(&c->move_vec.Vx,6000.0f);
abs_limit_fp(&c->move_vec.Vy,8000.0f); abs_limit_fp(&c->move_vec.Vy,6000.0f);
abs_limit_fp(&c->move_vec.Vw,6000.0f);
abs_limit_fp(&c->move_vec.Vw,8000.0f);
Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw); Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);

15
User/Module/config.c
View File

@ -37,10 +37,10 @@ static const ConfigParam_t param ={
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_M2006_angle_param = { .Pitch_M2006_angle_param = {
.p = 25.0f, .p = 400.0f,
.i = 0.0f, .i = 0.0f,
.d = 1.5f, .d = 3.0f,
.i_limit = 1000.0f, .i_limit = 2000.0f,
.out_limit = 3000.0f, .out_limit = 3000.0f,
}, },
.Pitch_M2006_speed_param={ .Pitch_M2006_speed_param={
@ -97,11 +97,10 @@ static const ConfigParam_t param ={
/*运球*/ /*运球*/
.DribbleConfig_Config = { .DribbleConfig_Config = {
.m3508_init_angle = 50, .m3508_init_angle = 50,
.m3508_high_angle = 1200, .m3508_translate_angle = 1200,
.go2_init_angle = 0, .m3508_dribble_Reverse_speed=-3000,
.go2_flip_angle = -250, . m3508_dribble_speed= 3000, // 转动速度
.flip_timing = 200,
.go2_release_threshold = -550.0f,
}, },
/*投球*/ /*投球*/
.PitchConfig_Config = { .PitchConfig_Config = {

207
User/Module/up.c
View File

@ -36,16 +36,16 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param )); PID_init (&u->pid .Dribble_translate_M3508_angle ,PID_POSITION ,&(u->param->Dribble_translate_M3508_angle_param ));
PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param )); PID_init (&u->pid .Dribble_translate_M3508_speed ,PID_POSITION ,&(u->param->Dribble_translate_M3508_speed_param ));
for(int i=0;i<3;i++){
PID_init (&u->pid .Dribble_M3508_speed ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param )); PID_init (&u->pid .Dribble_M3508_speed[i] ,PID_POSITION ,&(u->param->Dribble_M3508_speed_param ));
}
u->go_cmd =u->param ->go_cmd ; u->go_cmd =u->param ->go_cmd ;
// 初始化上层状态机 // 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球 if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值 u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
u->DribbleContext .DribbleState = Dribble_PREPARE; u->DribbleContext .DribbleState = DRIBBLE_PREPARE;
u->DribbleContext .is_initialized = 1; u->DribbleContext .is_initialized = 1;
} }
@ -197,9 +197,9 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out) int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
{ {
//电机控制 ，传进can //电机控制 ，传进can
DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed);
DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed,u->motor_target .Dribble_M3508_speed); DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed);
DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] , DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
&u->pid .Dribble_translate_M3508_angle , &u->pid .Dribble_translate_M3508_angle ,
&u->pid .Dribble_translate_M3508_speed , &u->pid .Dribble_translate_M3508_speed ,
@ -212,8 +212,10 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
// &u->pid .Pitch_M2006_angle , // &u->pid .Pitch_M2006_angle ,
// &u->pid .Pitch_M2006_speed , // &u->pid .Pitch_M2006_speed ,
// u->motor_target .Pitch_M2006_angle ); // u->motor_target .Pitch_M2006_angle );
u->final_out .DJfinal_out [5]=PID_calc (&(u->pid .Pitch_Angle_M2006_speed ), DJ_Speed_Control(u,0x206,&u->motorfeedback .DJmotor_feedback[5],&u->pid .Pitch_M2006_speed,
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle); (PID_calc (&(u->pid .Pitch_M2006_angle ),
u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
);
GO_SendData(u,u->motor_target .go_shoot,5 ); GO_SendData(u,u->motor_target .go_shoot,5 );
@ -241,6 +243,9 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
if(u ==NULL) return 0; if(u ==NULL) return 0;
static int is_pitch=1; static int is_pitch=1;
switch (c->CMD_CtrlType ) switch (c->CMD_CtrlType )
{ {
case RCcontrol: //在手动模式下 case RCcontrol: //在手动模式下
@ -259,15 +264,14 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球 u->PitchContext .PitchState = PITCH_PREPARE; //状态更新,开始夹球
// /*运球*/ /*运球*/
// u->motor_target .go_spin = u->DribbleContext.DribbleConfig .go2_init_angle ;
// u->motor_target .M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle ; RELAY1_TOGGLE(0);//关闭气缸
// u->DribbleContext .DribbleState = Dribble_PREPARE; //重置最初状态
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
u->motor_target.Dribble_M3508_speed=0;
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//球，0接1收
//
break; break;
@ -282,29 +286,20 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ; break ;
case UP_Adjust: case UP_Adjust:
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init = c->Vy;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100; u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->motor_target .Pitch_M2006_angle=u->PitchContext.PitchConfig.m2006_Screw_init; u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
break ; break ;
case Dribble: case Dribble:
{ {
/*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
if(u->DribbleContext.DribbleState== Dribble_PREPARE){ if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
u->DribbleContext .DribbleState=DRIBBLE_PROCESS;
u->DribbleContext .DribbleState =STATE_GRAB_BALL;
} }
// Dribble_Process(u,out); Dribble_Process(u,out);
}break ; }break ;
case Dribbl_transfer:
break ;
// Dribbl_transfer_a(u,out);
}
break;
default: default:
@ -319,7 +314,7 @@ return 0;
} }
}
@ -334,10 +329,6 @@ return 0;
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c) int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
{ {
// go1_position=u->PitchContext .PitchConfig .go1_init_position ;
// M2006_Screw_position=u->PitchContext .PitchConfig .m2006_Screw_init;
switch(u->PitchContext .PitchState){ switch(u->PitchContext .PitchState){
case PITCH_START: case PITCH_START:
@ -353,19 +344,15 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
case PITCH_PULL_TRIGGER: case PITCH_PULL_TRIGGER:
if(u->motorfeedback .DJmotor_feedback [0].total_angle >-1) //当2006的总角度小于1，可以认为已经勾上,误差为1 if(is_reached ( u->motorfeedback .DJmotor_feedback [0].total_angle,u->PitchContext .PitchConfig .m2006_trigger_angle,1.0f)) //当2006的总角度小于1，可以认为已经勾上,误差为1
{ {
u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
// u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
// u->PitchContext.PitchConfig.m2006_Screw_init += c->Vy;
} }
break ; break ;
} }
return 0; return 0;
@ -376,124 +363,38 @@ return 0;
int8_t Pitch_Adjust(UP_t *u, CAN_Output_t *out,CMD_t *c)
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
{ {
u->PitchContext.PitchConfig.go1_init_position += c->Vx ; switch (u->DribbleContext.DribbleState) {
u->PitchContext.PitchConfig.Pitch_angle += c->Vy;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
case DRIBBLE_TRANSLATE:
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动
break;
case DRIBBLE_PROCESS:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
if(is_reached(u->motorfeedback .DJmotor_feedback [0].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [1].rpm,u->motor_target.Dribble_M3508_speed,70.0f)&&
is_reached(u->motorfeedback .DJmotor_feedback [2].rpm,u->motor_target.Dribble_M3508_speed,70.0f)
) {
RELAY1_TOGGLE(1); //速度达到后打开气缸
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
} }
break;
case DRIBBLE_DONE:
u->motor_target.Dribble_M3508_speed=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
break;
default:
// 处理未知状态
break;
}
return 0;
}
//int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
//{
//
// switch (u->DribbleContext.DribbleState) {
// case STATE_GRAB_BALL://开始
//
// RELAY1_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)) {
// RELAY2_TOGGLE (1);
//
//// if(((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.1)&&(u->motorfeedback.M3508 .total_angle >1190)){
////
//// u->DribbleContext .DribbleState = STATE_RELEASE_BALL; //当go2到标准位置，标志位改变
//// }
//
// }
// break;
// case STATE_RELEASE_BALL:
// RELAY1_TOGGLE (1);//松球
//
//
//
//// if((u->motorfeedback.GO_motor_info[1]->Pos ) <-1.3){
//// u->motor_target.go_spin =-40; //恢复go2位置
//// u->DribbleContext .DribbleState = STATE_CATCH_PREP; //当go2到标准位置，标志位改变
//// }
// break;
// case STATE_CATCH_PREP:
//// if((u->motorfeedback.GO_motor_info[1]->Pos )> -0.4){
//// u->motor_target.M3508_angle =0 ; //当go2到初始位置，3508降
////
//// RELAY2_TOGGLE (0);//接球
//// }
// if(u->motorfeedback .M3508 .total_angle <51.0f){
// RELAY1_TOGGLE (0);//夹球，0夹1开
// u->DribbleContext .DribbleState = STATE_TRANSFER;
// }
//
// break;
//
//
//
// break ;
// default:
// break;
// }
// return 0;
//}
//void Dribbl_transfer_a(UP_t *u, CAN_Output_t *out)
//{
// switch (u->DribbleContext.DribbleState) {
//
//
//
// case STATE_TRANSFER:
// if((u->motorfeedback.M3508 .total_angle <52.0f )) //满足这个状态时认为go和3508到达初始位置，再夹上球
// {
//
// u->motor_target .go_spin =u->DribbleContext .DribbleConfig .go2_release_threshold ;
// }
//
//// if(u->motorfeedback .GO_motor_info [1]->Pos < -4.9)
//// {
//// RELAY1_TOGGLE (1);//夹球，0夹1开
////
//// if(u->motorfeedback .GO_motor_info [1]->Pos > -4.8)
//// {
//// u->DribbleContext .DribbleState = STATE_CATCH_DONE; //当go2到标准位置，标志位改变
//// }
//// }
//
//
// break ;
// case STATE_CATCH_DONE:
//
// RELAY1_TOGGLE (1);//夹球，0夹1开
// RELAY2_TOGGLE (0);//夹球，0接1收
// u->motor_target.go_spin=u->DribbleContext .DribbleConfig.go2_init_angle ;
// break;
// break;
// }
//}

26
User/Module/up.h
View File

@ -76,23 +76,20 @@ typedef struct {
/*运球*/ /*运球*/
typedef enum { typedef enum {
Dribble_PREPARE, DRIBBLE_PREPARE, // 准备阶段
STATE_GRAB_BALL, // 夹球升起阶段 DRIBBLE_START,
STATE_RELEASE_BALL, // 释放球体 DRIBBLE_TRANSLATE, // 平移过程
STATE_CATCH_PREP, // 接球准备 DRIBBLE_PROCESS, // 运球过程
STATE_CATCH_BALL, // 接球动作 DRIBBLE_DONE // 运球结束
STATE_TRANSFER, //转移球
STATE_CATCH_DONE //完成
} DribbleState_t; } DribbleState_t;
/* 参数配置结构体 */ /* 参数配置结构体 */
typedef struct { typedef struct {
fp32 m3508_init_angle; // 3508初始角度 fp32 m3508_init_angle; // 平移前位置
fp32 m3508_high_angle; // 3508升起角度 fp32 m3508_translate_angle; // 平移后位置
fp32 go2_init_angle; // GO2初始角度 fp32 m3508_dribble_speed; // 转动速度
fp32 go2_flip_angle; // GO2翻转角度 fp32 m3508_dribble_Reverse_speed;
fp32 flip_timing; // 翻转触发时机
fp32 go2_release_threshold; // 释放球
} DribbleConfig_t; } DribbleConfig_t;
/* 状态机上下文 */ /* 状态机上下文 */
@ -211,7 +208,7 @@ typedef struct{
pid_type_def Pitch_Angle_M2006_speed; pid_type_def Pitch_Angle_M2006_speed;
pid_type_def Dribble_M3508_speed;//三摩擦用的速度环 pid_type_def Dribble_M3508_speed[3];//三摩擦用的速度环
pid_type_def Dribble_translate_M3508_speed;//平移用的速度环 pid_type_def Dribble_translate_M3508_speed;//平移用的速度环
pid_type_def Dribble_translate_M3508_angle;//平移用的角度环 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
@ -252,6 +249,7 @@ int8_t DJ_processdata(DJmotor_feedback_t *f,fp32 ecd_to_angle);
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); 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);
int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed); int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c); int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);

2
User/device/can_use.c
View File

@ -76,7 +76,7 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
{ {
case 0x01: case 0x01:
feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24; feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
feedback->angle=feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f; feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
break; break;
} }
} }

3
User/device/cmd.c
View File

@ -196,8 +196,9 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
} }
else if(cmd ->CMD_CtrlType ==RCcontrol){ else if(cmd ->CMD_CtrlType ==RCcontrol){
/*手动下的*/ /*手动下的*/
if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode =UP_Adjust; if(rc->LD.key_G ==CMD_SW_DOWN ) cmd ->CMD_mode=Dribble ;
else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch; else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
else if(rc->LD .key_H==CMD_SW_UP) cmd ->CMD_mode =UP_Adjust;
else cmd ->CMD_mode =Normal; else cmd ->CMD_mode =Normal;
} }
} }

6
User/task/error_detect_task.c
View File

@ -96,10 +96,10 @@ void Task_error_detect(void *argument){
//若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转 //若所有设备在线，会不断翻转绿灯 存在设备离线则会红灯灯翻转
if(online_dev == ERROR_LIST_LENGHT) // if(online_dev == ERROR_LIST_LENGHT)
{ // {
HAL_GPIO_TogglePin(GPIOH,LED_G_Pin); HAL_GPIO_TogglePin(GPIOH,LED_G_Pin);
} // }
// else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin); // else HAL_GPIO_TogglePin(GPIOH,LED_R_Pin);
for (int i = 0; i < ERROR_LIST_LENGHT; i++) for (int i = 0; i < ERROR_LIST_LENGHT; i++)