末端三轴独立解算姿态 自定义控制器初步测试
This commit is contained in:
parent
9fc944012e
commit
2f654113c6
|
|
@ -60,6 +60,7 @@ static Robot_Status_e robot_state; // 机器人整体工作状态
|
|||
static referee_info_t *referee_data; // 用于获取裁判系统的数据
|
||||
|
||||
VT_info_t *referee_VT_data; //图传链路数据
|
||||
custom_control_t CustomControl; //自定义控制器数据
|
||||
|
||||
void RobotCMDInit() {
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
|
|
@ -194,6 +195,15 @@ static void RemoteControlSet() {
|
|||
gimbal_cmd_send.x_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r_;
|
||||
gimbal_cmd_send.y_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r1;
|
||||
gimbal_cmd_send.z_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_l1;
|
||||
|
||||
//自定义控制器控制末端姿态
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
|
||||
}
|
||||
|
||||
gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1;
|
||||
gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_;
|
||||
|
||||
}
|
||||
}
|
||||
// 云台软件限位
|
||||
|
|
@ -281,6 +291,9 @@ void RobotCMDTask() {
|
|||
#ifdef GIMBAL_BOARD
|
||||
chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
|
||||
#endif // GIMBAL_BOARD
|
||||
//获取自定义控制器数据
|
||||
memcpy(&CustomControl,&referee_VT_data->CustomControl,sizeof(custom_control_t));
|
||||
|
||||
SubGetMessage(to_stretch_feed_sub, &to_stretch_fetch_data);
|
||||
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
|
||||
update_ui_data();
|
||||
|
|
|
|||
|
|
@ -2,6 +2,16 @@
|
|||
#define ROBOT_CMD_H
|
||||
|
||||
|
||||
#pragma pack(1)
|
||||
typedef struct {
|
||||
float dx;
|
||||
float dy;
|
||||
float dz;
|
||||
|
||||
int16_t q[4]; //旋转四元数
|
||||
}custom_control_t;
|
||||
#pragma pack()
|
||||
//自定义控制器数据
|
||||
/**
|
||||
* @brief 机器人核心控制任务初始化,会被RobotInit()调用
|
||||
*
|
||||
|
|
|
|||
|
|
@ -58,12 +58,16 @@ Matrixf<4, 4> T_cmd; //拟运动学 期望末端变换矩阵
|
|||
Matrixf<5, 2> ik_q; //逆运动学 关节位置
|
||||
Matrixf<5, 1> ik_q_cmd;//逆运动学 期望关节位置
|
||||
Matrixf<3, 1> cmd_xyz; //逆运动学 末端期望位置
|
||||
const float l1 = 0.151 ,l3 = 0.350, l4 = 0.139;
|
||||
Matrixf<3, 3> cmd_R; //逆运动学 末端期望姿态
|
||||
Matrixf<4, 1> cmd_quat; //逆运动学 末端期望姿态
|
||||
Matrixf<3, 2> ik_q3; //逆运动学 球形手腕关节位置
|
||||
Matrixf<3, 1> ik_q3_cmd;//逆运动学 球形手腕期望关节位置
|
||||
const float l1 = 0.151 ,l3 = 0.350, l4 = 0;//0.139;
|
||||
void Arm_Init()
|
||||
{
|
||||
link[0] = robotics::Link(0,l1,0,PI/2);
|
||||
link[1] = robotics::Link(0,0,0,PI/2); link[1].offset_ = PI/2;
|
||||
link[2] = robotics::Link(0,l3,0,-PI/2);
|
||||
link[1] = robotics::Link(0,0,l3,PI/2);
|
||||
link[2] = robotics::Link(0,0,0,-PI/2);
|
||||
link[3] = robotics::Link(0,0,0,PI/2);
|
||||
link[4] = robotics::Link(0,l4,0,0);
|
||||
|
||||
|
|
@ -323,7 +327,7 @@ void GimbalTask()
|
|||
|
||||
//正运动学
|
||||
arm_q[0][0] = yaw_motor->measure.position;
|
||||
arm_q[1][0] = - pitch_motor_l->measure.position;
|
||||
arm_q[1][0] = -pitch_motor_l->measure.position;
|
||||
arm_q[2][0] = roll_motor->measure.position;
|
||||
arm_q[3][0] = diff_pitch_angle;
|
||||
arm_q[4][0] = diff_roll_angle;
|
||||
|
|
@ -336,6 +340,8 @@ void GimbalTask()
|
|||
//
|
||||
// ik_q_cmd = ik_q.block<5,1>(0,0);
|
||||
|
||||
// ik_q3 = robotics::spherical_wrist_ikine(robotics::t2r(fk_T),arm_q[0][0],arm_q[1][0]);
|
||||
|
||||
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
|
||||
{
|
||||
DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r);
|
||||
|
|
@ -375,63 +381,63 @@ void GimbalTask()
|
|||
if(last_gimbal_mode == GIMBAL_GYRO_MODE)
|
||||
{
|
||||
//切换至逆解模式时，目标值设置为当前值
|
||||
cmd_xyz = fk_p;
|
||||
cmd_R = robotics::t2r( fk_T);
|
||||
}
|
||||
cmd_xyz[0][0] += gimbal_cmd_recv.x_add;
|
||||
cmd_xyz[1][0] += gimbal_cmd_recv.y_add;
|
||||
cmd_xyz[2][0] += gimbal_cmd_recv.z_add;
|
||||
T_cmd = robotics::p2t(cmd_xyz);
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
|
||||
}
|
||||
float trans_rpy_data[3] = {-PI,0,0};
|
||||
Matrixf<3,1> trans_rpy(trans_rpy_data);
|
||||
cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态
|
||||
// T_cmd = robotics::p2t(cmd_xyz);
|
||||
|
||||
ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);
|
||||
|
||||
if(engineer_arm.ikine_analytic_check(T_cmd))
|
||||
{
|
||||
ik_q = engineer_arm.ikine_analytic(T_cmd);
|
||||
//后三关节的误差 选用误差小的一组解
|
||||
// float err[2] = {0};
|
||||
// Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
|
||||
// for (int i = 0; i < 2; ++i) {
|
||||
// Matrixf<3,1> ik_q3 = ik_q.block<3,1>(2,i);
|
||||
// err[i] = (ik_q3 - arm_q3).norm();
|
||||
// }
|
||||
|
||||
//if (err[1] >= err[0])
|
||||
//选用 roll角度小的一组解
|
||||
if (abs(ik_q[2][0]) <= abs(ik_q[2][1]))
|
||||
ik_q_cmd = ik_q.block<5,1>(0,0);
|
||||
else
|
||||
ik_q_cmd = ik_q.block<5,1>(0,1);
|
||||
|
||||
ik_q_cmd[0][0] = float_constrain(ik_q_cmd[0][0],-YAW * DEGREE_2_RAD,YAW* DEGREE_2_RAD);
|
||||
ik_q_cmd[1][0] = - ik_q_cmd[1][0];
|
||||
ik_q_cmd[1][0] = float_constrain(ik_q_cmd[1][0],PITCH_MIN * DEGREE_2_RAD,PITCH_MAX* DEGREE_2_RAD);
|
||||
|
||||
ik_q_cmd[2][0] = float_constrain(ik_q_cmd[2][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
|
||||
|
||||
//ik_q_cmd[3][0] = float_constrain(ik_q_cmd[3][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
|
||||
ik_q_cmd[4][0] = float_constrain(ik_q_cmd[4][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
|
||||
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
q_set[i] = ik_q_cmd[i][0];
|
||||
float err[2] = {0};
|
||||
Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
|
||||
err[i] = (ik - arm_q3).norm();
|
||||
}
|
||||
|
||||
if (err[1] >= err[0])
|
||||
ik_q3_cmd = ik_q3.block<3,1>(0,0);
|
||||
else
|
||||
ik_q3_cmd = ik_q3.block<3,1>(0,1);
|
||||
|
||||
// //选用 roll角度小的一组解
|
||||
// if (abs(ik_q3[0][0]) <= abs(ik_q3[0][1]))
|
||||
//
|
||||
// else
|
||||
// ik_q3_cmd = ik_q3.block<3,1>(0,1);
|
||||
|
||||
ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
|
||||
|
||||
//ik_q3_cmd[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
|
||||
ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
q_set[i] = ik_q3_cmd[i][0];
|
||||
}
|
||||
}
|
||||
else
|
||||
LOGWARNING("Arm can not approach pose!");
|
||||
|
||||
|
||||
|
||||
float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[3]);
|
||||
float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[4]);
|
||||
float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, q_set[1]);
|
||||
float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, q_set[2]);
|
||||
|
||||
float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out;
|
||||
float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out;
|
||||
|
||||
//pitch轴双环PID
|
||||
float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,q_set[1]);
|
||||
//pitch轴双环PID pitch yaw先遥控器控制
|
||||
float pitch_angle_out = PIDCalculate(&pitch_angle_loop,pitch_motor_l->measure.position,gimbal_cmd_recv.pitch * DEGREE_2_RAD);
|
||||
float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward;
|
||||
|
||||
DMMotorSetRef(pitch_motor_l,pitch_spd_out / 2);
|
||||
DMMotorSetRef(pitch_motor_r, -pitch_spd_out / 2);
|
||||
DMMotorSetRef(yaw_motor,q_set[0]);
|
||||
DMMotorSetRef(roll_motor,q_set[2]);
|
||||
DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
|
||||
|
||||
DMMotorSetRef(roll_motor,q_set[0]);
|
||||
|
||||
DMMotorSetRef(diff_r_motor,r_speed_set);
|
||||
DMMotorSetRef(diff_l_motor,-l_speed_set);
|
||||
|
|
|
|||
|
|
@ -107,6 +107,11 @@ typedef enum
|
|||
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
|
||||
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
|
||||
GIMBAL_IKINE_MODE, // 机械臂运动学逆解模式，输入6d-tof位置
|
||||
GIMBAL_SILVER_MODE, // 取银矿模式
|
||||
GIMBAL_GOLD_MODE, // 取金矿模式
|
||||
GIMBAL_STORAGE_MODE, // 放入矿石仓
|
||||
GIMBAL_FETCH_MODE, // 取出矿石
|
||||
|
||||
ADJUST_PITCH_MODE, //找pitch的限位
|
||||
} gimbal_mode_e;
|
||||
|
||||
|
|
@ -152,6 +157,8 @@ typedef struct
|
|||
float y_add; //末端目标位置增量
|
||||
float z_add;
|
||||
|
||||
float quat[4]; //末端姿态四元数
|
||||
|
||||
float chassis_rotate_wz;
|
||||
uint8_t MotorEnableFlag;
|
||||
gimbal_mode_e gimbal_mode;
|
||||
|
|
|
|||
|
|
@ -2,35 +2,38 @@
|
|||
|
||||
|
||||
Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:276:5, State=BP_STATE_DISABLED
|
||||
GraphedExpression="(gimbal_cmd_recv).pitch", Color=#e56a6f
|
||||
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=48
|
||||
GraphedExpression="(gimbal_cmd_recv).pitch", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f
|
||||
GraphedExpression="(referee_VT_data)->CmdID", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b
|
||||
OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59
|
||||
OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.h", Line=0
|
||||
OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107
|
||||
OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69
|
||||
OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=255
|
||||
OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3637
|
||||
OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29
|
||||
OpenDocument="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107
|
||||
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=51
|
||||
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=48
|
||||
OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315
|
||||
OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1255
|
||||
OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=0
|
||||
OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412
|
||||
OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=191, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=531, h=177, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=204, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=531, h=191, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=401, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x30A3D70C
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=450, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=437, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=1041, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=265, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=531, h=464, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="315;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="329;0"
|
||||
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=531, h=450, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="312;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=210, h=282, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[263;100;100;100;889]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (gimbal_cmd_recv).pitch"], ColWidths=[100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;100;100;100;100;100;100;107;107]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (gimbal_cmd_recv).pitch";" (referee_VT_data)->CmdID"], ColWidths=[100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[438;126;100;100;100;100;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;108;100]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;100;100]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;351]
|
||||
WatchedExpression="arm_q.data_", RefreshRate=2, Window=Watched Data 1
|
||||
|
|
|
|||
|
|
@ -23,6 +23,46 @@
|
|||
#define user_malloc malloc
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief 斜波函数初始化
|
||||
* @author RM
|
||||
* @param[in] 斜波函数结构体
|
||||
* @param[in] 间隔的时间，单位 s
|
||||
* @param[in] 最大值
|
||||
* @param[in] 最小值
|
||||
* @retval 返回空
|
||||
*/
|
||||
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
|
||||
{
|
||||
ramp_source_type->frame_period = frame_period;
|
||||
ramp_source_type->max_value = max;
|
||||
ramp_source_type->min_value = min;
|
||||
ramp_source_type->input = 0.0f;
|
||||
ramp_source_type->out = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
|
||||
* @author RM
|
||||
* @param[in] 斜波函数结构体
|
||||
* @param[in] 输入值
|
||||
* @param[in] 滤波参数
|
||||
* @retval 返回空
|
||||
*/
|
||||
void ramp_calc(ramp_function_source_t *ramp_source_type, float input)
|
||||
{
|
||||
ramp_source_type->input = input;
|
||||
ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
|
||||
if (ramp_source_type->out > ramp_source_type->max_value)
|
||||
{
|
||||
ramp_source_type->out = ramp_source_type->max_value;
|
||||
}
|
||||
else if (ramp_source_type->out < ramp_source_type->min_value)
|
||||
{
|
||||
ramp_source_type->out = ramp_source_type->min_value;
|
||||
}
|
||||
}
|
||||
|
||||
void *zmalloc(size_t size)
|
||||
{
|
||||
void *ptr = malloc(size);
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
|
|||
#ifndef PI
|
||||
#define PI 3.14159265354f
|
||||
#endif
|
||||
typedef __packed struct
|
||||
typedef struct
|
||||
{
|
||||
float input; //输入数据
|
||||
float out; //滤波输出的数据
|
||||
|
|
@ -61,6 +61,15 @@ typedef __packed struct
|
|||
float frame_period; //滤波的时间间隔 单位 s
|
||||
} first_order_filter_type_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float input; //输入数据
|
||||
float out; //输出数据
|
||||
float min_value; //限幅最小值
|
||||
float max_value; //限幅最大值
|
||||
float frame_period; //时间间隔
|
||||
} ramp_function_source_t;
|
||||
|
||||
#define VAL_LIMIT(val, min, max) \
|
||||
do \
|
||||
{ \
|
||||
|
|
|
|||
|
|
@ -0,0 +1,31 @@
|
|||
//
|
||||
// Created by SJQ on 2024/5/11.
|
||||
//
|
||||
|
||||
|
||||
|
||||
#include "air_pump.h"
|
||||
#include "stdlib.h"
|
||||
#include "memory.h"
|
||||
|
||||
|
||||
// 通过此函数注册一个舵机
|
||||
PumpInstance *PumpInit(TIM_HandleTypeDef *htim,uint32_t Channel) {
|
||||
PumpInstance *pump = (PumpInstance *) malloc(sizeof(PumpInstance));
|
||||
memset(pump, 0, sizeof(PumpInstance));
|
||||
|
||||
pump->htim = htim;
|
||||
pump->Channel = Channel;
|
||||
|
||||
HAL_TIM_PWM_Start(htim, Channel);
|
||||
return pump;
|
||||
}
|
||||
void Pump_open(PumpInstance *pump)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,20000);
|
||||
}
|
||||
|
||||
void Pump_stop(PumpInstance *pump)
|
||||
{
|
||||
__HAL_TIM_SET_COMPARE(pump->htim, pump->Channel,0);
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
//
|
||||
// Created by SJQ on 2024/5/11.
|
||||
//
|
||||
|
||||
#ifndef BASIC_FRAMEWORK_AIR_PUMP_H
|
||||
#define BASIC_FRAMEWORK_AIR_PUMP_H
|
||||
#include "main.h"
|
||||
#include "tim.h"
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// 使用的定时器类型及通道
|
||||
TIM_HandleTypeDef *htim;
|
||||
/*Channel值设定
|
||||
*TIM_CHANNEL_1
|
||||
*TIM_CHANNEL_2
|
||||
*TIM_CHANNEL_3
|
||||
*TIM_CHANNEL_4
|
||||
*TIM_CHANNEL_ALL
|
||||
*/
|
||||
uint32_t Channel;
|
||||
} PumpInstance;
|
||||
|
||||
PumpInstance *PumpInit(TIM_HandleTypeDef *htim,uint32_t Channel);
|
||||
void Pump_open(PumpInstance *pump);
|
||||
void Pump_stop(PumpInstance *pump);
|
||||
|
||||
#endif //BASIC_FRAMEWORK_AIR_PUMP_H
|
||||
|
|
@ -408,3 +408,44 @@ Matrixf<5,2> robotics::my_analytic_ikine(Matrixf<4, 4> T)
|
|||
Matrixf<5,2> ik_q(q_data);
|
||||
return ik_q;
|
||||
}
|
||||
|
||||
//末端标准正交球形腕求解
|
||||
Matrixf<3,2> robotics::spherical_wrist_ikine(const Matrixf<3,3>& R,float theta_1,float theta_2)
|
||||
{
|
||||
const float L1=0.151,L3=0.350,L4=0;
|
||||
//计算T02
|
||||
DH_t l1_dh = {
|
||||
.theta = theta_1,
|
||||
.d = L1,
|
||||
.a = 0,
|
||||
.alpha = PI/2,
|
||||
};
|
||||
DH_t l2_dh = {
|
||||
.theta = theta_2,
|
||||
.d = 0,
|
||||
.a = L3,
|
||||
.alpha = PI/2,
|
||||
};
|
||||
Matrixf<4,4> T02 = l1_dh.fkine() * l2_dh.fkine();
|
||||
|
||||
Matrixf<4,4> T = r2t(R); //末端位置仅由前两轴决定 计算球形手腕时无需考虑位置
|
||||
|
||||
Matrixf<4,4> T25 = invT(T02) * T;
|
||||
|
||||
|
||||
//腕关节(theta_5)朝上 >0
|
||||
float theta_30 = atan2(T25[1][2],T25[0][2]);
|
||||
float theta_40 = atan2(sqrtf(T25[0][2]*T25[0][2] + T25[1][2]*T25[1][2]),T25[2][2]);
|
||||
float theta_50 = atan2(T25[2][1],-T25[2][0]);
|
||||
//腕关节(theta_5)朝下 <0
|
||||
float theta_31 = atan2(-T25[1][2],-T25[0][2]);
|
||||
float theta_41 = atan2(-sqrtf(T25[0][2]*T25[0][2] + T25[1][2]*T25[1][2]),T25[2][2]);
|
||||
float theta_51 = atan2(-T25[2][1],T25[2][0]);
|
||||
|
||||
float q_data[6]={ theta_30,theta_31,
|
||||
theta_40,theta_41,
|
||||
theta_50,theta_51 };
|
||||
|
||||
Matrixf<3,2> ik_q(q_data);
|
||||
return ik_q;
|
||||
}
|
||||
|
|
@ -410,6 +410,10 @@ class Serial_Link {
|
|||
Matrixf<5, 2> my_analytic_ikine(Matrixf<4, 4> T);
|
||||
|
||||
bool check_ikine(Matrixf<4, 4> T);
|
||||
|
||||
|
||||
|
||||
Matrixf<3,2> spherical_wrist_ikine(const Matrixf<3,3>& R,float theta_1,float theta_2);
|
||||
}; // namespace robotics
|
||||
|
||||
#endif // ROBOTICS_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue