MaRuihan
/
STM32_BHBF_SWING
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EmergencyStop

master
L1NG\42961 5 hours ago
parent
c7e009de0d
commit
2e53afede5
7 changed files with 84 additions and 15 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      Core/BASE/Protobuf/PSource/bsp_GV.pb.h
  2. 4
      Core/BASE/Protobuf/Proto/bsp_GV.proto
  3. 2
      Core/FSM/Inc/Handset_Status_Setting.h
  4. 48
      Core/FSM/Src/Handset_Status_Setting.c
  5. 13
      Core/FSM/Src/fsm_state_control.c
  6. 7
      Core/FSM/Src/robot_move_actions.c
  7. 15
      疑问记录.txt

10
Core/BASE/Protobuf/PSource/bsp_GV.pb.h
View File

@ -19,8 +19,8 @@
/* Struct definitions */
typedef struct _GV_struct_define {
int32_t TempatureE_2C; /* E_2C=0.01 Celsius 0.01摄氏度 */
int32_t Left_Compensation; /* E_2D =0.01 Degree 0.01度 */
int32_t Right_Compensation; /* 0.01度 */
float Left_Compensation; /* E_2D =0.01 Degree 0.01度 */
float Right_Compensation; /* 0.01度 */
float Robot_Angle_Desire; /* 机器人期望角度 //改为 float，单位：度（不再需要0.01缩放） */
float Robot_Move_Speed; /* 0.1rpm */
float Robot_Desired_Speed; /* 0.1rpm */
@ -80,8 +80,8 @@ extern "C" {
/* Struct field encoding specification for nanopb */
#define GV_struct_define_FIELDLIST(X, a) \
X(a, STATIC, SINGULAR, INT32, TempatureE_2C, 1) \
X(a, STATIC, SINGULAR, INT32, Left_Compensation, 2) \
X(a, STATIC, SINGULAR, INT32, Right_Compensation, 3) \
X(a, STATIC, SINGULAR, FLOAT, Left_Compensation, 2) \
X(a, STATIC, SINGULAR, FLOAT, Right_Compensation, 3) \
X(a, STATIC, SINGULAR, FLOAT, Robot_Angle_Desire, 4) \
X(a, STATIC, SINGULAR, FLOAT, Robot_Move_Speed, 5) \
X(a, STATIC, SINGULAR, FLOAT, Robot_Desired_Speed, 6) \
@ -116,7 +116,7 @@ extern const pb_msgdesc_t GV_struct_define_msg;
/* Maximum encoded size of messages (where known) */
#define BSP_GV_PB_H_MAX_SIZE GV_struct_define_size
#define GV_struct_define_size 1768
#define GV_struct_define_size 1756
#ifdef __cplusplus
} /* extern "C" */

4
Core/BASE/Protobuf/Proto/bsp_GV.proto
View File

@ -12,8 +12,8 @@ import "bsp_IO.proto";
message GV_struct_define
{
int32 TempatureE_2C=1; // E_2C=0.01 Celsius 0.01摄氏度
int32 Left_Compensation=2; // E_2D =0.01 Degree 0.01度
int32 Right_Compensation=3; //0.01度
float Left_Compensation=2; // E_2D =0.01 Degree 0.01度
float Right_Compensation=3; //0.01度
float Robot_Angle_Desire=4; //机器人期望角度 //改为 float，单位：度（不再需要0.01缩放）
float Robot_Move_Speed=5; // 0.1rpm
float Robot_Desired_Speed=6; // 0.1rpm

2
Core/FSM/Inc/Handset_Status_Setting.h
View File

@ -25,6 +25,8 @@ typedef enum {
INPUT_KEY_AUTO_WORK_UP, // CH7_SD = -1000 自动作业上
INPUT_KEY_AUTO_WORK_DOWN, // CH7_SD = 1000 自动作业下
EMERGENCE_STOP, // SE和SF同时按下触发急停
KEY_COUNT, // 输入总数
} InputEvent;

48
Core/FSM/Src/Handset_Status_Setting.c
View File

@ -147,56 +147,94 @@ static InputEvent CalculateRockerEvent(void)
return INPUT_ROCKER_STOP;
}
// 摇杆事件计算
static InputEvent Check_Emergency_Event(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
}
// ============ 各模式事件获取函数 ============
static InputEvent GetHaltModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckAllKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetManualModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetHorizontalModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetFlatModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetVerticalLeftModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetVerticalRightModeEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
InputEvent key = CheckCommonKeys();
return (key != INPUT_NONE) ? key : CalculateRockerEvent();
}
static InputEvent GetRegionalHorizontalTaskEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
return CalculateRockerEvent();
}
static InputEvent GetRegionalFlatTaskEvents(void)
{
if(P_MK32->CH8_SE==-1000&&P_MK32->CH9_SF==-1000)
{
return EMERGENCE_STOP;
}
if (P_MK32->CH7_SD == -1000) return INPUT_KEY_AUTO_WORK_UP;
return CalculateRockerEvent();
}
@ -222,9 +260,11 @@ void PV_control(void)
void IV_control(void)
{
IV.Robot_Move_Deri_Speed = 0;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Robot_Gyro = GV.TL720DParameters.RF_Angle_Roll;
IV.Is_Online = GV.P_MK32.IsOnline;
IV.SystemError = GV.SystemErrorData.Com_Error_Code;
IV.Robot_Move_Deri_Speed = GV.Left_Speed_M_min*10;
// IV.Left_Motor_Err = GV.LeftMotor.Motor_Fault;
// IV.Right_Motor_Err = GV.RightMotor.Motor_Fault;
// IV.TimeStamp = SystemTimeMiliCount;
@ -243,7 +283,7 @@ void IV_control(void)
* @param pComp 指向补偿变量的指针（int32_t*）
* @param pCnt 指向计数器的指针（int*）
*/
static void Update_Single_Compensation(int32_t chVal, int32_t* pComp, int* pCnt)
static void Update_Single_Compensation(int32_t chVal, float* pComp, int* pCnt)
{
if (chVal > COMP_DEADZONE) {
(*pCnt)++;
@ -271,8 +311,10 @@ void Compensation_Update()
static int leftCnt = 0; /* 左通道计数器 */
static int rightCnt = 0; /* 右通道计数器 */
Update_Single_Compensation(P_MK32->CH14_LT, &GV.Left_Compensation, &leftCnt);
Update_Single_Compensation(P_MK32->CH15_RT, &GV.Right_Compensation, &rightCnt);
}

13
Core/FSM/Src/fsm_state_control.c
View File

@ -38,6 +38,7 @@ static void change_Road_group(void);
static void vertical_forward_group(void); /* 虽未在表中使用，但保留 */
static void vertical_backward_group(void);
static void horizontal_auto_group(void);
static void Emergency_Stop(void);
/*=========================== 6. 函数指针表 ===========================*/
/* 动作映射表：模式 × 按键 -> 执行函数 */
@ -54,6 +55,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
[INPUT_ROCKER_TURN_RIGHT] = manual_right_group,
[INPUT_KEY_FORWARD_CRUISE] = manual_forward_group,
[INPUT_KEY_BACKWARD_CRUISE] = manual_backward_group,
[EMERGENCE_STOP] = Emergency_Stop,
},
// Horizontal_Mode - 水平模式
@ -67,6 +69,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
[INPUT_KEY_BACKWARD_CRUISE] = horizontal_backward_group,
[INPUT_KEY_AUTO_WORK_UP] = horizontal_auto_group,
[INPUT_KEY_LANE_CHANGE_UP] = change_Road_group,
[EMERGENCE_STOP] = Emergency_Stop,
// 其他按键暂未实现，留空（NULL）
},
@ -85,6 +88,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
[INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group,
[INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group,
[INPUT_KEY_LANE_CHANGE_UP] = change_Road_group,
[EMERGENCE_STOP] = Emergency_Stop,
// 其他预留
},
@ -98,6 +102,7 @@ ActionFunc actionTable[MODE_COUNT][KEY_COUNT] = {
[INPUT_KEY_FORWARD_CRUISE] = vertical_forward_group,
[INPUT_KEY_BACKWARD_CRUISE] = vertical_backward_group,
[INPUT_KEY_LANE_CHANGE_UP] = change_Road_group,
[EMERGENCE_STOP] = Emergency_Stop,
// 未实现
},
@ -178,6 +183,14 @@ static void vertical_backward_group(void)
vertical_forward();
}
static void Emergency_Stop(void)
{
GV.SwingMotor.Position_immediately1_Lag2=1;
GV.SwingMotor.Tar_Position_Velcity_Degree_S=0;
GV.Left_Speed_M_min=0;
GV.Right_Speed_M_min=0;
flag_reset();
}
/*=========================== 8. 对外接口函数（需外部调用） ===========================*/
/*-------------------------------------------------------------------

7
Core/FSM/Src/robot_move_actions.c
View File

@ -93,7 +93,7 @@ int Change_stop_flag=0;
static int S1_Last_Value;
static int S2_Last_Value;
int center_angle;
float center_angle=0;
int offset_angle;
int center_position; //中间位置-944334
@ -226,6 +226,7 @@ void Turn_Right(void)
void Robot_Stop(void)
{
Compensation_Update();
get_swing_mode();
Robot_Swing_Operation_Function();
GV.Left_Speed_M_min = 0;
@ -484,10 +485,10 @@ static void get_swing_mode(void)
{
if(GV.PV.Robot_symmetricalOrNot==1)
{
if(GV.PV.Robot_Swing_Range_Angle!=offset_angle)
if(GV.PV.Robot_Swing_Range_Angle!=offset_angle && GV.SwingMotor.Real_Position!=0)
{
offset_angle=GV.PV.Robot_Swing_Range_Angle;
center_angle=-(GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2;
center_angle=-((float)GV.SwingMotor.Real_Position+944334)/TT_One_Deg_Count2;
}

15
疑问记录.txt
View File

@ -44,8 +44,19 @@ GV_Robot_backMode=(int)CV.PV.Robot_backMode;
绑定电机 测距 调试摆臂 调内部PID
问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合）√ EPRROM的CV的配置 自动巡航√ 非对称摆臂 实现急停两个按键
问题：摆臂CAN解析√ 变量的传导√ 报错信息 焊缝跟踪的验证√ 换道精度（最小二乘拟合） EPRROM的CV的配置 自动巡航√ 非对称摆臂√ 实现急停两个按键
变量的传导 用不同的标志位
变量的传导√ 用不同的标志位√ 调整左补偿与右补偿
B03-14摆臂机器人3月13日计划完成：
1.在水平模式中，实现作业过程中的左补偿与右补偿
2.读取报错信息
3.在android软件上将能够显示的参数显示出来
4.最小二乘法拟合换道精度，之前的参数测试过之后发现不太准，今天需要再测一遍

Write Preview
Loading…
Cancel
Save