diff --git a/Src/usart.c b/Src/usart.c index 7cc1d9c..7233b5b 100644 --- a/Src/usart.c +++ b/Src/usart.c @@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void) /* USER CODE END USART1_Init 1 */ huart1.Instance = USART1; - huart1.Init.BaudRate = 921600; + huart1.Init.BaudRate = 115200;//921600; huart1.Init.WordLength = UART_WORDLENGTH_8B; huart1.Init.StopBits = UART_STOPBITS_1; huart1.Init.Parity = UART_PARITY_NONE; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index d14bcd5..1f941af 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -15,6 +15,7 @@ #include "bsp_dwt.h" #include "bsp_log.h" #include "referee_task.h" +#include "referee_VT.h" // 私有宏,自动将编码器转换成角度值 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360 @@ -58,11 +59,14 @@ static To_stretch_Upload_Data_s to_stretch_fetch_data; // 从发射伸缩的反 static Robot_Status_e robot_state; // 机器人整体工作状态 static referee_info_t *referee_data; // 用于获取裁判系统的数据 +VT_info_t *referee_VT_data; //图传链路数据 + void RobotCMDInit() { rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 vision_recv_data = VisionInit(&huart1); // 视觉通信串口 referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI + referee_VT_data = VTInit(&huart1); gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); @@ -85,7 +89,7 @@ void RobotCMDInit() { }; cmd_can_comm = CANCommInit(&comm_conf); #endif // GIMBAL_BOARD - gimbal_cmd_send.pitch = 0; + gimbal_cmd_send.pitch = 40; robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 } @@ -135,7 +139,7 @@ static void update_ui_data() { // 出招表 static void RemoteControlSet() { - if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动,伸缩保持不动 + if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动 { chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP; @@ -170,6 +174,7 @@ static void RemoteControlSet() { // 左侧开关状态为[下],前两轴 if(switch_is_down(rc_data[TEMP].rc.switch_left)) { + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; 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_; @@ -177,10 +182,19 @@ static void RemoteControlSet() { // 左侧开关状态为[中],后三轴 if(switch_is_mid(rc_data[TEMP].rc.switch_left)) { + gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; 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_; } + // 左侧开关状态为[上],逆解模式 + if(switch_is_up(rc_data[TEMP].rc.switch_left)) + { + gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; + 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; + } } // 云台软件限位 gimbal_cmd_send.pitch = float_constrain(gimbal_cmd_send.pitch,PITCH_MIN,PITCH_MAX); @@ -273,11 +287,11 @@ void RobotCMDTask() { // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 - if (switch_is_down(rc_data[TEMP].rc.switch_left) || - switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制 +// if (switch_is_down(rc_data[TEMP].rc.switch_left) || +// switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制 RemoteControlSet(); - else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 - MouseKeySet(); +// else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 +// MouseKeySet(); EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 diff --git a/application/gimbal/gimbal.cpp b/application/gimbal/gimbal.cpp index a0a1a07..b2a5493 100644 --- a/application/gimbal/gimbal.cpp +++ b/application/gimbal/gimbal.cpp @@ -10,12 +10,15 @@ extern "C" { #include "general_def.h" #include "bmi088.h" #include "user_lib.h" +#include "bsp_log.h" +#include "referee_VT.h" #ifdef __cplusplus } #endif #include "matrix.h" #include "robotics.h" + static attitude_t *gimba_IMU_data; // 云台IMU数据 static DMMotorInstance *yaw_motor, *pitch_motor_l,*pitch_motor_r, *roll_motor; static DMMotorInstance *diff_r_motor,*diff_l_motor; @@ -33,6 +36,8 @@ static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 +static gimbal_mode_e last_gimbal_mode; //上次模式 用于模式切换数据过渡 + first_order_filter_type_t pitch_spd_filter,yaw_spd_filter,roll_spd_filter; static float pitch_spd,yaw_spd,roll_spd; @@ -40,14 +45,19 @@ 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}; // 机械臂各关节位置 +//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, 1> arm_q; //正运动学 末端关节位置 -Matrixf<5, 2> ik_q; //逆运动学 关节位置 + +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; void Arm_Init() { @@ -59,6 +69,7 @@ void Arm_Init() engineer_arm = robotics::Serial_Link<5>(link); engineer_arm.ikine_analytic = robotics::my_analytic_ikine; + engineer_arm.ikine_analytic_check = robotics::check_ikine; } void GimbalInit() @@ -266,6 +277,8 @@ void GimbalInit() // YAW gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); + + } //static void DMMotroEnable() //{ @@ -289,7 +302,7 @@ void GimbalTask() /* 控制参数计算 ------------------------------------------------------------------------*/ //大臂重力补偿力矩 - arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position); + arm_gravity_feedforward = - GRAVITY_COMP * arm_cos_f32(- pitch_motor_l->measure.position); //电机速度滤波 first_order_filter_cali(&pitch_spd_filter,pitch_motor_l->measure.velocity); pitch_spd = pitch_spd_filter.out; @@ -306,63 +319,131 @@ void GimbalTask() float diff_pitch_angle = (diff_r_motor->measure.position + (-diff_l_motor->measure.position))/2; float diff_roll_angle = (diff_r_motor->measure.position - (-diff_l_motor->measure.position))/2 * 18/52; - float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); - float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); - - 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,gimbal_cmd_recv.pitch * DEGREE_2_RAD); - float pitch_spd_out = PIDCalculate(&pitch_spd_loop,pitch_spd,pitch_angle_out) + arm_gravity_feedforward; + /* 控制参数计算 ------------------------------------------------------------------------*/ //正运动学 - arm_q[0] = yaw_motor->measure.position; - arm_q[1] = pitch_motor_l->measure.position; - arm_q[2] = roll_motor->measure.position; - arm_q[3] = diff_pitch_angle; - arm_q[4] = diff_roll_angle; + arm_q[0][0] = yaw_motor->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; 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); +// ik_q = engineer_arm.ikine_analytic(fk_T); +// +// ik_q_cmd = ik_q.block<5,1>(0,0); - // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘 - // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref - switch (gimbal_cmd_recv.gimbal_mode) + if(gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE) { - // 停止 - case GIMBAL_ZERO_FORCE: - DMMotorStop(yaw_motor); - DMMotorStop(pitch_motor_l); - DMMotorStop(pitch_motor_r); - DMMotorStop(roll_motor); - DMMotorStop(diff_r_motor); - DMMotorStop(diff_l_motor); + DMMotorStop(pitch_motor_l);DMMotorStop(pitch_motor_r); + DMMotorStop(yaw_motor);DMMotorStop(roll_motor); + DMMotorStop(diff_r_motor);DMMotorStop(diff_l_motor); + } + else + { + DMMotorEnable(pitch_motor_l);DMMotorEnable(pitch_motor_r); + DMMotorEnable(yaw_motor);DMMotorEnable(roll_motor); + DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor); + } - break; - // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 - case GIMBAL_GYRO_MODE: // 后续只保留此模式 - DMMotorEnable(yaw_motor); - DMMotorEnable(pitch_motor_l); - DMMotorEnable(pitch_motor_r); - DMMotorEnable(roll_motor); + if(gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE) //各关节分开控制 + { + float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); + float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); + + 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,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(pitch_motor,gimbal_cmd_recv.pitch * DEGREE_2_RAD); DMMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD); DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD); - DMMotorEnable(diff_r_motor); - DMMotorEnable(diff_l_motor); DMMotorSetRef(diff_r_motor,r_speed_set); DMMotorSetRef(diff_l_motor,-l_speed_set); - break; - default: - break; } + if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE) + { + static float q_set[5] ; //记录五个关节的目标值 + if(last_gimbal_mode == GIMBAL_GYRO_MODE) + { + //切换至逆解模式时，目标值设置为当前值 + cmd_xyz = fk_p; + } + 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); + + 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]; + } + } + 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 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]); + 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(diff_r_motor,r_speed_set); + DMMotorSetRef(diff_l_motor,-l_speed_set); + + } + //保存上次模式 + last_gimbal_mode = gimbal_cmd_recv.gimbal_mode; + + + + + // 在合适的地方添加pitch重力补偿前馈力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 diff --git a/application/robot_def.h b/application/robot_def.h index df23ee0..911d495 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -30,8 +30,8 @@ #define DIFF_ROLL 180 #define ROLL 90 -#define PITCH_MIN (-160.0f) -#define PITCH_MAX 20.0F +#define PITCH_MIN (-160.0f) //注意此处和坐标系是反的 +#define PITCH_MAX (40.0F) #define YAW 60 @@ -106,6 +106,7 @@ typedef enum GIMBAL_ZERO_FORCE = 0, // 电流零输入 GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据? GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 + GIMBAL_IKINE_MODE, // 机械臂运动学逆解模式，输入6d-tof位置 ADJUST_PITCH_MODE, //找pitch的限位 } gimbal_mode_e; @@ -146,6 +147,11 @@ typedef struct float roll; float diff_pitch; //差速器pitch float diff_roll; //差速器roll + + float x_add; + float y_add; //末端目标位置增量 + float z_add; + float chassis_rotate_wz; uint8_t MotorEnableFlag; gimbal_mode_e gimbal_mode; diff --git a/application/robot_task.h b/application/robot_task.h index 6e9ccb3..50adf39 100644 --- a/application/robot_task.h +++ b/application/robot_task.h @@ -86,9 +86,9 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument) motor_start = DWT_GetTimeline_ms(); MotorControlTask(); motor_dt = DWT_GetTimeline_ms() - motor_start; - if (motor_dt > 1) + if (motor_dt > 2) LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt); - osDelay(1); + osDelay(2); } } diff --git a/engineering.jdebug b/engineering.jdebug index 708c732..1a5c097 100644 --- a/engineering.jdebug +++ b/engineering.jdebug @@ -33,7 +33,6 @@ void OnProjectLoad (void) { // // User settings // - Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ); Project.SetOSPlugin ("FreeRTOSPlugin_CM4"); File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf"); } diff --git a/engineering.jdebug.user b/engineering.jdebug.user index b4869ce..78ba10d 100644 --- a/engineering.jdebug.user +++ b/engineering.jdebug.user @@ -1,58 +1,44 @@ -Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:269:5, State=BP_STATE_DISABLED -GraphedExpression="(pitch_angle_loop).Ref", Color=#e56a6f -GraphedExpression="(pitch_angle_loop).Measure", Color=#35792b -GraphedExpression="(pitch_spd_loop).Ref", Color=#769dda -GraphedExpression="(pitch_spd_loop).Measure", Color=#b14f0d -OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=13 -OpenDocument="tasks.c", FilePath="D:/CLion/Project/engineering/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3422 -OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=0 +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 +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="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=69 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=184, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=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="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=457, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=450, 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=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="493;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="507;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="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";" (pitch_angle_loop).Ref";" (pitch_angle_loop).Measure";" (pitch_spd_loop).Ref";" (pitch_spd_loop).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;102;102;118;100;118;110] +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="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;129;100] +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;108;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="diff_r_motor", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="diff_l_motor", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="diff_pitch_loop", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="diff_pitch_angle", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="diff_pitch_angle", Window=Watched Data 1 -WatchedExpression="diff_roll_loop", Window=Watched Data 1 -WatchedExpression="diff_pitch_spd_loop", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="diff_roll_spd_loop", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="(diff_roll_spd_loop).Measure", Window=Watched Data 1 -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 -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="arm_q.data_", RefreshRate=2, Window=Watched Data 1 WatchedExpression="ik_q.data_", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="pitch_motor", Window=Watched Data 1 -WatchedExpression="pitch_motor_l", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="pitch_spd_loop", RefreshRate=2, Window=Watched Data 1 -WatchedExpression="pitch_angle_loop", Window=Watched Data 1 \ No newline at end of file +WatchedExpression="fk_p.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="ik_q_cmd.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="cmd_xyz.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="T_cmd", Window=Watched Data 1 +WatchedExpression="T_cmd.data_", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="referee_VT_data", RefreshRate=2, Window=Watched Data 1 +WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 \ No newline at end of file diff --git a/modules/robotics/robotics.cpp b/modules/robotics/robotics.cpp index e144327..668f5dc 100644 --- a/modules/robotics/robotics.cpp +++ b/modules/robotics/robotics.cpp @@ -339,6 +339,23 @@ Matrixf<4, 4> robotics::Link::T(float q) { return dh_.fkine(); } +bool robotics::check_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; + float length = Pw.norm(); + //满足构成三角形条件，theta2才有解 + if(length <= (L1+L3) && length >= (L3-L1)) + return true; + else + return false; + +} + Matrixf<5,2> robotics::my_analytic_ikine(Matrixf<4, 4> T) { const float L1=0.151,L3=0.350,L4=0.139; @@ -378,9 +395,15 @@ Matrixf<5,2> robotics::my_analytic_ikine(Matrixf<4, 4> T) 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 +// +// 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 +// + float q_data[10]={ theta_1,theta_1 , + theta_2 - PI/2 , theta_2 - PI/2, + theta_30,theta_31 , + theta_40 , theta_41 , + theta_50,theta_51 }; Matrixf<5,2> ik_q(q_data); return ik_q; diff --git a/modules/robotics/robotics.h b/modules/robotics/robotics.h index d17c9c2..0744deb 100644 --- a/modules/robotics/robotics.h +++ b/modules/robotics/robotics.h @@ -282,6 +282,10 @@ class Serial_Link { // (Reserved function) inverse kinematic, analytic solution(geometric method) Matrixf<_n, 2> (*ikine_analytic)(Matrixf<4, 4> T); + // (Reserved function) check inverse kinematic , analytic solution(geometric method) + // 检查解析解是否存在 + bool (*ikine_analytic_check)(Matrixf<4, 4> T); + // inverse dynamic, Newton-Euler method // param[in] q: joint variable vector // param[in] qv: dq/dt @@ -404,6 +408,8 @@ class Serial_Link { }; Matrixf<5, 2> my_analytic_ikine(Matrixf<4, 4> T); + + bool check_ikine(Matrixf<4, 4> T); }; // namespace robotics #endif // ROBOTICS_H