运动学逆解测试 图传链路测试
This commit is contained in:
parent
2de0ed56ca
commit
9fc944012e
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
|
|
|
|||
|
|
@ -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电机角度反馈计算出当前配重下的重力矩
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue