You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
169 lines
3.6 KiB
169 lines
3.6 KiB
/*
|
|
* swing_action.c
|
|
*
|
|
* Created on: Mar 4, 2026
|
|
* Author: 42961
|
|
*/
|
|
#include "swing_action.h"
|
|
#include "Handset_Status_Setting.h"
|
|
|
|
#include "fsm_state.h"
|
|
#include "BHBF_ROBOT.h"
|
|
|
|
|
|
#define TT_One_Deg_Count 11014///32768*121/360(减速比121)=11014
|
|
#define LEFT_LIMIT 1
|
|
#define RIGHT_LIMIT 2
|
|
float Swing_Speed_Deg_Sencond=201.7;//HJ32-121
|
|
//int center_angle;
|
|
//int offset_angle;
|
|
//int center_position; //中间位置-944334 -1354574
|
|
int limit_record=0;
|
|
float left_compare_value=-30;
|
|
float right_compare_value=30;
|
|
float left_compare_updata;
|
|
float right_compare_updata;
|
|
|
|
int lef_positon=-569580;
|
|
int Right_positon=-1230420;
|
|
|
|
int swing_delay_count=0;
|
|
int com_delay=0;
|
|
int pos_flag=0; //控制延时停摆臂只读一次角度
|
|
int pos_direction=0; //获取延时停摆臂的方向
|
|
//手动摆臂函数
|
|
void Robot_Swing_Operation_Function()
|
|
{
|
|
com_delay=(P_MK32->CH10_LD1+1000)/2;
|
|
if(P_MK32->CH0_RY_H>500)
|
|
{
|
|
swing_delay_count++;
|
|
if(swing_delay_count>com_delay)
|
|
{
|
|
pos_flag=0;
|
|
pos_direction=1;
|
|
swing_delay_count=0;
|
|
GV.SwingMotor.Position_immediately1_Lag2=1;
|
|
GV.Tar_Position_angle=90;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
|
|
|
|
}
|
|
else if(P_MK32->CH0_RY_H<-500)
|
|
{
|
|
swing_delay_count++;
|
|
if(swing_delay_count>com_delay)
|
|
{
|
|
pos_flag=0;
|
|
pos_direction=2;
|
|
swing_delay_count=0;
|
|
GV.SwingMotor.Position_immediately1_Lag2=1;
|
|
GV.Tar_Position_angle=-90;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Move_Swing_Halt_Func_Do();
|
|
swing_delay_count=0;
|
|
pos_direction=0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//摆臂电机左转延时生效
|
|
void Move_Swing_Left_Func_Do_lay(void)
|
|
{
|
|
GV.SwingMotor.Position_immediately1_Lag2=2;
|
|
GV.Tar_Position_angle=left_compare_value;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
|
|
//摆臂电机右转延时生效
|
|
void Move_Swing_Right_Func_Do_lay(void)
|
|
{
|
|
|
|
GV.SwingMotor.Position_immediately1_Lag2=2;
|
|
GV.Tar_Position_angle=right_compare_value;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
|
|
//摆臂电机左转立即执行
|
|
void Move_Swing_Left_Func_Do_imm(void)
|
|
{
|
|
|
|
//pos_flag=0;
|
|
GV.SwingMotor.Position_immediately1_Lag2=1;
|
|
|
|
GV.Tar_Position_angle=left_compare_value;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
|
|
|
|
}
|
|
|
|
//摆臂电机右转立即执行
|
|
void Move_Swing_Right_Func_Do_imm(void)
|
|
{
|
|
|
|
//pos_flag=0;
|
|
GV.SwingMotor.Position_immediately1_Lag2=1;
|
|
GV.Tar_Position_angle=right_compare_value;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
|
|
}
|
|
|
|
int32_t Position_angle;
|
|
|
|
void Move_Swing_Halt_Func_Do(void)
|
|
{
|
|
GV.SwingMotor.Position_immediately1_Lag2=1;
|
|
if(pos_flag==0)
|
|
{
|
|
pos_flag=1;
|
|
if(pos_direction==1) //顺时针
|
|
{
|
|
//延几度再停
|
|
Position_angle= -(float)GV.SwingMotor.Real_Position/11014
|
|
+GV.Robot_Swing_Speed*0.5;
|
|
if(Position_angle>90) //别超右极限
|
|
{
|
|
Position_angle=90;
|
|
}
|
|
GV.Tar_Position_angle=Position_angle;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
else if(pos_direction==2) //逆时针
|
|
{
|
|
//延几度再停
|
|
Position_angle= -(float)GV.SwingMotor.Real_Position/11014
|
|
-GV.Robot_Swing_Speed*0.5;
|
|
if(Position_angle<-90) //别超左极限
|
|
{
|
|
Position_angle=-90;
|
|
}
|
|
GV.Tar_Position_angle=Position_angle;
|
|
GV.Tar_Position_Velcity_Degree_S=GV.Robot_Swing_Speed;
|
|
}
|
|
else
|
|
{
|
|
GV.Tar_Position_Velcity_Degree_S=0;
|
|
GV.SwingMotor.Tar_Position_count=0;
|
|
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
//GV.Tar_Position_Velcity_Degree_S=0;
|
|
//GV.SwingMotor.Tar_Position_count=0;
|
|
//GV.Tar_Position_Velcity_Degree_S=0;
|
|
}
|
|
|
|
|
|
|
|
//GV.SwingMotor.Tar_Position_count=0;
|
|
//GV.Tar_Position_Velcity_Degree_S=0;
|
|
|
|
}
|
|
|