目前问题:末端pitch丢零点 逆解时转动过程超过限位 两条can线概率抽风
This commit is contained in:
parent
c8bb8154f9
commit
dccb283f4f
|
|
@ -68,7 +68,7 @@ custom_control_t CustomControl; //自定义控制器数据
|
|||
|
||||
void RobotCMDInit() {
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
//vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
|
||||
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
referee_VT_data = VTInit(&huart1);
|
||||
|
|
@ -96,7 +96,7 @@ void RobotCMDInit() {
|
|||
};
|
||||
cmd_can_comm = CANCommInit(&comm_conf);
|
||||
#endif // GIMBAL_BOARD
|
||||
gimbal_cmd_send.pitch = 40;
|
||||
//gimbal_cmd_send.pitch = PITCH_MIN;
|
||||
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
}
|
||||
|
|
@ -106,26 +106,26 @@ void RobotCMDInit() {
|
|||
* 单圈绝对角度的范围是0~360,说明文档中有图示
|
||||
*
|
||||
*/
|
||||
static void CalcOffsetAngle() {
|
||||
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||
static float angle;
|
||||
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
||||
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
||||
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
else if (angle > 180.0f + YAW_ALIGN_ANGLE)
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
|
||||
else
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
#else // 小于180度
|
||||
if (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)
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
else
|
||||
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
|
||||
#endif
|
||||
}
|
||||
//static void CalcOffsetAngle() {
|
||||
// // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
|
||||
// static float angle;
|
||||
// angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
|
||||
//#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
|
||||
// if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
|
||||
// chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
// else if (angle > 180.0f + YAW_ALIGN_ANGLE)
|
||||
// chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE - 360.0f;
|
||||
// else
|
||||
// chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
//#else // 小于180度
|
||||
// if (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)
|
||||
// chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
|
||||
// else
|
||||
// chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE + 360.0f;
|
||||
//#endif
|
||||
//}
|
||||
|
||||
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[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;
|
||||
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.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
|
||||
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_MIN_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;
|
||||
} else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动
|
||||
{
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
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.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)) {
|
||||
// 左侧开关状态为[下],前两轴
|
||||
to_stretch_cmd_send.tc = 0;
|
||||
// 左侧开关状态为[下],大臂 + 龙门架 + 底盘平移
|
||||
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_;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_MANUAL_MODE;
|
||||
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
|
||||
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))
|
||||
{
|
||||
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;
|
||||
|
||||
//自定义控制器控制末端姿态
|
||||
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 = 0.001f * (float) rc_data[TEMP].rc.rocker_l1; //增量控制
|
||||
chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
|
||||
}
|
||||
}
|
||||
// 云台软件限位
|
||||
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() {
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000;
|
||||
chassis_cmd_send.wz = (float) rc_data[TEMP].mouse.x / 660 * 4;
|
||||
|
||||
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按键开关气泵
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_G] % 2) //G按键开关气泵
|
||||
{
|
||||
case 0:
|
||||
Pump_stop(air_pump);
|
||||
|
|
@ -275,17 +256,113 @@ static void MouseKeySet() {
|
|||
ui_data.pump_mode = PUMP_OPEN;
|
||||
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:
|
||||
// chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
// gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
// case 1:
|
||||
// gimbal_cmd_send.gimbal_mode = ADJUST_PITCH_MODE; //调整pitch零点
|
||||
// rc_data[TEMP].key_count[KEY_PRESS_WITH_CTRL][Key_X]++;
|
||||
// break;
|
||||
// default:
|
||||
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
// 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);
|
||||
update_ui_data();
|
||||
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
|
||||
CalcOffsetAngle();
|
||||
//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();
|
||||
|
||||
// 云台软件限位
|
||||
// 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(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
|
|
@ -349,6 +442,5 @@ void RobotCMDTask() {
|
|||
#endif // GIMBAL_BOARD
|
||||
PubPushMessage(to_stretch_cmd_pub, (void *) &to_stretch_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
|
||||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
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}; // 机械臂各关节位置
|
||||
robotics::Link link[5];
|
||||
|
|
@ -83,9 +87,9 @@ void GimbalInit()
|
|||
.controller_param_init_config = {
|
||||
.other_speed_feedback_ptr = &yaw_spd,
|
||||
.speed_PID = {
|
||||
.Kp = 3.5f,//5,
|
||||
.Kp = 0.0f,//3.5f,//5,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.02f,
|
||||
.Kd = 0,//0.02f,
|
||||
.MaxOut = 10,
|
||||
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||||
.IntegralLimit = 10.0F,
|
||||
|
|
@ -109,10 +113,7 @@ void GimbalInit()
|
|||
.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 = {
|
||||
.controller_param_init_config = {
|
||||
|
|
@ -120,8 +121,6 @@ void GimbalInit()
|
|||
//.current_feedforward_ptr = &arm_gravity_feedforward,
|
||||
},
|
||||
.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,
|
||||
.close_loop_type = OPEN_LOOP,
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
|
|
@ -131,10 +130,7 @@ void GimbalInit()
|
|||
.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= {
|
||||
.Kp = 2.5f,
|
||||
|
|
@ -160,15 +156,15 @@ void GimbalInit()
|
|||
.controller_param_init_config = {
|
||||
.other_speed_feedback_ptr = &roll_spd,
|
||||
.speed_PID = {
|
||||
.Kp = 0.8f,
|
||||
.Ki = 0.3f,
|
||||
.Kp = 0.2f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.02f,
|
||||
.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,
|
||||
},
|
||||
.angle_PID = {
|
||||
.Kp = 10.0f,
|
||||
.Kp = 25.0f,
|
||||
.Ki = 0.0f,
|
||||
.Kd = 0.0f,
|
||||
.MaxOut = 10,
|
||||
|
|
@ -186,17 +182,14 @@ void GimbalInit()
|
|||
.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 = {
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 0.2f,//0.6f,
|
||||
.Ki = 0,//0.1f,
|
||||
.Kd = 0.02,//0.02f,
|
||||
.Kp = 0.6f,
|
||||
.Ki = 0.1f,
|
||||
.Kd = 0.02f,
|
||||
.MaxOut = 10,
|
||||
.Improve = static_cast<PID_Improvement_e>( PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||||
.IntegralLimit = 10.0F,
|
||||
|
|
@ -213,17 +206,7 @@ void GimbalInit()
|
|||
.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;
|
||||
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_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= {
|
||||
.Kp = 20.0F,//15.0f,
|
||||
.Kp = 20.0f,//20.0F,//15.0f,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 10,
|
||||
|
|
@ -246,16 +234,6 @@ void GimbalInit()
|
|||
};
|
||||
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= {
|
||||
.Kp = 14.0f,
|
||||
.Ki = 0,
|
||||
|
|
@ -266,15 +244,36 @@ void GimbalInit()
|
|||
};
|
||||
PIDInit(&diff_roll_loop,&diff_roll_config);
|
||||
|
||||
PID_Init_Config_s diff_roll_spd_config= {
|
||||
.Kp = 1.0f,
|
||||
.Ki = 1.0f,
|
||||
.Kd = 0.02f,
|
||||
.MaxOut = 10,
|
||||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
||||
.IntegralLimit = 100,
|
||||
};
|
||||
PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
|
||||
|
||||
//需要按id顺序初始化 否则有电机不能正确运行
|
||||
|
||||
//决定放弃yaw轴
|
||||
// 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);
|
||||
|
||||
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.rx_id = 12;
|
||||
|
|
@ -285,8 +284,17 @@ void GimbalInit()
|
|||
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_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()
|
||||
//{
|
||||
// if(gimbal_cmd_recv.MotorEnableFlag)
|
||||
|
|
@ -301,16 +309,16 @@ void GimbalInit()
|
|||
|
||||
/* 机器人云台控制核心任务,后续考虑只保留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()
|
||||
{
|
||||
// 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.Kd = debug_diff_kd;
|
||||
//
|
||||
// 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.Kd = debug_diff_kd;
|
||||
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.Kd = debug_diff_kd;
|
||||
|
||||
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.Kd = debug_diff_kd;
|
||||
// 获取云台控制数据
|
||||
// 后续增加未收到数据的处理
|
||||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||||
|
|
@ -337,8 +345,8 @@ void GimbalTask()
|
|||
/* 控制参数计算 ------------------------------------------------------------------------*/
|
||||
|
||||
//正运动学
|
||||
arm_q[0][0] = yaw_motor->measure.position;
|
||||
arm_q[1][0] = -pitch_motor_l->measure.position;
|
||||
arm_q[0][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;
|
||||
|
|
@ -366,30 +374,25 @@ void GimbalTask()
|
|||
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);
|
||||
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(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);
|
||||
q_set[0] = 0;
|
||||
q_set[1] += gimbal_cmd_recv.pitch * DEGREE_2_RAD;
|
||||
q_set[2] += gimbal_cmd_recv.roll * DEGREE_2_RAD;
|
||||
q_set[3] += gimbal_cmd_recv.diff_pitch * DEGREE_2_RAD;
|
||||
q_set[4] += gimbal_cmd_recv.diff_roll * DEGREE_2_RAD;
|
||||
}
|
||||
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);
|
||||
|
|
@ -397,67 +400,102 @@ void GimbalTask()
|
|||
for (int i = 0; i < 4; ++i) {
|
||||
cmd_quat[i][0] = gimbal_cmd_recv.quat[i];
|
||||
}
|
||||
float trans_rpy_data[3] = {-PI,0,0};
|
||||
Matrixf<3,1> trans_rpy(trans_rpy_data);
|
||||
cmd_R = robotics::quat2r(cmd_quat) * robotics::rpy2r(trans_rpy); //转到基座坐标系下的姿态
|
||||
// T_cmd = robotics::p2t(cmd_xyz);
|
||||
|
||||
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)
|
||||
{
|
||||
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]);
|
||||
|
||||
//后三关节的误差 选用误差小的一组解
|
||||
float err[2] = {0};
|
||||
float err[2][3] = {0};
|
||||
Matrixf<3,1> arm_q3 = arm_q.block<3,1>(2,0);
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
Matrixf<3,1> ik = ik_q3.block<3,1>(0,i);
|
||||
err[i] = (ik - arm_q3).norm();
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
err[i][j] = loop_float_constrain(ik[j][0] - arm_q3[j][0],-PI,PI);
|
||||
}
|
||||
}
|
||||
|
||||
// if (err[1] >= err[0])
|
||||
float err1 = abs(err[1][0]) + abs(err[1][1]) + abs(err[1][2]);
|
||||
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);
|
||||
// else
|
||||
// ik_q3_cmd = ik_q3.block<3,1>(0,1);
|
||||
|
||||
// //选用 roll误差小的一组解
|
||||
if (abs(ik_q3[0][0] - arm_q3[0][0]) <= abs(ik_q3[0][1]- arm_q3[0][0]))
|
||||
// // 选用 roll 误差小的一组解
|
||||
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);
|
||||
else
|
||||
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[1][0] = float_constrain(ik_q3_cmd[1][0],-DIFF_PITCH* DEGREE_2_RAD,DIFF_PITCH* DEGREE_2_RAD);
|
||||
ik_q3_cmd[2][0] = float_constrain(ik_q3_cmd[2][0],-DIFF_ROLL* DEGREE_2_RAD,DIFF_ROLL* DEGREE_2_RAD);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
q_set[i] = ik_q3_cmd[i][0];
|
||||
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;
|
||||
|
||||
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
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -32,11 +32,11 @@ void RobotInit()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
//To_stretchInit();
|
||||
To_stretchInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
//ChassisInit();
|
||||
ChassisInit();
|
||||
#endif
|
||||
|
||||
OSTaskInit(); // 创建基础任务
|
||||
|
|
@ -50,11 +50,11 @@ void RobotTask()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDTask();
|
||||
GimbalTask();
|
||||
//To_stretchTask();
|
||||
To_stretchTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
//ChassisTask();
|
||||
ChassisTask();
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
@ -29,18 +29,18 @@
|
|||
#define DIFF_PITCH 90 //差动手腕关节
|
||||
#define DIFF_ROLL 180
|
||||
|
||||
#define ROLL 90
|
||||
#define PITCH_MIN (-160.0f) //注意此处和坐标系是反的
|
||||
#define ROLL 170
|
||||
#define PITCH_MIN (-174.0f) //注意此处和坐标系是反的
|
||||
#define PITCH_MAX (40.0F)
|
||||
|
||||
#define YAW 60
|
||||
|
||||
//龙门架限位
|
||||
#define UD_MAX 100
|
||||
#define UD_MAX 210
|
||||
#define UD_MIN 0
|
||||
|
||||
#define FB_MIN 0
|
||||
#define FB_MAX 100
|
||||
#define FB_MAX 205
|
||||
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
|
||||
|
|
@ -112,14 +112,15 @@ typedef enum
|
|||
{
|
||||
GIMBAL_ZERO_FORCE = 0, // 电流零输入
|
||||
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_FLOOR_MODE, // 地矿模式
|
||||
GIMBAL_SILVER_MODE, // 取银矿模式
|
||||
GIMBAL_GOLD_MODE, // 取金矿模式
|
||||
GIMBAL_STORAGE_MODE, // 放入矿石仓
|
||||
GIMBAL_FETCH_MODE, // 取出矿石
|
||||
|
||||
ADJUST_PITCH_MODE, //找pitch的限位
|
||||
ADJUST_PITCH_MODE, // 调整pitch零点
|
||||
} gimbal_mode_e;
|
||||
//气泵模式
|
||||
typedef enum
|
||||
|
|
@ -171,6 +172,7 @@ typedef struct
|
|||
float z_add;
|
||||
|
||||
float quat[4]; //末端姿态四元数
|
||||
float rpy[3]; //末端姿态rpy角
|
||||
|
||||
float chassis_rotate_wz;
|
||||
uint8_t MotorEnableFlag;
|
||||
|
|
@ -210,8 +212,7 @@ typedef struct
|
|||
typedef struct
|
||||
{
|
||||
attitude_t gimbal_imu_data;
|
||||
float yaw_motor_single_round_angle;
|
||||
float pitch_motor_total_angle;
|
||||
float diff_pitch_feedback;
|
||||
} Gimbal_Upload_Data_s;
|
||||
|
||||
typedef struct
|
||||
|
|
|
|||
|
|
@ -162,8 +162,8 @@ void To_stretchInit() {
|
|||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = ANGLE_LOOP,
|
||||
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = M2006
|
||||
|
|
@ -248,6 +248,7 @@ void To_stretchTask()
|
|||
DJIMotorSetRef(motor_ru, -ud_speed_set);
|
||||
DJIMotorSetRef(motor_ld, -fb_speed_set);
|
||||
DJIMotorSetRef(motor_rd, fb_speed_set);
|
||||
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
|
||||
break;
|
||||
case TO_STRETCH_KEEP:
|
||||
DJIMotorEnable(motor_lu);
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ 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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,16 +2,18 @@
|
|||
|
||||
|
||||
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
|
||||
OpenDocument="referee_task.c", FilePath="D:/CLion/Project/engineering/modules/referee/referee_task.c", Line=221
|
||||
GraphedExpression="(lift_position_loop).Ref", Color=#e56a6f
|
||||
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="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="controller.c", FilePath="D:/CLion/Project/engineering/modules/algorithm/controller.c", Line=141
|
||||
OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/engineering/application/cmd/robot_cmd.c", Line=270
|
||||
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="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="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="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
|
||||
|
|
@ -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="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=367, 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="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=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="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
|
||||
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"], ColWidths=[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 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;110;100;100;100;107;107]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[257;177;104;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).angle_PID).Ref", 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
|
||||
|
|
@ -39,7 +39,13 @@ static void DMMotorDecode(CANInstance *motor_can)
|
|||
DMMotorInstance *motor = (DMMotorInstance *)motor_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;
|
||||
|
||||
|
|
@ -71,6 +77,10 @@ static void DMMotorLostCallback(void *motor_ptr)
|
|||
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
|
||||
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);
|
||||
|
||||
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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -8,8 +8,8 @@
|
|||
|
||||
#define DM_MOTOR_CNT 6
|
||||
|
||||
#define DM_P_MIN (-12.5f)//(-3.1415926f)
|
||||
#define DM_P_MAX 12.5f//3.1415926f
|
||||
#define DM_P_MIN (-12.56637)//(-3.1415926f)
|
||||
#define DM_P_MAX 12.56637//3.1415926f
|
||||
#define DM_V_MIN (-45.0f)
|
||||
#define DM_V_MAX 45.0f
|
||||
#define DM_T_MIN (-18.0f)
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ void MotorControlTask()
|
|||
// static uint8_t cnt = 0; 设定不同电机的任务频率
|
||||
// if(cnt%5==0) //200hz
|
||||
// if(cnt%10==0) //100hz
|
||||
//DJIMotorControl();
|
||||
DJIMotorControl();
|
||||
|
||||
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
|
||||
//LKMotorControl();
|
||||
|
|
|
|||
|
|
@ -54,7 +54,6 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
|
|||
|
||||
void UITask()
|
||||
{
|
||||
RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
MyUIRefresh(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_dyn[7]; // 机器人状态,动态先add才能change
|
||||
static Graph_Data_t UI_State_dyn_graph[2];
|
||||
static Graph_Data_t UI_angle[2];
|
||||
|
||||
#define AXIS_LEN 250
|
||||
#define AXIS_X0 550
|
||||
|
|
@ -79,9 +79,16 @@ void MyUIInit()
|
|||
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[1]);
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
|
||||
|
||||
// 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[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无法刷新
|
||||
|
||||
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
|
||||
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 720, 10);
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_ADD, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
|
||||
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]);
|
||||
// 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)
|
||||
{
|
||||
UIChangeCheck(_Interactive_data);
|
||||
// chassis
|
||||
// if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
|
||||
// {
|
||||
// switch (_Interactive_data->chassis_mode)
|
||||
// {
|
||||
// case CHASSIS_ZERO_FORCE:
|
||||
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
|
||||
// break;
|
||||
// case CHASSIS_ROTATE:
|
||||
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate ");
|
||||
// // 此处注意字数对齐问题，字数相同才能覆盖掉
|
||||
// break;
|
||||
// case CHASSIS_NO_FOLLOW:
|
||||
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
|
||||
// break;
|
||||
// case CHASSIS_FOLLOW_GIMBAL_YAW:
|
||||
// UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
|
||||
// break;
|
||||
// }
|
||||
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
|
||||
// _Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
|
||||
// }
|
||||
// // gimbal
|
||||
// if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
|
||||
// {
|
||||
// switch (_Interactive_data->gimbal_mode)
|
||||
// {
|
||||
// case GIMBAL_ZERO_FORCE:
|
||||
// {
|
||||
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
|
||||
// break;
|
||||
// }
|
||||
// case GIMBAL_FREE_MODE:
|
||||
// {
|
||||
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free ");
|
||||
// break;
|
||||
// }
|
||||
// case GIMBAL_GYRO_MODE:
|
||||
// {
|
||||
// UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro ");
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
|
||||
// _Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
|
||||
// }
|
||||
// 显示 pitch 轴编码器位置
|
||||
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]);
|
||||
// gimbal
|
||||
if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
|
||||
{
|
||||
switch (_Interactive_data->gimbal_mode)
|
||||
{
|
||||
case GIMBAL_ZERO_FORCE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
|
||||
break;
|
||||
}
|
||||
case GIMBAL_MANUAL_MODE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "MANUAL ");
|
||||
break;
|
||||
}
|
||||
case GIMBAL_SILVER_MODE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "SILVER ");
|
||||
break;
|
||||
}
|
||||
case GIMBAL_FLOOR_MODE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "FLOOR ");
|
||||
break;
|
||||
}
|
||||
case GIMBAL_IKINE_MODE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "CUSTOM ");
|
||||
break;
|
||||
}
|
||||
case GIMBAL_STORAGE_MODE:
|
||||
{
|
||||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "STORAGE ");
|
||||
break;
|
||||
}
|
||||
}
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
|
||||
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
|
||||
}
|
||||
// pump
|
||||
if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1)
|
||||
{
|
||||
switch (_Interactive_data->pump_mode) {
|
||||
case PUMP_STOP:
|
||||
//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;
|
||||
case PUMP_OPEN:
|
||||
//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;
|
||||
}
|
||||
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)
|
||||
{
|
||||
const float y_max = 100;//单位mm
|
||||
const float z_max = 100;//单位mm
|
||||
const float y_max = FB_MAX;//单位mm
|
||||
const float z_max = UD_MAX;//单位mm
|
||||
|
||||
float y_ratio = _Interactive_data->xyz[1]/y_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[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));
|
||||
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]);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -95,6 +95,7 @@ typedef struct
|
|||
|
||||
uint8_t aim_last_fire;
|
||||
float xyz[3]; // xyz三轴位置
|
||||
float diff_pitch_angle;
|
||||
|
||||
} Referee_Interactive_info_t;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue