diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 1f941af..72311c5 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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(); diff --git a/application/cmd/robot_cmd.h b/application/cmd/robot_cmd.h index 74b0d32..982828c 100644 --- a/application/cmd/robot_cmd.h +++ b/application/cmd/robot_cmd.h @@ -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()调用 * diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index b2a5493..4b28fe5 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -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); diff --git a/application/robot_def.h b/application/robot_def.h index 911d495..74a5f2c 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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; diff --git a/engineering.jdebug.user b/engineering.jdebug.user index 78ba10d..9bb76cc 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -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 diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 75d0dd5..5695cbb 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -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); diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 93b8d08..5864275 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -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 \ { \ diff --git a/modules/motor/Air_pump/air_pump.c b/modules/motor/Air_pump/air_pump.c new file mode 100644 index 0000000..dc46049 --- /dev/null +++ b/modules/motor/Air_pump/air_pump.c @@ -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); +} \ No newline at end of file diff --git a/modules/motor/Air_pump/air_pump.h b/modules/motor/Air_pump/air_pump.h new file mode 100644 index 0000000..2ef1256 --- /dev/null +++ b/modules/motor/Air_pump/air_pump.h @@ -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 + +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 diff --git a/modules/robotics/robotics.cpp b/modules/robotics/robotics.cpp index 668f5dc..57aa971 100644 --- a/modules/robotics/robotics.cpp +++ b/modules/robotics/robotics.cpp @@ -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; +} \ No newline at end of file diff --git a/modules/robotics/robotics.h b/modules/robotics/robotics.h index 0744deb..3d417b0 100644 --- a/modules/robotics/robotics.h +++ b/modules/robotics/robotics.h @@ -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