diff --git a/MDK-ARM/AUTO_CHASSIS.uvoptx b/MDK-ARM/AUTO_CHASSIS.uvoptx
index 5fb0e6c..ace4ba9 100644
--- a/MDK-ARM/AUTO_CHASSIS.uvoptx
+++ b/MDK-ARM/AUTO_CHASSIS.uvoptx
@@ -140,7 +140,7 @@
0
CMSIS_AGDI
- -X"Horco CMSIS-DAP" -U8626380832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)
+ -X"Horco CMSIS-DAP" -U4626385832 -O206 -S0 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO65554 -TC168000000 -TT168000000 -TP20 -TDS8010 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)
0
@@ -469,7 +469,7 @@
Drivers/STM32F4xx_HAL_Driver
- 0
+ 1
0
0
0
@@ -741,7 +741,7 @@
Drivers/CMSIS
- 0
+ 1
0
0
0
diff --git a/User/Algorithm/user_math.c b/User/Algorithm/user_math.c
index 6d118c6..8b7dec0 100644
--- a/User/Algorithm/user_math.c
+++ b/User/Algorithm/user_math.c
@@ -309,3 +309,16 @@ uint8_t average(uint8_t arr[], uint8_t n) {
}
return (float) sum / n;
}
+
+
+bool is_arrived(float target, float current, float mistake) {
+ // 计算当前值与目标值的差值的绝对值
+ float diff = fabs(target - current);
+
+ // 判断是否在误差范围内
+ if (diff <= mistake) {
+ return true; // 在误差范围内
+ } else {
+ return false; // 不在误差范围内
+ }
+}
\ No newline at end of file
diff --git a/User/Algorithm/user_math.h b/User/Algorithm/user_math.h
index 21cf578..e66dd23 100644
--- a/User/Algorithm/user_math.h
+++ b/User/Algorithm/user_math.h
@@ -157,4 +157,7 @@ PolarCoordinate_t addPolarVectors(PolarCoordinate_t v1, PolarCoordinate_t v2);
/// @param n 元素数
/// @return 平均值
uint8_t average(uint8_t arr[], uint8_t n);
+/*判断是否在误差内*/
+bool is_arrived(float target, float current, float mistake) ;
+
#endif
diff --git a/User/Module/config.c b/User/Module/config.c
index cf74443..4e3b965 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -7,18 +7,6 @@
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
-//#ifdef DEBUG
-
-//ConfigParam_t param_up ={
-
-//#else
-//static const ConfigParam_t param_up ={
-//#endif
-//
-//
-//
-//
-//};
#ifdef DEBUG
@@ -31,7 +19,9 @@ static const ConfigParam_t param_chassis ={
#endif
.up={
-
+
+
+ /*上层pid参数*/
.M2006_angle_param = {
.p = 25.0f,
.i = 0.0f,
@@ -60,114 +50,34 @@ static const ConfigParam_t param_chassis ={
.i_limit = 200.0f,
.out_limit = 3000.0f
},
-.M3508_speed_param={
- .p = 15.1f,
- .i = 0.02f,
- .d = 3.2f,
- .i_limit = 200.0f,
- .out_limit =6000.0f,
-},
-.go_param={
+
+.go_param[0]={
.rev = 0,
.T=0.1,
.W=0.1,
.K_P=0.1,
.K_W=0.1,
-}
+},
+.go_param[1]={
+ .rev = 0,
+ .T=0.1,
+ .W=0.1,
+ .K_P=0.10f,
+ .K_W=0.1,
+},
+/*上层其他参数*/
+ /*运球*/
+ .DribbleConfig_Config = {
+ .m3508_init_angle = 0,
+ .m3508_high_angle = 900,
+ .go2_init_angle = 35,
+ .go2_flip_angle = -130,
+ .flip_timing = 200,
+ .release_threshold = -1.2f,
+},
},
-
-// .chassis = {/**/
-// .C6020pitAngle_param = {
-// .p = 15.0f,
-// .i = 0.3f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f,
-// },
-// .C6020pitOmega_param = {
-// .p =30.0f,
-// .i =0.3f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f
-// },
-//
-// .Gimbal_yawAngle_param = {
-// .p =8.0f,
-// .i =0.0f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f
-// },
-//
-// .Gimbal_yawOmega_param = {
-// .p =18.0f,
-// .i =0.0f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f
-// },
-//
-// .Gimbal_pitchAngle_param = {
-// .p =8.0f,
-// .i =0.0f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f
-// },
-//
-// .Gimbal_pitchOmega_param = {
-// .p =18.0f,
-// .i =0.0f,
-// .d =0.0f,
-// .i_limit = 200.0f,
-// .out_limit = 3000.0f
-// },
-// .AngleCor_param = {
-// .p =0.8f,
-// .i =0.0f,
-// .d =1.0f,
-// .i_limit = 0.0f,
-// .out_limit =5000.0f,
-// },
-//
-// .OmegaCor_param = {
-// .p =23.5f,
-// .i =0.0f,
-// .d =0.05f,
-// .i_limit = 0.0f,
-// .out_limit =5000.0f,
-// },
-//
-// .ImuCor_param = {
-// .p =95.0f,
-// .i =0.0f,
-// .d =0.0f,
-// .i_limit = 0.0f,
-// .out_limit =200.0f,
-// },
-//
-// .DisCamera_param = {
-// .p =80.0f,
-// .i =0.1f,
-// .d =0.0f,
-// .i_limit = 0.0f,
-// .out_limit =5000.0f,
-// },
-
-// .M3508_param = {
-// .p = 15.1f,
-// .i = 0.02f,
-// .d = 3.2f,
-// .i_limit = 200.0f,
-// .out_limit =6000.0f,
-// }
-//
-
-// },
-
.can = {
.pitch6020 = BSP_CAN_1,
diff --git a/User/Module/up.c b/User/Module/up.c
index 9c35bbd..87ed06f 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -11,11 +11,26 @@
#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 GO2_INIT_ANGLE (0) //go2的初始角度
+#define M3508_HIGH_ANGLE (900) //3508，升起角度
+#define GO2_Flip_timing (200) // go的翻转时机，以3508角度反馈值为准
+#define GO2_Flip_ANGLE (-160) //go2翻转角度
+#define BALL_REL_TIME (-1.2) //球放开时机，以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)
{
@@ -37,14 +52,19 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
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 );
+ 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 );
}
- /**/
- u->state .Dribble_flag =Not_started_dri;
- u->state. Pitch_flag=Not_started_Pit;
- u->state .last_state = Not_started_dri;
+ // 初始化状态机
+ if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机
+ u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
+ u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球
+ u->DribbleContext .is_initialized = 1;
+ }
+
+
+
}
@@ -78,7 +98,7 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
/*上层电机控制,使用can1的id1和2*/
int8_t UP_angle_control(UP_t *u, fp32 target_angle,MotorType_t motor) {
// 获取当前编码器角度
- int8_t cnt=0;
+ int8_t cnt=0;
fp32 angle ,delta;
switch(motor)
@@ -186,7 +206,7 @@ int8_t GM6020_control(UP_t *u,fp32 angle)
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 );
+ 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 );
}
@@ -261,29 +281,40 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
case Normal :
- u->state .Pitch_flag =Not_started_Pit;
- u->state .last_state = Not_started_Pit;
+ u->Oper_control_state .Pitch_flag =Not_started_Pit;
+ u->Oper_control_state .last_state = Not_started_Pit;
u->motor_target .go_shoot =0;
- u->motor_target .M2006_angle =-140;
+ u->motor_target .M2006_angle =M2006_INIT_ANGLE;
+ u->motor_target .go_spin =u->DribbleContext.DribbleConfig .go2_init_angle ;
+ u->motor_target .M3508_angle =0;
+ u->DribbleContext .DribbleState = STATE_GRAB_BALL; //状态更新,开始夹球
+
+ RELAY1_TOGGLE (1);//接球，1开0关
+ RELAY2_TOGGLE (1);//夹球，0关1开
+
break;
case Pitch_pull :
- if(u->state .last_state == Not_started_Pit)
+ if(u->Oper_control_state .last_state == Not_started_Pit)
{
- u->motor_target .go_shoot =-300;
+
+ u->motor_target .go_shoot =-2050;
u->motor_target .M2006_angle =-140;
- if(u->motorfeedback .GO_motor_info[0]->Pos < (-4.8)) //到达位置后再扣扳机
+ 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)) //到达位置后再扣扳机
{
u->motor_target .M2006_angle =0;
if(u->motorfeedback .M2006.total_angle>-5)
{//避免没勾上就拉
- u->motor_target .go_shoot =0;
- u->state .Pitch_flag = Launch_Ready ;
- u->state .last_state = Launch_Ready;
+ u->motor_target .go_shoot =GO1_INIT_POSITION;
+ u->Oper_control_state .Pitch_flag = Launch_Ready ;
+ u->Oper_control_state .last_state = Launch_Ready;
}
@@ -292,35 +323,48 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
break ;
case Pitch_shoot :
- if (u->state .last_state == Launch_Ready)
+ if (u->Oper_control_state .last_state == Launch_Ready)
{
- u->motor_target .M2006_angle =-140;
- u->state .Pitch_flag = Done_Pit ;
- u->state .last_state = Done_Pit;
+ u->motor_target .M2006_angle =M2006_INIT_ANGLE;
+ u->Oper_control_state .Pitch_flag = Done_Pit ;
+ u->Oper_control_state .last_state = Done_Pit;
}
{
}
break;
-
-
+
+
+
+
+
+
+
+
+
+ case Dribble:
+ {
+ /*夹球 -> 3508 升起 ，同时go2翻转 -> 到位置后，继电器开，放球，同时3508降，go2翻回->接球，收 */
+ Dribble_Process(u,out);
+
+ }break ;
+
+
+
+
+
+
+
+
+
+
+
}
break;
-
- case MID_NAVI :
- {
- }
- break ;
-
- case PICK_t :
- {
- }
-
-
- break;
+
default:
break;
@@ -334,5 +378,67 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
}
+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);
+
+
+ break;
+
+ default:
+ break;
+ }
+
+ return 0;
+}
+
diff --git a/User/Module/up.h b/User/Module/up.h
index 6630a7b..7ff34bd 100644
--- a/User/Module/up.h
+++ b/User/Module/up.h
@@ -14,7 +14,38 @@
#include "GO_M8010_6_Driver.h"
#include "bsp_usart.h"
-
+/*
+ 状态机
+
+状态结构体，触发标志位结构体
+
+配置层，-->config.c
+ |
+中间层，-->up.h,统一UP_t
+ |
+运行函数，switch(状态) 运行相应函数
+
+
+
+
+
+
+
+
+
+
+
+*/
+
+
+
+
+
+
+
+
+
+
typedef enum
{
@@ -25,25 +56,42 @@ typedef enum
}Pitch_flag_t;
-typedef enum{
+/*运球*/
+typedef enum {
+ STATE_GRAB_BALL, // 夹球升起阶段
+ STATE_RELEASE_BALL, // 释放球体
+ STATE_CATCH_PREP, // 接球准备
+ STATE_CATCH_BALL, // 接球动作
+ STATE_CATCH_DONE //完成
+} DribbleState_t;
+
+/* 参数配置结构体 */
+typedef struct {
+ fp32 m3508_init_angle; // 3508初始角度
+ fp32 m3508_high_angle; // 3508升起角度
+ fp32 go2_init_angle; // GO2初始角度
+ fp32 go2_flip_angle; // GO2翻转角度
+ fp32 flip_timing; // 翻转触发时机
+ fp32 release_threshold; // 释放球
+} DribbleConfig_t;
+
+/* 状态机上下文 */
+typedef struct {
+ DribbleState_t DribbleState;
+ DribbleConfig_t DribbleConfig;
+
+ uint8_t is_initialized;
- Not_started_dri=0,//未开始
- No_ball =1, //抓上无球
- Have_ball_F=2, //刚开始有球
- Have_ball_S=3, //中途有球
- Done_dri =4 //已完成
-
-}Dribble_flag_t;
+} DribbleContext_t;
/*运行控制中的控制*/
+
typedef struct{
/*投球过程*/
Pitch_flag_t Pitch_flag;
- /*运球过程*/
- Dribble_flag_t Dribble_flag;
-
- int last_state;
+
+ char last_state;
} Oper_control_state_t;
@@ -76,8 +124,7 @@ typedef struct {
/*角度环控制电机类型*/
typedef enum {
M2006 = 1,
- M3508
-
+ M3508
} MotorType_t;
@@ -97,7 +144,9 @@ typedef struct
pid_param_t M3508_speed_param;
pid_param_t M3508_angle_param;
- GO_param_t go_param;
+ GO_param_t go_param[2];
+
+ DribbleConfig_t DribbleConfig_Config;
}UP_Param_t;
@@ -119,11 +168,22 @@ typedef struct{
uint8_t up_task_run;
const UP_Param_t *param;
+ int state;
+
+
+
+ /*运球过程*/
+ DribbleContext_t DribbleContext;
+
+
+
+
+
UP_Imu_t pos088;
/*控制及状态*/
CMD_t *cmd;
- Oper_control_state_t state;//上层机构的运行状态
+ Oper_control_state_t Oper_control_state;//上层机构的运行状态
struct{
fp32 rotor_pit6020ecd;
@@ -172,8 +232,8 @@ typedef struct{
pid_type_def M2006_angle;
pid_type_def M2006_speed;
- pid_type_def M3508_angle;
- pid_type_def M3508_speed;
+ pid_type_def M3508_angle;
+ pid_type_def M3508_speed;
}pid;
@@ -211,6 +271,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out);
int8_t UP_M2006_angle(UP_t *u,fp32 target_angle);
int8_t UP_M3508_speed(UP_t *u,fp32 speed);
+int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
#endif
diff --git a/User/device/GO_M8010_6_Driver.c b/User/device/GO_M8010_6_Driver.c
index e65f115..a4880ce 100644
--- a/User/device/GO_M8010_6_Driver.c
+++ b/User/device/GO_M8010_6_Driver.c
@@ -165,6 +165,7 @@ void GO_M8010_recv_data(uint8_t* Temp_buffer)
motor->Temp = Temp;
motor->MError = Temp_buffer[12] & 0x7;
+ motor ->angle = AngleChange(DEGREE,motor ->Pos);
}
//力位混合控制
diff --git a/User/device/GO_M8010_6_Driver.h b/User/device/GO_M8010_6_Driver.h
index b4bab93..70caadf 100644
--- a/User/device/GO_M8010_6_Driver.h
+++ b/User/device/GO_M8010_6_Driver.h
@@ -64,11 +64,13 @@ typedef struct {
float T;
float W;
float Pos;
-
+ float angle;
+
int footForce;
uint8_t buffer[17];
uint8_t Rec_buffer[16];
ControlData_t motor_send_data;
+
}GO_Motorfield;
diff --git a/User/task/up_task.c b/User/task/up_task.c
index 1fe2e44..ed1f781 100644
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@@ -68,7 +68,17 @@ void Task_up(void *argument)
//
// GO_SendData(&UP, 1,CCC);
// GO_SendData(&UP, 0,aaa);
+
UP_control(&UP,&UP_CAN_out,&up_cmd);
+// UP.motor_target .go_shoot =aaa;
+// UP.motor_target .M2006_angle =bbb ;
+
+// UP_control(&UP,&UP_CAN_out,&up_cmd);
+// UP.motor_target .M3508_angle =CCC;
+// UP.motor_target .go_shoot =aaa;
+// UP.motor_target .M2006_angle =bbb;
+// UP.motor_target .go_spin =ddd;
+
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);