zyx
/
engineering
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

目前问题:末端pitch丢零点 逆解时转动过程超过限位 两条can线概率抽风

This commit is contained in:
宋家齐 2024-05-16 16:04:48 +08:00
parent c8bb8154f9
commit dccb283f4f
12 changed files with 450 additions and 333 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

272
application/cmd/robot_cmd.c
View File

@ -68,7 +68,7 @@ custom_control_t CustomControl; //自定义控制器数据
void RobotCMDInit() { void RobotCMDInit() {
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个 rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
vision_recv_data = VisionInit(&huart1); // 视觉通信串口 //vision_recv_data = VisionInit(&huart1); // 视觉通信串口
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
referee_VT_data = VTInit(&huart1); referee_VT_data = VTInit(&huart1);
@ -96,7 +96,7 @@ void RobotCMDInit() {
}; };
cmd_can_comm = CANCommInit(&comm_conf); cmd_can_comm = CANCommInit(&comm_conf);
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
gimbal_cmd_send.pitch = 40; //gimbal_cmd_send.pitch = PITCH_MIN;
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入 robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
} }
@ -106,26 +106,26 @@ void RobotCMDInit() {
* 单圈绝对角度的范围是0~360,说明文档中有图示 * 单圈绝对角度的范围是0~360,说明文档中有图示
* *
*/ */
static void CalcOffsetAngle() { //static void CalcOffsetAngle() {
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数 // // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
static float angle; // static float angle;
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度 // angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度 //#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE) // if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
else if (angle > 180.0f + YAW_ALIGN_ANGLE) // else if (angle > 180.0f + YAW_ALIGN_ANGLE)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
else // else
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
#else // 小于180度 //#else // 小于180度
if (angle > YAW_ALIGN_ANGLE) // if (angle > YAW_ALIGN_ANGLE)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f) // else if (angle <= YAW_ALIGN_ANGLE && angle >= YAW_ALIGN_ANGLE - 180.0f)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
else // else
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f; // chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
#endif //#endif
} //}
static void death_check() static void death_check()
{ {
@ -145,6 +145,7 @@ static void update_ui_data() {
ui_data.xyz[1] = to_stretch_fetch_data.protract_x; ui_data.xyz[1] = to_stretch_fetch_data.protract_x;
ui_data.xyz[2] = to_stretch_fetch_data.lift_z; ui_data.xyz[2] = to_stretch_fetch_data.lift_z;
ui_data.diff_pitch_angle = gimbal_fetch_data.diff_pitch_feedback;
} }
@ -165,24 +166,25 @@ static void RemoteControlSet() {
if (switch_is_up(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; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度
chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转 chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转
to_stretch_cmd_send.tc += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//图传 to_stretch_cmd_send.tc = 3.0F * (float) rc_data[TEMP].rc.rocker_l1;//图传
// 图传限位 // 图传限位
if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE; // if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE; // if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动 } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动
{ {
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
to_stretch_cmd_send.tc = 0;
chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向 chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向 chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
@ -197,47 +199,42 @@ static void RemoteControlSet() {
// 右侧开关状态为[下],机械臂 // 右侧开关状态为[下],机械臂
if (switch_is_down(rc_data[TEMP].rc.switch_right)) { if (switch_is_down(rc_data[TEMP].rc.switch_right)) {
// 左侧开关状态为[下],前两轴 to_stretch_cmd_send.tc = 0;
// 左侧开关状态为[下],大臂 + 龙门架 + 底盘平移
if(switch_is_down(rc_data[TEMP].rc.switch_left)) if(switch_is_down(rc_data[TEMP].rc.switch_left))
{ {
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
gimbal_cmd_send.pitch += 0.001f * (float) rc_data[TEMP].rc.rocker_r1; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
gimbal_cmd_send.yaw += 0.001f * (float) rc_data[TEMP].rc.rocker_r_; gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制
to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_r_;//抬升
to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_r1;//前伸
} }
// 左侧开关状态为[中],后三轴 // // 左侧开关状态为[中],后三轴
// if(switch_is_mid(rc_data[TEMP].rc.switch_left))
// {
// gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_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_mid(rc_data[TEMP].rc.switch_left)) 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.gimbal_mode = GIMBAL_IKINE_MODE;
gimbal_cmd_send.x_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r_; gimbal_cmd_send.pitch = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制
gimbal_cmd_send.y_add = 1e-6f * (float) rc_data[TEMP].rc.rocker_r1; chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
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_;
} }
} }
// 云台软件限位
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.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); for (int i = 0; i < 4; ++i) {
gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
}
QuaternionToEularAngle(gimbal_cmd_send.quat,&gimbal_cmd_send.rpy[2],&gimbal_cmd_send.rpy[1],&gimbal_cmd_send.rpy[0]);
} }
@ -246,25 +243,9 @@ static void RemoteControlSet() {
* *
*/ */
static void MouseKeySet() { static void MouseKeySet() {
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测 chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000; to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
chassis_cmd_send.wz = (float) rc_data[TEMP].mouse.x / 660 * 4; switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 2) //G按键开关气泵
to_stretch_cmd_send.tc -= (float) rc_data[TEMP].mouse.y / 660 * 6;
to_stretch_cmd_send.ud += rc_data[TEMP].key[KEY_PRESS].q * 10 - rc_data[TEMP].key[KEY_PRESS].e * 10;
to_stretch_cmd_send.fb += rc_data[TEMP].key[KEY_PRESS].z * 10 - rc_data[TEMP].key[KEY_PRESS].c * 10;
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
{
case 1:
MyUIInit();
rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
break;
default:
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F按键开关气泵
{ {
case 0: case 0:
Pump_stop(air_pump); Pump_stop(air_pump);
@ -275,17 +256,113 @@ static void MouseKeySet() {
ui_data.pump_mode = PUMP_OPEN; ui_data.pump_mode = PUMP_OPEN;
break; break;
} }
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_B] % 2) // CTRL + B键手动刷新UI
{
case 1:
MyUIInit();
rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_B]++;
break;
default:
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_X] % 2) // X 取银矿模式
{
case 1:
gimbal_cmd_send.gimbal_mode = GIMBAL_SILVER_MODE; //取银矿姿态
rc_data[TEMP].key_count[KEY_PRESS][Key_X]++;
break;
default:
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_Z] % 2) // ctrl + Z 取地矿模式
{
case 1:
gimbal_cmd_send.gimbal_mode = GIMBAL_FLOOR_MODE; //取银矿姿态
rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_Z]++;
break;
default:
break;
}
// switch (rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_X] % 2) // ctrl+x 调整零点
// { // {
// case 0: // case 1:
// chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; // gimbal_cmd_send.gimbal_mode = ADJUST_PITCH_MODE; //调整pitch零点
// gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; // rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_X]++;
// break; // break;
// default: // default:
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
// break; // break;
// } // }
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_V] % 2) // V 开启自定义控制器
{
case 1:
gimbal_cmd_send.gimbal_mode = GIMBAL_IKINE_MODE; //自定义
rc_data[TEMP].key_count[KEY_PRESS][Key_V]++;
break;
default:
break;
}
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) //B 切手动
{
case 0:
gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE; //自定义
rc_data[TEMP].key_count[KEY_PRESS][Key_B]++;
break;
default:
break;
}
//银矿 吸盘朝前
if (gimbal_cmd_send.gimbal_mode == GIMBAL_SILVER_MODE)
{ gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -180; gimbal_cmd_send.rpy[2] = 0;}
//地矿 吸盘朝下
if (gimbal_cmd_send.gimbal_mode == GIMBAL_FLOOR_MODE)
{ gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -90; gimbal_cmd_send.rpy[2] = 0;}
//存矿 矿仓角度
if (gimbal_cmd_send.gimbal_mode == GIMBAL_STORAGE_MODE)
{ gimbal_cmd_send.rpy[0] = 0; gimbal_cmd_send.rpy[1] = -30; gimbal_cmd_send.rpy[2] = 0;}
//手动模式 可以用遥控器调节末端姿态
if (gimbal_cmd_send.gimbal_mode == GIMBAL_MANUAL_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 (gimbal_cmd_send.gimbal_mode == GIMBAL_IKINE_MODE)
{//自定义控制器控制末端姿态
if(rc_data[TEMP].key[KEY_PRESS].ctrl)
{
for (int i = 0; i < 4; ++i) {
gimbal_cmd_send.quat[i] = (float)(CustomControl.q[i])*1e-4f;
}
QuaternionToEularAngle(gimbal_cmd_send.quat,&gimbal_cmd_send.rpy[2],&gimbal_cmd_send.rpy[1],&gimbal_cmd_send.rpy[0]);
//ctrl按下调整姿态
} else
{
to_stretch_cmd_send.fb += CustomControl.dx * 0.04f;
to_stretch_cmd_send.ud += CustomControl.dz * 0.15f;
}
}
else
{
to_stretch_cmd_send.ud += (float)(rc_data[TEMP].key[KEY_PRESS].q - rc_data[TEMP].key[KEY_PRESS].e) * 0.15f;
to_stretch_cmd_send.fb += (float)(rc_data[TEMP].key[KEY_PRESS].z - rc_data[TEMP].key[KEY_PRESS].c) * 0.3f;
}
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 8000 - rc_data[TEMP].key[KEY_PRESS].s * 8000; // 系数待测
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 8000 - rc_data[TEMP].key[KEY_PRESS].d * 8000;
chassis_cmd_send.wz = -(float) rc_data[TEMP].mouse.x * 8;
to_stretch_cmd_send.tc = -(float) rc_data[TEMP].mouse.y * 20;
gimbal_cmd_send.pitch = (float)(rc_data[TEMP].key[KEY_PRESS].r - rc_data[TEMP].key[KEY_PRESS].f) * 0.25f; //增量控制
} }
/** /**
@ -328,13 +405,29 @@ void RobotCMDTask() {
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data); SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
update_ui_data(); update_ui_data();
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle(); //CalcOffsetAngle();
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
// if (switch_is_down(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)) // 遥控器左侧开关状态为[下],[中],遥控器控制 switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
RemoteControlSet(); RemoteControlSet();
// else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制 else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
// MouseKeySet(); MouseKeySet();
// 云台软件限位
// 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.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);
//抬升软件限位
if(to_stretch_cmd_send.ud <= UD_MIN) to_stretch_cmd_send.ud = UD_MIN;
if(to_stretch_cmd_send.ud >= UD_MAX) to_stretch_cmd_send.ud = UD_MAX;
//前伸软件限位
if(to_stretch_cmd_send.fb <= FB_MIN) to_stretch_cmd_send.fb = FB_MIN;
if(to_stretch_cmd_send.fb >= FB_MAX) to_stretch_cmd_send.fb = FB_MAX;
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况 EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
@ -349,6 +442,5 @@ void RobotCMDTask() {
#endif // GIMBAL_BOARD #endif // GIMBAL_BOARD
PubPushMessage(to_stretch_cmd_pub, (void *) &to_stretch_cmd_send); PubPushMessage(to_stretch_cmd_pub, (void *) &to_stretch_cmd_send);
PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send); PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
VisionSend(&vision_send_data);
} }

272
application/gimbal/gimbal.cpp
View File

@ -44,6 +44,10 @@ static float pitch_spd,yaw_spd,roll_spd;
first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter; first_order_filter_type_t diff_r_spd_filter,diff_l_spd_filter;
static float diff_r_spd,diff_l_spd; static float diff_r_spd,diff_l_spd;
first_order_filter_type_t Qset_filter[3]; //关节目标值滤波
float q_set[5] ; //五个关节的目标值
//机械臂参数初始化 //机械臂参数初始化
//float arm_q[5] = {0}; // 机械臂各关节位置 //float arm_q[5] = {0}; // 机械臂各关节位置
robotics::Link link[5]; robotics::Link link[5];
@ -83,9 +87,9 @@ void GimbalInit()
.controller_param_init_config = { .controller_param_init_config = {
.other_speed_feedback_ptr = &yaw_spd, .other_speed_feedback_ptr = &yaw_spd,
.speed_PID = { .speed_PID = {
.Kp = 3.5f,//5, .Kp = 0.0f,//3.5f,//5,
.Ki = 0.0f, .Ki = 0.0f,
.Kd = 0.02f, .Kd = 0,//0.02f,
.MaxOut = 10, .MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement), .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 10.0F, .IntegralLimit = 10.0F,
@ -109,10 +113,7 @@ void GimbalInit()
.motor_type = DM6006, .motor_type = DM6006,
}; };
yaw_motor_config.can_init_config.can_handle = &hcan2;
yaw_motor_config.can_init_config.tx_id = 1;
yaw_motor_config.can_init_config.rx_id = 2;
yaw_motor = DMMotorInit(&yaw_motor_config);
Motor_Init_Config_s pitch_motor_config = { Motor_Init_Config_s pitch_motor_config = {
.controller_param_init_config = { .controller_param_init_config = {
@ -120,8 +121,6 @@ void GimbalInit()
//.current_feedforward_ptr = &arm_gravity_feedforward, //.current_feedforward_ptr = &arm_gravity_feedforward,
}, },
.controller_setting_init_config = { .controller_setting_init_config = {
// .outer_loop_type = ANGLE_LOOP,
// .close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
.outer_loop_type = OPEN_LOOP, .outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP, .close_loop_type = OPEN_LOOP,
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
@ -131,10 +130,7 @@ void GimbalInit()
.motor_type = DM6006, .motor_type = DM6006,
}; };
pitch_motor_config.can_init_config.can_handle = &hcan2;
pitch_motor_config.can_init_config.tx_id = 0x03;
pitch_motor_config.can_init_config.rx_id = 0x04;
pitch_motor_l = DMMotorInit(&pitch_motor_config);
PID_Init_Config_s pitch_spd_config= { PID_Init_Config_s pitch_spd_config= {
.Kp = 2.5f, .Kp = 2.5f,
@ -160,15 +156,15 @@ void GimbalInit()
.controller_param_init_config = { .controller_param_init_config = {
.other_speed_feedback_ptr = &roll_spd, .other_speed_feedback_ptr = &roll_spd,
.speed_PID = { .speed_PID = {
.Kp = 0.8f, .Kp = 0.2f,
.Ki = 0.3f, .Ki = 0.0f,
.Kd = 0.02f, .Kd = 0.02f,
.MaxOut = 10, .MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), .Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 10.0F, .IntegralLimit = 10.0F,
}, },
.angle_PID = { .angle_PID = {
.Kp = 10.0f, .Kp = 25.0f,
.Ki = 0.0f, .Ki = 0.0f,
.Kd = 0.0f, .Kd = 0.0f,
.MaxOut = 10, .MaxOut = 10,
@ -186,17 +182,14 @@ void GimbalInit()
.motor_type = DM4310, .motor_type = DM4310,
}; };
roll_motor_config.can_init_config.can_handle = &hcan2;
roll_motor_config.can_init_config.tx_id = 5;
roll_motor_config.can_init_config.rx_id = 6;
roll_motor = DMMotorInit(&roll_motor_config);
Motor_Init_Config_s diff_motor_config = { Motor_Init_Config_s diff_motor_config = {
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 0.2f,//0.6f, .Kp = 0.6f,
.Ki = 0,//0.1f, .Ki = 0.1f,
.Kd = 0.02,//0.02f, .Kd = 0.02f,
.MaxOut = 10, .MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement), .Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 10.0F, .IntegralLimit = 10.0F,
@ -213,17 +206,7 @@ void GimbalInit()
.motor_type = DM4310, .motor_type = DM4310,
}; };
diff_motor_config.can_init_config.can_handle = &hcan2;
diff_motor_config.can_init_config.tx_id = 7;
diff_motor_config.can_init_config.rx_id = 8;
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
diff_r_motor = DMMotorInit(&diff_motor_config);
diff_motor_config.can_init_config.can_handle = &hcan2;
diff_motor_config.can_init_config.tx_id = 9;
diff_motor_config.can_init_config.rx_id = 10;
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
diff_l_motor = DMMotorInit(&diff_motor_config);
const float spd_filter_num = 0.05f; const float spd_filter_num = 0.05f;
first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num); first_order_filter_init(&pitch_spd_filter,5e-3,&spd_filter_num);
@ -236,8 +219,13 @@ void GimbalInit()
first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff); first_order_filter_init(&diff_r_spd_filter,5e-3,&spd_filter_num_diff);
first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff); first_order_filter_init(&diff_l_spd_filter,5e-3,&spd_filter_num_diff);
const float Qset_filter_num = 0.1f;
first_order_filter_init(&Qset_filter[0],5e-3,&Qset_filter_num);
first_order_filter_init(&Qset_filter[1],5e-3,&Qset_filter_num);
first_order_filter_init(&Qset_filter[2],5e-3,&Qset_filter_num);
PID_Init_Config_s diff_pitch_config= { PID_Init_Config_s diff_pitch_config= {
.Kp = 20.0F,//15.0f, .Kp = 20.0f,//20.0F,//15.0f,
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.MaxOut = 10, .MaxOut = 10,
@ -246,16 +234,6 @@ void GimbalInit()
}; };
PIDInit(&diff_pitch_loop,&diff_pitch_config); PIDInit(&diff_pitch_loop,&diff_pitch_config);
PID_Init_Config_s diff_pitch_spd_config= {
.Kp = 0.6f,
.Ki = 2.0f,
.Kd = 0.01f,
.MaxOut = 10,
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
.IntegralLimit = 100,
};
PIDInit(&diff_pitch_spd_loop,&diff_pitch_spd_config);
PID_Init_Config_s diff_roll_config= { PID_Init_Config_s diff_roll_config= {
.Kp = 14.0f, .Kp = 14.0f,
.Ki = 0, .Ki = 0,
@ -266,15 +244,36 @@ void GimbalInit()
}; };
PIDInit(&diff_roll_loop,&diff_roll_config); PIDInit(&diff_roll_loop,&diff_roll_config);
PID_Init_Config_s diff_roll_spd_config= {
.Kp = 1.0f, //需要按id顺序初始化 否则有电机不能正确运行
.Ki = 1.0f,
.Kd = 0.02f, //决定放弃yaw轴
.MaxOut = 10, // yaw_motor_config.can_init_config.can_handle = &hcan2;
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement), // yaw_motor_config.can_init_config.tx_id = 1;
.IntegralLimit = 100, // yaw_motor_config.can_init_config.rx_id = 2;
}; // yaw_motor = DMMotorInit(&yaw_motor_config);
PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
pitch_motor_config.can_init_config.can_handle = &hcan2;
pitch_motor_config.can_init_config.tx_id = 3;
pitch_motor_config.can_init_config.rx_id = 4;
pitch_motor_l = DMMotorInit(&pitch_motor_config);
roll_motor_config.can_init_config.can_handle = &hcan2;
roll_motor_config.can_init_config.tx_id = 5;
roll_motor_config.can_init_config.rx_id = 6;
roll_motor = DMMotorInit(&roll_motor_config);
diff_motor_config.can_init_config.can_handle = &hcan2;
diff_motor_config.can_init_config.tx_id = 7;
diff_motor_config.can_init_config.rx_id = 8;
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_r_spd;
diff_r_motor = DMMotorInit(&diff_motor_config);
diff_motor_config.can_init_config.can_handle = &hcan2;
diff_motor_config.can_init_config.tx_id = 9;
diff_motor_config.can_init_config.rx_id = 10;
diff_motor_config.controller_param_init_config.other_speed_feedback_ptr = &diff_l_spd;
diff_l_motor = DMMotorInit(&diff_motor_config);
pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！ pitch_motor_config.can_init_config.tx_id = 11; //按顺序初始化！！！
pitch_motor_config.can_init_config.rx_id = 12; pitch_motor_config.can_init_config.rx_id = 12;
@ -285,8 +284,17 @@ void GimbalInit()
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
// //上电校准两个4310零点
// DMMotorCaliEncoder(diff_r_motor);
// DMMotorCaliEncoder(diff_l_motor);
////水平方向3.02 矿仓 0
// DMMotorCaliEncoder(pitch_motor_l);
q_set[1] = PITCH_MIN * DEGREE_2_RAD; //上电给pitch轴赋值
} }
//#define PITCH_OFFSET 3.02F //pitch轴水平时编码器值
//static void DMMotroEnable() //static void DMMotroEnable()
//{ //{
// if(gimbal_cmd_recv.MotorEnableFlag) // if(gimbal_cmd_recv.MotorEnableFlag)
@ -301,16 +309,16 @@ void GimbalInit()
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */ /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
//float debug_diff_kp = 0.1f,debug_diff_ki=0,debug_diff_kd = 0; float debug_diff_kp = 0.3f,debug_diff_ki=0,debug_diff_kd = 0.01f;
void GimbalTask() void GimbalTask()
{ {
// diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp; diff_r_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
// diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki; diff_r_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
// diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd; diff_r_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
//
// diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp; diff_l_motor->motor_controller.speed_PID.Kp = debug_diff_kp;
// diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki; diff_l_motor->motor_controller.speed_PID.Ki = debug_diff_ki;
// diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd; diff_l_motor->motor_controller.speed_PID.Kd = debug_diff_kd;
// 获取云台控制数据 // 获取云台控制数据
// 后续增加未收到数据的处理 // 后续增加未收到数据的处理
SubGetMessage(gimbal_sub, &gimbal_cmd_recv); SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
@ -337,8 +345,8 @@ void GimbalTask()
/* 控制参数计算 ------------------------------------------------------------------------*/ /* 控制参数计算 ------------------------------------------------------------------------*/
//正运动学 //正运动学
arm_q[0][0] = yaw_motor->measure.position; arm_q[0][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[2][0] = roll_motor->measure.position;
arm_q[3][0] = diff_pitch_angle; arm_q[3][0] = diff_pitch_angle;
arm_q[4][0] = diff_roll_angle; arm_q[4][0] = diff_roll_angle;
@ -366,30 +374,25 @@ void GimbalTask()
DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor); DMMotorEnable(diff_r_motor);DMMotorEnable(diff_l_motor);
} }
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE) //各关节分开控制 // if(gimbal_cmd_recv.gimbal_mode == ADJUST_PITCH_MODE)
// {
// //校准两个4310零点
// DMMotorCaliEncoder(diff_r_motor);
// DMMotorCaliEncoder(diff_l_motor);
// }
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_MANUAL_MODE) //各关节分开控制
{ {
float diff_pitch_angle_out = PIDCalculate(&diff_pitch_loop, diff_pitch_angle, gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD); q_set[0] = 0;
float diff_roll_angle_out = PIDCalculate(&diff_roll_loop, diff_roll_angle, gimbal_cmd_recv.diff_roll * DEGREE_2_RAD); q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
q_set[2] += gimbal_cmd_recv.roll * DEGREE_2_RAD;
float r_speed_set = diff_pitch_angle_out + diff_roll_angle_out; q_set[3] += gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD;
float l_speed_set = diff_pitch_angle_out - diff_roll_angle_out; q_set[4] += gimbal_cmd_recv.diff_roll * DEGREE_2_RAD;
//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(yaw_motor,gimbal_cmd_recv.yaw * DEGREE_2_RAD);
DMMotorSetRef(roll_motor,gimbal_cmd_recv.roll * DEGREE_2_RAD);
DMMotorSetRef(diff_r_motor,r_speed_set);
DMMotorSetRef(diff_l_motor,-l_speed_set);
} }
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE) if(gimbal_cmd_recv.gimbal_mode != GIMBAL_MANUAL_MODE && gimbal_cmd_recv.gimbal_mode != GIMBAL_ZERO_FORCE)
{ {
static float q_set[5] ; //记录五个关节的目标值
if(last_gimbal_mode == GIMBAL_GYRO_MODE) if(last_gimbal_mode == GIMBAL_MANUAL_MODE)
{ {
//切换至逆解模式时，目标值设置为当前值 //切换至逆解模式时，目标值设置为当前值
cmd_R = robotics::t2r( fk_T); cmd_R = robotics::t2r( fk_T);
@ -397,67 +400,102 @@ void GimbalTask()
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
cmd_quat[i][0] = gimbal_cmd_recv.quat[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); if(gimbal_cmd_recv.gimbal_mode == GIMBAL_SILVER_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_FLOOR_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_STORAGE_MODE)
cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态 {
// T_cmd = robotics::p2t(cmd_xyz); float trans_rpy_data[3] = {gimbal_cmd_recv.rpy[0]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[1]*DEGREE_2_RAD,gimbal_cmd_recv.rpy[2]*DEGREE_2_RAD};
Matrixf<3,1> trans_rpy(trans_rpy_data);
cmd_R = robotics::rpy2r(trans_rpy);
}
if(gimbal_cmd_recv.gimbal_mode == GIMBAL_IKINE_MODE)
{
float trans_data[3] = {0,-PI,0};
Matrixf<3,1> trans(trans_data);
cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans); //转到基座坐标系下的姿态
}
ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]); ik_q3 = robotics::spherical_wrist_ikine(cmd_R,arm_q[0][0],arm_q[1][0]);
//后三关节的误差 选用误差小的一组解 //后三关节的误差 选用误差小的一组解
float err[2] = {0}; float err[2][3] = {0};
Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0); Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
Matrixf<3,1> ik = ik_q3.block<3,1>(0,i); Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
err[i] = (ik - arm_q3).norm(); for (int j = 0; j < 3; ++j) {
err[i][j] = loop_float_constrain(ik[j][0] - arm_q3[j][0],-PI,PI);
}
} }
float err1 = abs(err[1][0]) + abs(err[1][1]) + abs(err[1][2]);
// if (err[1] >= err[0]) float err0 = abs(err[0][0]) + abs(err[0][1]) + abs(err[0][2]);
//选用 总 误差小的一组解
// if (err1 >= err0)
// ik_q3_cmd = ik_q3.block<3,1>(0,0); // ik_q3_cmd = ik_q3.block<3,1>(0,0);
// else // else
// ik_q3_cmd = ik_q3.block<3,1>(0,1); // ik_q3_cmd = ik_q3.block<3,1>(0,1);
// //选用 roll误差小的一组解 // // 选用 roll 误差小的一组解
if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0])) float roll_err1 = ik_q3[0][0] - arm_q3[0][0];
roll_err1 = loop_float_constrain(roll_err1,-PI,PI);
float roll_err2 = ik_q3[1][0] - arm_q3[0][0];
roll_err2 = loop_float_constrain(roll_err2,-PI,PI);
if (abs(roll_err1) <= abs(roll_err2))
ik_q3_cmd = ik_q3.block<3,1>(0,0); ik_q3_cmd = ik_q3.block<3,1>(0,0);
else else
ik_q3_cmd = ik_q3.block<3,1>(0,1); ik_q3_cmd = ik_q3.block<3,1>(0,1);
//选用 第一组解
//ik_q3_cmd = ik_q3.block<3,1>(0,0);
ik_q3_cmd[0][0] += loop_float_constrain(ik_q3_cmd[0][0] - arm_q3[0][0],-PI,PI);
ik_q3_cmd[1][0] += loop_float_constrain(ik_q3_cmd[1][0] - arm_q3[1][0],-PI,PI);
ik_q3_cmd[2][0] += loop_float_constrain(ik_q3_cmd[2][0] - arm_q3[2][0],-PI,PI);
ik_q3_cmd[0][0] = float_constrain(ik_q3_cmd[0][0],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD); 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[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); 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) { for (int i = 0; i < 3; ++i) {
q_set[i] = ik_q3_cmd[i][0]; first_order_filter_cali(&Qset_filter[i],ik_q3_cmd[i][0]);
q_set[i+2] = Qset_filter[i].out;
} }
//大臂pitch用遥控器数据
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
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 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,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);
} }
//保存上次模式 //保存上次模式
last_gimbal_mode = gimbal_cmd_recv.gimbal_mode; last_gimbal_mode = gimbal_cmd_recv.gimbal_mode;
q_set[1] = float_constrain(q_set[1],PITCH_MIN* DEGREE_2_RAD,PITCH_MAX* DEGREE_2_RAD);
q_set[2] = float_constrain(q_set[2],-ROLL* DEGREE_2_RAD,ROLL* DEGREE_2_RAD);
q_set[3] = float_constrain(q_set[3],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
q_set[4] = float_constrain(q_set[4],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
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 pitch yaw先遥控器控制
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);
@ -468,7 +506,7 @@ void GimbalTask()
// 设置反馈数据,主要是imu和yaw的ecd // 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data; gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round; gimbal_feedback_data.diff_pitch_feedback = diff_pitch_angle * RAD_2_DEGREE;
// 推送消息 // 推送消息
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data); PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);

8
application/robot.c
View File

@ -32,11 +32,11 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit(); RobotCMDInit();
GimbalInit(); GimbalInit();
//To_stretchInit(); To_stretchInit();
#endif #endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
//ChassisInit(); ChassisInit();
#endif #endif
OSTaskInit(); // 创建基础任务 OSTaskInit(); // 创建基础任务
@ -50,11 +50,11 @@ void RobotTask()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask(); RobotCMDTask();
GimbalTask(); GimbalTask();
//To_stretchTask(); To_stretchTask();
#endif #endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
//ChassisTask(); ChassisTask();
#endif #endif
} }

17
application/robot_def.h
View File

@ -29,18 +29,18 @@
#define DIFF_PITCH 90 //差动手腕关节 #define DIFF_PITCH 90 //差动手腕关节
#define DIFF_ROLL 180 #define DIFF_ROLL 180
#define ROLL 90 #define ROLL 170
#define PITCH_MIN (-160.0f) //注意此处和坐标系是反的 #define PITCH_MIN (-174.0f) //注意此处和坐标系是反的
#define PITCH_MAX (40.0F) #define PITCH_MAX (40.0F)
#define YAW 60 #define YAW 60
//龙门架限位 //龙门架限位
#define UD_MAX 100 #define UD_MAX 210
#define UD_MIN 0 #define UD_MIN 0
#define FB_MIN 0 #define FB_MIN 0
#define FB_MAX 100 #define FB_MAX 205
// 云台参数 // 云台参数
#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改 #define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
@ -112,14 +112,15 @@ typedef enum
{ {
GIMBAL_ZERO_FORCE = 0, // 电流零输入 GIMBAL_ZERO_FORCE = 0, // 电流零输入
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据? GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式 GIMBAL_MANUAL_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
GIMBAL_IKINE_MODE, // 机械臂运动学逆解模式，输入6d-tof位置 GIMBAL_IKINE_MODE, // 机械臂运动学逆解模式，输入6d-tof位置
GIMBAL_FLOOR_MODE, // 地矿模式
GIMBAL_SILVER_MODE, // 取银矿模式 GIMBAL_SILVER_MODE, // 取银矿模式
GIMBAL_GOLD_MODE, // 取金矿模式 GIMBAL_GOLD_MODE, // 取金矿模式
GIMBAL_STORAGE_MODE, // 放入矿石仓 GIMBAL_STORAGE_MODE, // 放入矿石仓
GIMBAL_FETCH_MODE, // 取出矿石 GIMBAL_FETCH_MODE, // 取出矿石
ADJUST_PITCH_MODE, //找pitch的限位 ADJUST_PITCH_MODE, // 调整pitch零点
} gimbal_mode_e; } gimbal_mode_e;
//气泵模式 //气泵模式
typedef enum typedef enum
@ -171,6 +172,7 @@ typedef struct
float z_add; float z_add;
float quat[4]; //末端姿态四元数 float quat[4]; //末端姿态四元数
float rpy[3]; //末端姿态rpy角
float chassis_rotate_wz; float chassis_rotate_wz;
uint8_t MotorEnableFlag; uint8_t MotorEnableFlag;
@ -210,8 +212,7 @@ typedef struct
typedef struct typedef struct
{ {
attitude_t gimbal_imu_data; attitude_t gimbal_imu_data;
float yaw_motor_single_round_angle; float diff_pitch_feedback;
float pitch_motor_total_angle;
} Gimbal_Upload_Data_s; } Gimbal_Upload_Data_s;
typedef struct typedef struct

5
application/to_stretch/to_stretch.c
View File

@ -162,8 +162,8 @@ void To_stretchInit() {
.controller_setting_init_config = { .controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = ANGLE_LOOP, .outer_loop_type = SPEED_LOOP,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP, .close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = M2006 .motor_type = M2006
@ -248,6 +248,7 @@ void To_stretchTask()
DJIMotorSetRef(motor_ru, -ud_speed_set); DJIMotorSetRef(motor_ru, -ud_speed_set);
DJIMotorSetRef(motor_ld, -fb_speed_set); DJIMotorSetRef(motor_ld, -fb_speed_set);
DJIMotorSetRef(motor_rd, fb_speed_set); DJIMotorSetRef(motor_rd, fb_speed_set);
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
break; break;
case TO_STRETCH_KEEP: case TO_STRETCH_KEEP:
DJIMotorEnable(motor_lu); DJIMotorEnable(motor_lu);

1
engineering.jdebug
View File

@ -33,6 +33,7 @@ void OnProjectLoad (void) {
// //
// User settings // User settings
// //
Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ);
Project.SetOSPlugin ("FreeRTOSPlugin_CM4"); Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf"); File.Open ("$(ProjectDir)/cmake-build-debug/basic_framework.elf");
} }

28
engineering.jdebug.user
View File

@ -2,16 +2,18 @@
Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED Breakpoint=D:/CLion/Project/engineering/application/gimbal/gimbal.cpp:280:5, State=BP_STATE_DISABLED
OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52 GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=221 GraphedExpression="(lift_position_loop).Measure", Color=#35792b
OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141
OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=202
OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.c", Line=9 OpenDocument="referee_UI.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.c", Line=9
OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=70 OpenDocument="main.c", FilePath="D:/CLion/Project/engineering/Src/main.c", Line=70
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=36 OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=270
OpenDocument="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141 OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=108
OpenDocument="referee_UI.h", FilePath="D:/CLion/Project/engineering/modules/referee/referee_UI.h", Line=0 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="stm32f4xx_it.c", FilePath="D:/CLion/Project/engineering/Src/stm32f4xx_it.c", Line=107
OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=213 OpenDocument="gimbal.cpp", FilePath="D:/CLion/Project/engineering/application/gimbal/gimbal.cpp", Line=213
OpenDocument="to_stretch.c", FilePath="D:/CLion/Project/engineering/application/to_stretch/to_stretch.c", Line=9 OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/engineering/Startup/startup_stm32f407ighx.s", Line=52
OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316 OpenDocument="stm32f4xx_hal.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c", Line=316
OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/engineering/bsp/dwt/bsp_dwt.c", Line=29
OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59 OpenDocument="referee_VT.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_VT.c", Line=59
@ -22,16 +24,16 @@ OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/engineering/Middl
OpenDocument="stm32f4xx_hal_can.c", FilePath="D:/CLion/Project/engineering/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c", Line=1412 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 OpenDocument="chassis.c", FilePath="D:/CLion/Project/engineering/application/chassis/chassis.c", Line=165
OpenToolbar="Debug", Floating=0, x=0, y=0 OpenToolbar="Debug", Floating=0, x=0, y=0
OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=367, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=614, h=360, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=264, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=614, h=271, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=687, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=687, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=979, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=979, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, 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="710;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="724;0" OpenWindow="Timeline", DockArea=RIGHT, x=0, y=0, w=611, h=632, 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=252, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=252, h=292, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" 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="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"], ColWidths=[100;100] TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (lift_position_loop).Ref";" (lift_position_loop).Measure"], 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=[341;100;100;100;100;100;100;107;478] TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[341;100;100;110;100;100;100;107;107]
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] 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="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;100;100] TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;100;100]
@ -63,4 +65,8 @@ WatchedExpression="yaw_motor", RefreshRate=2, Window=Watched Data 1
WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1 WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Ref", Window=Watched Data 1
WatchedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Window=Watched Data 1 WatchedExpression="(((yaw_motor)->motor_controller).angle_PID).Ref", Window=Watched Data 1
WatchedExpression="yaw_spd_filter", Window=Watched Data 1 WatchedExpression="yaw_spd_filter", Window=Watched Data 1
WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1 WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1
WatchedExpression="tuchuan", RefreshRate=2, Window=Watched Data 1
WatchedExpression="lift_position_loop", RefreshRate=2, Window=Watched Data 1
WatchedExpression="rc_data[TEMP].key_count[KEY_PRESS][Key_Shift]", Window=Watched Data 1
WatchedExpression="rc_data", Window=Watched Data 1

12
modules/motor/DMmotor/dmmotor.c
View File

@ -39,7 +39,13 @@ static void DMMotorDecode(CANInstance *motor_can)
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id; DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针 DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
DaemonReload(motor->motor_daemon); // DaemonReload(motor->motor_daemon);
//0 失能 1 使能 8 超压 9 欠压 A 过流 B MOS过压 C 电机线圈过温 D 通信丢失 E 过载
tmp = (uint8_t)(rxbuff[0]>>4);
measure->state = tmp;
if(measure->state){
DaemonReload(motor->motor_daemon);
}
measure->last_position = measure->position; measure->last_position = measure->position;
@ -71,6 +77,10 @@ static void DMMotorLostCallback(void *motor_ptr)
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2; uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id); LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
LOGWARNING("[dm_motor] Tring to restart motor, can bus [%d] , id [%d]\"", can_bus, motor->motor_can_instance->tx_id);
} }
void DMMotorCaliEncoder(DMMotorInstance *motor) void DMMotorCaliEncoder(DMMotorInstance *motor)
{ {

4
modules/motor/DMmotor/dmmotor.h
View File

@ -8,8 +8,8 @@
#define DM_MOTOR_CNT 6 #define DM_MOTOR_CNT 6
#define DM_P_MIN (-12.5f)//(-3.1415926f) #define DM_P_MIN (-12.56637)//(-3.1415926f)
#define DM_P_MAX 12.5f//3.1415926f #define DM_P_MAX 12.56637//3.1415926f
#define DM_V_MIN (-45.0f) #define DM_V_MIN (-45.0f)
#define DM_V_MAX 45.0f #define DM_V_MAX 45.0f
#define DM_T_MIN (-18.0f) #define DM_T_MIN (-18.0f)

2
modules/motor/motor_task.c
View File

@ -10,7 +10,7 @@ void MotorControlTask()
// static uint8_t cnt = 0; 设定不同电机的任务频率 // static uint8_t cnt = 0; 设定不同电机的任务频率
// if(cnt%5==0) //200hz // if(cnt%5==0) //200hz
// if(cnt%10==0) //100hz // if(cnt%10==0) //100hz
//DJIMotorControl(); DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
//LKMotorControl(); //LKMotorControl();

161
modules/referee/referee_task.c
View File

@ -54,7 +54,6 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
void UITask() void UITask()
{ {
RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
MyUIRefresh(referee_recv_info, Interactive_data); MyUIRefresh(referee_recv_info, Interactive_data);
xyz_refresh(referee_recv_info, Interactive_data); xyz_refresh(referee_recv_info, Interactive_data);
} }
@ -62,6 +61,7 @@ void UITask()
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次 static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change
static Graph_Data_t UI_State_dyn_graph[2]; static Graph_Data_t UI_State_dyn_graph[2];
static Graph_Data_t UI_angle[2];
#define AXIS_LEN 250 #define AXIS_LEN 250
#define AXIS_X0 550 #define AXIS_X0 550
@ -79,9 +79,16 @@ void MyUIInit()
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
// 绘制车辆状态标志指示 // 绘制车辆状态标志指示
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 20, 2, 10, 750, "F.AIR_PUMP:"); UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 750, "F.AIR_PUMP:");
UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 700, "ARM_MODE:");
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 650, "PITCH:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]); UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
// xyz坐标系 静态 // xyz坐标系 静态
UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0); UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN); UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN);
@ -100,116 +107,75 @@ void MyUIInit()
// 绘制车辆状态标志，动态 // 绘制车辆状态标志，动态
// 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新 // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743); UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_ADD, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 720, 10); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]); UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
} // 底盘功率显示,动态
UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
static uint8_t count = 0;
static uint16_t count1 = 0;
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
{
count++;
if (count >= 50)
{
count = 0;
count1++;
}
switch (count1 % 4)
{
case 0:
{
_Interactive_data->chassis_mode = CHASSIS_ZERO_FORCE;
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
}
case 1:
{
_Interactive_data->chassis_mode = CHASSIS_ROTATE;
_Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
break;
}
case 2:
{
_Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW;
_Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
break;
}
case 3:
{
_Interactive_data->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
break;
}
default:
break;
}
} }
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data) static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{ {
UIChangeCheck(_Interactive_data); UIChangeCheck(_Interactive_data);
// chassis // 显示 pitch 轴编码器位置
// if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1) UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
// { UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
// switch (_Interactive_data->chassis_mode) // gimbal
// { if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
// case CHASSIS_ZERO_FORCE: {
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); switch (_Interactive_data->gimbal_mode)
// break; {
// case CHASSIS_ROTATE: case GIMBAL_ZERO_FORCE:
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate "); {
// // 此处注意字数对齐问题，字数相同才能覆盖掉 UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
// break; break;
// case CHASSIS_NO_FOLLOW: }
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow "); case GIMBAL_MANUAL_MODE:
// break; {
// case CHASSIS_FOLLOW_GIMBAL_YAW: UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "MANUAL ");
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow "); break;
// break; }
// } case GIMBAL_SILVER_MODE:
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); {
// _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0; UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "SILVER ");
// } break;
// // gimbal }
// if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1) case GIMBAL_FLOOR_MODE:
// { {
// switch (_Interactive_data->gimbal_mode) UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "FLOOR ");
// { break;
// case GIMBAL_ZERO_FORCE: }
// { case GIMBAL_IKINE_MODE:
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); {
// break; UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "CUSTOM ");
// } break;
// case GIMBAL_FREE_MODE: }
// { case GIMBAL_STORAGE_MODE:
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free "); {
// break; UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "STORAGE ");
// } break;
// case GIMBAL_GYRO_MODE: }
// { }
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro "); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
// break; _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
// } }
// }
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
// _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
// }
// pump // pump
if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1) if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1)
{ {
switch (_Interactive_data->pump_mode) { switch (_Interactive_data->pump_mode) {
case PUMP_STOP: case PUMP_STOP:
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743); //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 720, 10); UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
break; break;
case PUMP_OPEN: case PUMP_OPEN:
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695); //UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695);
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 720, 25); UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 800, 25);
break; break;
} }
UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]); UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
@ -247,8 +213,8 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data) static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{ {
const float y_max = 100;//单位mm const float y_max = FB_MAX;//单位mm
const float z_max = 100;//单位mm const float z_max = UD_MAX;//单位mm
float y_ratio = _Interactive_data->xyz[1]/y_max; float y_ratio = _Interactive_data->xyz[1]/y_max;
float z_ratio = _Interactive_data->xyz[2]/z_max; float z_ratio = _Interactive_data->xyz[2]/z_max;
@ -256,6 +222,7 @@ static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_i
UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0); UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio)); UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio));
UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio)); UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio));
UIGraphRefresh(&referee_recv_info->referee_id,3,xyz_axis[3],xyz_axis[4],xyz_axis[5]); UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[3],xyz_axis[4]);
UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[5]);
} }

1
modules/referee/rm_referee.h
View File

@ -95,6 +95,7 @@ typedef struct
uint8_t aim_last_fire; uint8_t aim_last_fire;
float xyz[3]; // xyz三轴位置 float xyz[3]; // xyz三轴位置
float diff_pitch_angle;
} Referee_Interactive_info_t; } Referee_Interactive_info_t;