diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index b583564..09a0d70 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -156,16 +156,17 @@ static void RemoteControlSet() { // 右侧开关状态为[下],机械臂 if (switch_is_down(rc_data[TEMP].rc.switch_right)) { - // 左侧开关状态为[下],前三轴 + // 左侧开关状态为[下],前两轴 if(switch_is_down(rc_data[TEMP].rc.switch_left)) { 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_; - gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_; + } - // 左侧开关状态为[中],后两轴 + // 左侧开关状态为[中],后三轴 if(switch_is_mid(rc_data[TEMP].rc.switch_left)) { + gimbal_cmd_send.roll += 0.001f * (float) rc_data[TEMP].rc.rocker_l_; gimbal_cmd_send.diff_pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1; gimbal_cmd_send.diff_roll += 0.001f * (float) rc_data[TEMP].rc.rocker_r_; } @@ -173,7 +174,7 @@ static void RemoteControlSet() { // 云台软件限位 gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX); gimbal_cmd_send.yaw = float_constrain(gimbal_cmd_send.yaw,-YAW,YAW); - //gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL); + gimbal_cmd_send.roll = float_constrain(gimbal_cmd_send.roll,-ROLL,ROLL); gimbal_cmd_send.diff_pitch = float_constrain(gimbal_cmd_send.diff_pitch,-DIFF_PITCH,DIFF_PITCH); gimbal_cmd_send.diff_roll = float_constrain(gimbal_cmd_send.diff_roll,-DIFF_ROLL,DIFF_ROLL); diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index eaa7f37..c58dae3 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -37,8 +37,31 @@ static float pitch_spd,yaw_spd,roll_spd; first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter; static float diff_r_spd,diff_l_spd; +//机械臂参数初始化 +float arm_q[5] = {0}; // 机械臂各关节位置 +robotics::Link link[5]; +robotics::Serial_Link<5> engineer_arm(link); +Matrixf<4, 4> fk_T; //正运动学 末端变换矩阵 +Matrixf<3, 1> fk_p; //正运动学 末端位置向量 +Matrixf<3, 1> fk_rpy; //正运动学 末端欧拉角 + +Matrixf<5, 2> ik_q; //逆运动学 关节位置 +const float l1 = 0.151 ,l3 = 0.350, l4 = 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[3] = robotics::Link(0,0,0,PI/2); + link[4] = robotics::Link(0,l4,0,0); + + engineer_arm = robotics::Serial_Link<5>(link); + engineer_arm.ikine_analytic = robotics::my_analytic_ikine; +} + void GimbalInit() { + Arm_Init(); Motor_Init_Config_s yaw_motor_config = { .controller_param_init_config = { .other_speed_feedback_ptr = &yaw_spd, @@ -147,9 +170,19 @@ void GimbalInit() roll_motor = DMMotorInit(&roll_motor_config); Motor_Init_Config_s diff_motor_config = { + .controller_param_init_config = { + .speed_PID = { + .Kp = 0.6f, + .Ki = 0.1f, + .Kd = 0.02f, + .MaxOut = 10, + .Improve = static_cast(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), + .IntegralLimit = 10.0F, + }, + }, .controller_setting_init_config = { - .outer_loop_type = OPEN_LOOP, - .close_loop_type = OPEN_LOOP, + .outer_loop_type = SPEED_LOOP, + .close_loop_type = SPEED_LOOP, .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = OTHER_FEED, //.feedforward_flag = CURRENT_FEEDFORWARD, @@ -230,16 +263,12 @@ void GimbalInit() /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ void GimbalTask() { - /* Example 1: PUMA560 ------------------------------------------------------------------------*/ - float m[6] = {0.2645, 0.17, 0.1705, 0, 0, 0}; - float data[18]={0, -8.5e-2, 0, 0, 0, 0, - 13.225e-2, 0, 0, 0, 0, 0, - 0, 3.7e-2, 8.525e-2, 0, 0, 0}; - Matrixf<3, 6> rc(data); - Matrixf<3, 3> I[6]; + // 获取云台控制数据 // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); + + /* 控制参数计算 ------------------------------------------------------------------------*/ //大臂重力补偿力矩 arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor->measure.position); //电机速度滤波 @@ -249,7 +278,6 @@ void GimbalTask() yaw_spd = yaw_spd_filter.out; first_order_filter_cali(&roll_spd_filter,roll_motor->measure.velocity); roll_spd = roll_spd_filter.out; - //手腕关节 first_order_filter_cali(&diff_r_spd_filter,diff_r_motor->measure.velocity); diff_r_spd = diff_r_spd_filter.out; @@ -262,13 +290,25 @@ void GimbalTask() float pitch_angle_out = PIDCalculate(&diff_pitch_loop,diff_pitch_angle,gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); float roll_angle_out = PIDCalculate(&diff_roll_loop,diff_roll_angle,gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); - float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2; - float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52; - float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out); - float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out); +// float diff_pitch_spd = (diff_r_spd + (-diff_l_spd)) / 2; +// float diff_roll_spd = (diff_r_spd - (-diff_l_spd)) /2 * 18/52; +// float pitch_out = PIDCalculate(&diff_pitch_spd_loop,diff_pitch_spd,pitch_angle_out); +// float roll_out = PIDCalculate(&diff_roll_spd_loop,diff_roll_spd,roll_angle_out); - float r_speed_set = pitch_out + roll_out; - float l_speed_set = pitch_out - roll_out; + float r_speed_set = pitch_angle_out + roll_angle_out; + float l_speed_set = pitch_angle_out - roll_angle_out; + + //正运动学 + arm_q[0] = yaw_motor->measure.position; + arm_q[1] = pitch_motor->measure.position; + arm_q[2] = roll_motor->measure.position; + arm_q[3] = diff_pitch_angle; + arm_q[4] = diff_roll_angle; + fk_T = engineer_arm.fkine(arm_q); + fk_p = robotics::t2p(fk_T); + fk_rpy = robotics::t2rpy(fk_T); + //逆运动学 + ik_q = engineer_arm.ikine_analytic(fk_T); // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref diff --git a/engineering.jdebug.user b/engineering.jdebug.user index e5aa726..eba6cf5 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,35 +1,32 @@ -Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:259:5, State=BP_STATE_DISABLED -GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Color=#e56a6f -GraphedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Color=#35792b -GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Color=#769dda -GraphedExpression="(((yaw_motor)->motor_controller).angle_PID).Measure", Color=#b14f0d -OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 +Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:265:1, State=BP_STATE_DISABLED +GraphedExpression="(diff_pitch_loop).Measure", Color=#e56a6f +GraphedExpression="(diff_pitch_loop).Ref", Color=#35792b +OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=72 OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3422 -OpenDocument="arm_mat_mult_f32.c", FilePath="C:/Users/clamar01/fork3/CMSIS_5/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c", Line=0 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=0 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="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=315 -OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=153 +OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=5 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=602, h=160, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=176, 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=602, h=481, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=465, 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=0, w=672, h=642, 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;66", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0" +OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=531, h=642, 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="70;20", CodeGraphLegendShown=0, CodeGraphLegendPosition="80;20" 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";" (((yaw_motor)->motor_controller).speed_PID).Ref";" (((yaw_motor)->motor_controller).speed_PID).Measure";" (((yaw_motor)->motor_controller).angle_PID).Ref";" (((yaw_motor)->motor_controller).angle_PID).Measure"], ColWidths=[100;100;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;100;126;110;102;126;100;118;110] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (diff_pitch_loop).Measure";" (diff_pitch_loop).Ref"], 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;100;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;124;104;117;100] @@ -48,4 +45,9 @@ WatchedExpression="gimbal_cmd_recv", RefreshRate=2, Window=Watched Data 1 WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1 WatchedExpression="roll_motor", RefreshRate=2, Window=Watched Data 1 WatchedExpression="(((roll_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1 -WatchedExpression="roll_spd_filter", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file +WatchedExpression="roll_spd_filter", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="(fk_p).data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="(fk_rpy).data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="arm_q", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="ik_q.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="pitch_motor", Window=Watched Data 1 \ No newline at end of file diff --git a/modules/robotics/robotics.cpp b/modules/robotics/robotics.cpp index 3a73e3b..e144327 100644 --- a/modules/robotics/robotics.cpp +++ b/modules/robotics/robotics.cpp @@ -338,3 +338,50 @@ Matrixf<4, 4> robotics::Link::T(float q) { } return dh_.fkine(); } + +Matrixf<5,2> robotics::my_analytic_ikine(Matrixf<4, 4> T) +{ + const float L1=0.151,L3=0.350,L4=0.139; + Matrixf<3,1> Pe = T.block<3, 1>(0, 3); // p=T(1:3,4) + Matrixf<3,1> Ze = T.block<3, 1>(0, 2); // z=T(1:3,3) + //求解W点（球形腕） + Matrixf<3,1> Pw = Pe-L4*Ze; + //求解theta1 + float theta_1 = atan2(Pw[1][0],Pw[0][0]); + //求解theta2 + //L1^2 + L3^2 - 2*L1*L3*cos_th2 = norm(Pw)^2 + float cos_th2 = (L1*L1 + L3*L3 - Pw.norm()*Pw.norm())/(2*L1*L3); + float theta_2 = acosf(cos_th2) ; //关节2 有PI/2的offset + //计算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 = 0, + .alpha = PI/2, + }; + Matrixf<4,4> T02 = l1_dh.fkine() * l2_dh.fkine(); + + 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[10] = {theta_1,theta_2 - PI/2,theta_30,theta_40,theta_50, + theta_1,theta_2 - PI/2,theta_31,theta_41,theta_51};//关节2 有PI/2的offset + + Matrixf<5,2> ik_q(q_data); + return ik_q; +} diff --git a/modules/robotics/robotics.h b/modules/robotics/robotics.h index d5f8839..d17c9c2 100644 --- a/modules/robotics/robotics.h +++ b/modules/robotics/robotics.h @@ -280,7 +280,7 @@ class Serial_Link { } // (Reserved function) inverse kinematic, analytic solution(geometric method) - Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T); + Matrixf<_n, 2> (*ikine_analytic)(Matrixf<4, 4> T); // inverse dynamic, Newton-Euler method // param[in] q: joint variable vector @@ -402,6 +402,8 @@ class Serial_Link { Matrixf<4, 4> T_; Matrixf<6, _n> J_; }; + + Matrixf<5, 2> my_analytic_ikine(Matrixf<4, 4> T); }; // namespace robotics #endif // ROBOTICS_H