增加shift使用超电，UI显示摩擦轮转速（可调）和温度
This commit is contained in:
parent
713a9e1a4d
commit
d45d68e076
|
|
@ -60,9 +60,11 @@ float chassis_power = 0;
|
|||
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
|
||||
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
|
||||
float P_cmdall = 0;
|
||||
|
||||
static uint8_t last_chassis_power_limit=0;
|
||||
void ChassisInit()
|
||||
{
|
||||
|
||||
|
||||
// 四个轮子的参数一样,改tx_id和反转标志位即可
|
||||
Motor_Init_Config_s chassis_motor_config = {
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
|
|
@ -112,16 +114,14 @@ void ChassisInit()
|
|||
motor_rb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
|
||||
|
||||
// SuperCap_Init_Config_s cap_conf = {
|
||||
// .can_config = {
|
||||
// .can_handle = &hcan2,
|
||||
// .tx_id = 0x210,
|
||||
// .rx_id = 0x211,
|
||||
// }};
|
||||
// cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
// SuperCapSetPower(cap,70); // 超级电容限制功率
|
||||
|
||||
SuperCap_Init_Config_s cap_conf = {
|
||||
.can_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 0x210,
|
||||
.rx_id = 0x211,
|
||||
}};
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
SuperCapSetPower(cap,70); // 超级电容限制功率
|
||||
|
||||
// PowerMeter_Init_Config_s power_conf = {
|
||||
// .can_config = {
|
||||
|
|
@ -176,7 +176,6 @@ static void MecanumCalculate()
|
|||
*/
|
||||
static void LimitChassisOutput()
|
||||
{
|
||||
|
||||
float initial_total_power[4]={motor_rf->motor_controller.motor_power_predict ,
|
||||
motor_rb->motor_controller.motor_power_predict ,
|
||||
motor_lb->motor_controller.motor_power_predict ,
|
||||
|
|
@ -188,11 +187,12 @@ static void LimitChassisOutput()
|
|||
continue;
|
||||
P_cmdall += initial_total_power[i];
|
||||
}
|
||||
// float P_cmd = motor_rf->motor_controller.motor_power_predict +
|
||||
// motor_rb->motor_controller.motor_power_predict +
|
||||
// motor_lb->motor_controller.motor_power_predict +
|
||||
// motor_lf->motor_controller.motor_power_predict + 3.6f;
|
||||
float P_max = chassis_cmd_recv.chassis_power_limit ;
|
||||
|
||||
|
||||
float P_max = (float)(chassis_cmd_recv.chassis_power_limit + 5 + chassis_cmd_recv.P_SuperCap) ;
|
||||
|
||||
if(chassis_cmd_recv.chassis_power_buffer<10)//当缓冲功率过小时，限制功率给小；
|
||||
P_max = chassis_cmd_recv.chassis_power_limit;
|
||||
float K = P_max/P_cmdall;
|
||||
|
||||
|
||||
|
|
@ -211,7 +211,19 @@ static void LimitChassisOutput()
|
|||
|
||||
|
||||
}
|
||||
/**
|
||||
* @brief 每次随等级更新超电的设定功率
|
||||
*
|
||||
*/
|
||||
static void SuperCapSetUpdate()
|
||||
{
|
||||
|
||||
if(last_chassis_power_limit != chassis_cmd_recv.chassis_power_limit)
|
||||
{
|
||||
SuperCapSetPower(cap,chassis_cmd_recv.chassis_power_limit); // 超级电容限制功率
|
||||
last_chassis_power_limit = chassis_cmd_recv.chassis_power_limit;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
|
||||
* 对于双板的情况,考虑增加来自底盘板IMU的数据
|
||||
|
|
@ -267,12 +279,16 @@ void ChassisTask()
|
|||
chassis_cmd_recv.wz = 4500;
|
||||
break;
|
||||
case CHASSIS_VERTICAL_YAW:
|
||||
chassis_cmd_recv.offset_angle -= 90;
|
||||
chassis_cmd_recv.offset_angle -= 45;
|
||||
chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
if(chassis_cmd_recv.wz > 4500)
|
||||
chassis_cmd_recv.wz = 4500;
|
||||
if(chassis_cmd_recv.wz < -4500)
|
||||
chassis_cmd_recv.wz = -4500;
|
||||
case CHASSIS_FIXED:
|
||||
chassis_cmd_recv.vx=0;
|
||||
chassis_cmd_recv.vy=0;
|
||||
chassis_cmd_recv.wz = 0;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
|
@ -300,8 +316,8 @@ void ChassisTask()
|
|||
// 根据电机的反馈速度和IMU(如果有)计算真实速度
|
||||
EstimateSpeed();
|
||||
|
||||
//获取底盘功率
|
||||
|
||||
//更新超电设定值
|
||||
SuperCapSetUpdate();
|
||||
|
||||
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||||
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
|
||||
|
|
|
|||
|
|
@ -61,15 +61,15 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
|||
static Robot_Status_e robot_state; // 机器人整体工作状态
|
||||
static referee_info_t* referee_data; // 用于获取裁判系统的数据
|
||||
static referee_info_vt_t* referee_vt_data;
|
||||
float DeltaPitchAngle=0,PitchMotorAngle=0;
|
||||
static float base_HP=12.0f;
|
||||
|
||||
static float DeltaPitchAngle=0,PitchMotorAngle=0;
|
||||
static uint8_t Pitch_Limit_Flag=1;
|
||||
static float Horizontal_Pitch_total_angle=0;
|
||||
void RobotCMDInit()
|
||||
{
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(); // 视觉通信,用的是USB通讯
|
||||
vt_data = VTRefereeInit(&huart1);
|
||||
|
||||
//vt_data = VTRefereeInit(&huart1);
|
||||
//@TODO:找一个可以自由切换图传链路和遥控器控制的逻辑
|
||||
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
||||
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||
|
|
@ -93,6 +93,7 @@ void RobotCMDInit()
|
|||
#endif // GIMBAL_BOARD
|
||||
referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
shoot_cmd_send.motor_speed = 5500;
|
||||
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
}
|
||||
|
|
@ -135,28 +136,32 @@ static void death_check()
|
|||
if(referee_data->GameRobotState.current_HP <= 0)
|
||||
{
|
||||
rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0;
|
||||
gimbal_cmd_send.YawMotorEnableFlag = 5;
|
||||
}
|
||||
}
|
||||
|
||||
static void update_ui_data()
|
||||
{
|
||||
base_HP+=0.01f;
|
||||
if(base_HP >=24)
|
||||
{base_HP=12.0f;}
|
||||
|
||||
ui_data.chassis_mode = chassis_cmd_send.chassis_mode;
|
||||
ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode;
|
||||
ui_data.friction_mode = shoot_cmd_send.friction_mode;
|
||||
ui_data.shoot_mode = shoot_cmd_send.shoot_mode;
|
||||
ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
|
||||
ui_data.Vision_fire = aim_select.suggest_fire;
|
||||
ui_data.Chassis_Power_Data.cap_vol = base_HP;
|
||||
ui_data.Chassis_Power_Data.cap_vol = chassis_fetch_data.cap_vol/100.0f;
|
||||
ui_data.Chassis_Power_Data.motor_speed = shoot_cmd_send.motor_speed;
|
||||
ui_data.Motor_Temperature = shoot_fetch_data.Motor_Temperature;
|
||||
}
|
||||
static void pitch_limit()
|
||||
{
|
||||
PitchMotorAngle = -0.0019f * gimbal_fetch_data.pitch_motor_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度
|
||||
DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;
|
||||
gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
|
||||
if(!Pitch_Limit_Flag)
|
||||
{
|
||||
float Delta_total_angle;//pitch电机自水平位置转过的角度
|
||||
Delta_total_angle = gimbal_fetch_data.pitch_motor_total_angle-Horizontal_Pitch_total_angle;
|
||||
PitchMotorAngle = -0.0019f * Delta_total_angle - 0.4059;//电机旋转导致的云台相对底盘角度
|
||||
DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;//陀螺仪和电机角的误差
|
||||
gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
|
||||
}
|
||||
}
|
||||
static void auto_aim_mode()
|
||||
{
|
||||
|
|
@ -228,14 +233,12 @@ static void RemoteControlSet()
|
|||
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
|
||||
{
|
||||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
|
||||
}
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随云台
|
||||
{
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
shoot_cmd_send.tele_mode = TELE_CLOSE;
|
||||
|
|
@ -266,7 +269,6 @@ static void RemoteControlSet()
|
|||
// 发射参数
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],调整水平pitch参数
|
||||
{
|
||||
gimbal_cmd_send.gimbal_mode = ADJUST_PITCH_MODE;
|
||||
shoot_cmd_send.tele_mode = TELE_OPEN;
|
||||
}// 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
else
|
||||
|
|
@ -277,13 +279,15 @@ static void RemoteControlSet()
|
|||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
else
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
;
|
||||
//shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
|
||||
if (rc_data[TEMP].rc.dial < -500)
|
||||
shoot_cmd_send.load_mode = LOAD_1_BULLET;
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
|
||||
shoot_cmd_send.shoot_rate = 300;
|
||||
shoot_cmd_send.shoot_rate = 500;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -292,8 +296,8 @@ static void RemoteControlSet()
|
|||
*/
|
||||
static void MouseKeySet()
|
||||
{
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 30000 - rc_data[TEMP].key[KEY_PRESS].d * 30000; // 系数待测
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 30000 - rc_data[TEMP].key[KEY_PRESS].s * 30000;
|
||||
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 60000 - rc_data[TEMP].key[KEY_PRESS].d * 60000; // 系数待测
|
||||
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 60000 - rc_data[TEMP].key[KEY_PRESS].s * 60000;
|
||||
|
||||
gimbal_cmd_send.yaw -= (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
|
||||
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 *5 ;
|
||||
|
|
@ -335,7 +339,7 @@ static void MouseKeySet()
|
|||
switch (rc_data[TEMP].key[KEY_PRESS].v) // V键开启连发//
|
||||
{
|
||||
case 0:
|
||||
shoot_cmd_send.shoot_rate= 300;
|
||||
shoot_cmd_send.shoot_rate= 500;
|
||||
break;
|
||||
case 1:
|
||||
shoot_cmd_send.shoot_rate = 50;
|
||||
|
|
@ -351,6 +355,24 @@ static void MouseKeySet()
|
|||
default:
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_B] % 2) // B键加100转速
|
||||
{
|
||||
case 1:
|
||||
shoot_cmd_send.motor_speed +=100;
|
||||
rc_data[TEMP].key_count[KEY_PRESS][Key_B]++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 2) // C键减100转速
|
||||
{
|
||||
case 1:
|
||||
shoot_cmd_send.motor_speed -=100;
|
||||
rc_data[TEMP].key_count[KEY_PRESS][Key_C]++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
|
||||
{
|
||||
case 0:
|
||||
|
|
@ -360,12 +382,12 @@ static void MouseKeySet()
|
|||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关摩擦轮
|
||||
switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关倍镜
|
||||
{
|
||||
case 0:
|
||||
shoot_cmd_send.tele_mode = TELE_CLOSE;
|
||||
break;
|
||||
default:
|
||||
case 1:
|
||||
shoot_cmd_send.tele_mode = TELE_OPEN;
|
||||
break;
|
||||
}
|
||||
|
|
@ -380,29 +402,16 @@ static void MouseKeySet()
|
|||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
break;
|
||||
}
|
||||
switch (rc_data[TEMP].key[KEY_PRESS].z) // Z键侧身用于瞄准
|
||||
switch (rc_data[TEMP].key[KEY_PRESS].shift) // V键开启连发//
|
||||
{
|
||||
case 0:
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
chassis_cmd_send.P_SuperCap=0;
|
||||
break;
|
||||
case 1:
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_VERTICAL_YAW;
|
||||
chassis_cmd_send.P_SuperCap=195;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
|
||||
{
|
||||
case 1:
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
pitch_limit();
|
||||
death_check();
|
||||
}
|
||||
/**
|
||||
* @brief 输入为（图传链路）键鼠时模式和控制量设置
|
||||
|
|
@ -454,7 +463,7 @@ static void VTMouseKeySet()
|
|||
switch (vt_data[TEMP].key[KEY_PRESS].v) // V键开启连发//
|
||||
{
|
||||
case 0:
|
||||
shoot_cmd_send.shoot_rate= 300;
|
||||
shoot_cmd_send.shoot_rate= 500;
|
||||
break;
|
||||
case 1:
|
||||
shoot_cmd_send.shoot_rate = 50;
|
||||
|
|
@ -529,7 +538,7 @@ static void VTMouseKeySet()
|
|||
}
|
||||
|
||||
pitch_limit();
|
||||
death_check();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -556,8 +565,13 @@ static void EmergencyHandler()
|
|||
// 遥控器右侧开关为[上],恢复正常运行
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
|
||||
robot_state = ROBOT_READY;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
|
||||
|
||||
LOGINFO("[CMD] reinstate, robot ready");
|
||||
}
|
||||
}
|
||||
|
|
@ -584,8 +598,13 @@ void RobotCMDTask()
|
|||
RemoteControlSet();
|
||||
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||
{
|
||||
//MouseKeySet();
|
||||
VTMouseKeySet();
|
||||
MouseKeySet();
|
||||
if(Pitch_Limit_Flag)
|
||||
{
|
||||
Pitch_Limit_Flag = 0;
|
||||
Horizontal_Pitch_total_angle = gimbal_fetch_data.pitch_motor_total_angle;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
|
@ -595,6 +614,7 @@ void RobotCMDTask()
|
|||
//根据裁判系统反馈确定底盘功率上限和缓冲功率
|
||||
chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
|
||||
chassis_cmd_send.chassis_power_buffer = referee_data->PowerHeatData.buffer_energy;
|
||||
death_check();
|
||||
// 推送消息,双板通信,视觉通信等
|
||||
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
|
||||
#ifdef ONE_BOARD
|
||||
|
|
@ -605,5 +625,4 @@ void RobotCMDTask()
|
|||
#endif // GIMBAL_BOARD
|
||||
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
|
||||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,12 +30,11 @@ void GimbalInit()
|
|||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 0.5f,//0.5, // 8
|
||||
.Kp = 2.0f,//0.5, // 8
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.Kd = 0.02f,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,
|
||||
|
||||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
|
|
@ -67,15 +66,15 @@ void GimbalInit()
|
|||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 0.3f,//20, // 65
|
||||
.Kp = 0.35f,//20, // 65
|
||||
.Ki = 0,
|
||||
.Kd = 0,//1,
|
||||
.Kd = 0.005f,//1,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,//100
|
||||
.MaxOut = 500,//500
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 3500,//11000, // 50
|
||||
.Kp = 6000,//11000, // 50
|
||||
.Ki = 1000, // 350
|
||||
.Kd = 0,//15000, // 0
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
|
|
@ -96,6 +95,7 @@ void GimbalInit()
|
|||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
|
||||
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
|
||||
yaw_motor = DMMotorInit(&yaw_config);
|
||||
pitch_motor = DJIMotorInit(&pitch_config);
|
||||
|
|
@ -104,12 +104,21 @@ void GimbalInit()
|
|||
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
static void YawMotorEnable(void)
|
||||
{
|
||||
if(gimbal_cmd_recv.YawMotorEnableFlag)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_MOTOR_MODE, yaw_motor);
|
||||
gimbal_cmd_recv.YawMotorEnableFlag--;
|
||||
}
|
||||
}
|
||||
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
|
||||
void GimbalTask()
|
||||
{
|
||||
// 获取云台控制数据
|
||||
// 后续增加未收到数据的处理
|
||||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||||
YawMotorEnable();
|
||||
// @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
|
||||
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
|
||||
switch (gimbal_cmd_recv.gimbal_mode)
|
||||
|
|
|
|||
|
|
@ -29,6 +29,8 @@ void RobotInit()
|
|||
|
||||
BSPInit();
|
||||
|
||||
|
||||
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
|
|
@ -39,6 +41,7 @@ void RobotInit()
|
|||
ChassisInit();
|
||||
#endif
|
||||
|
||||
|
||||
OSTaskInit(); // 创建基础任务
|
||||
|
||||
// 初始化完成,开启中断
|
||||
|
|
|
|||
|
|
@ -26,8 +26,8 @@
|
|||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 2.812502 // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define YAW_CHASSIS_ALIGN_ECD 4.386964 // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
|
||||
#define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 13 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MIN_ANGLE -35 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
|
|
@ -87,6 +87,7 @@ typedef enum
|
|||
CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移
|
||||
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
|
||||
CHASSIS_VERTICAL_YAW,
|
||||
CHASSIS_FIXED,
|
||||
} chassis_mode_e;
|
||||
|
||||
// 云台模式设置
|
||||
|
|
@ -112,8 +113,8 @@ typedef enum
|
|||
|
||||
typedef enum
|
||||
{
|
||||
TELE_OPEN = 0, // 弹舱盖打开
|
||||
TELE_CLOSE, // 弹舱盖关闭
|
||||
TELE_CLOSE = 0, // 倍镜打开
|
||||
TELE_OPEN, // 倍镜关闭
|
||||
} tele_mode_e;
|
||||
|
||||
typedef enum
|
||||
|
|
@ -130,6 +131,7 @@ typedef struct
|
|||
{ // 功率控制
|
||||
uint16_t chassis_power_mx;
|
||||
float cap_vol;
|
||||
uint16_t motor_speed;
|
||||
} Chassis_Power_Data_s;
|
||||
|
||||
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
|
||||
|
|
@ -148,6 +150,7 @@ typedef struct
|
|||
chassis_mode_e chassis_mode;
|
||||
int chassis_power_buffer;
|
||||
uint16_t chassis_power_limit;
|
||||
uint8_t P_SuperCap;
|
||||
// UI部分
|
||||
// ...
|
||||
|
||||
|
|
@ -159,6 +162,8 @@ typedef struct
|
|||
float yaw;
|
||||
float pitch;
|
||||
float chassis_rotate_wz;
|
||||
uint8_t YawMotorEnableFlag;
|
||||
|
||||
|
||||
gimbal_mode_e gimbal_mode;
|
||||
} Gimbal_Ctrl_Cmd_s;
|
||||
|
|
@ -173,6 +178,8 @@ typedef struct
|
|||
float bullet_speed; // 发射周期
|
||||
uint8_t rest_heat;
|
||||
float shoot_rate; // 连续发射的射频,unit per s,发/秒
|
||||
uint16_t motor_speed;
|
||||
|
||||
} Shoot_Ctrl_Cmd_s;
|
||||
|
||||
/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
|
||||
|
|
@ -209,6 +216,7 @@ typedef struct
|
|||
// code to go here
|
||||
// ...
|
||||
uint8_t stalled_flag; //堵转标志
|
||||
uint8_t Motor_Temperature;
|
||||
} Shoot_Upload_Data_s;
|
||||
|
||||
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ void OSTaskInit()
|
|||
osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
|
||||
uiTaskHandle = osThreadCreate(osThread(uitask), NULL);
|
||||
|
||||
HTMotorControlInit(); // 没有注册HT电机则不会执行
|
||||
DMMotorControlInit(); // 没有注册DM电机则不会执行
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@ static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信
|
|||
float Kp=2;
|
||||
float Ki=1;
|
||||
float Kd=0;
|
||||
|
||||
static float stop_location;
|
||||
// dwt定时,计算冷却用
|
||||
static float hibernate_time = 0, dead_time = 0;
|
||||
|
|
@ -82,13 +81,13 @@ void ShootInit()
|
|||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 50, // 10
|
||||
.Kp = 70, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.Kd = 0.01f,
|
||||
.MaxOut = 15000,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 2.4f, // 10
|
||||
.Kp = 3.8f, // 10
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
|
|
@ -237,9 +236,11 @@ void ShootTask()
|
|||
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
|
||||
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
|
||||
{
|
||||
// DJIMotorSetRef(friction_l, 39000);
|
||||
DJIMotorSetRef(friction_r, 36000);
|
||||
DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500
|
||||
|
||||
shoot_cmd_recv.motor_speed = LIMIT_MIN_MAX(shoot_cmd_recv.motor_speed,4000,8500);
|
||||
DJIMotorSetRef(friction_r, (float)(6*shoot_cmd_recv.motor_speed));
|
||||
DJIMotorSetRef(friction_z, (float)(6*shoot_cmd_recv.motor_speed));
|
||||
// DJIMotorSetRef(friction_l, (float)(6*shoot_cmd_recv.motor_speed));
|
||||
}
|
||||
else // 关闭摩擦轮
|
||||
{
|
||||
|
|
@ -251,17 +252,13 @@ void ShootTask()
|
|||
// 开关弹舱盖
|
||||
if (shoot_cmd_recv.tele_mode == TELE_CLOSE)
|
||||
{
|
||||
DJIMotorOuterLoop(telescope, ANGLE_LOOP);
|
||||
DJIMotorSetRef(telescope, 0);
|
||||
}
|
||||
else if (shoot_cmd_recv.tele_mode == TELE_OPEN)
|
||||
{
|
||||
DJIMotorOuterLoop(telescope, ANGLE_LOOP);
|
||||
DJIMotorSetRef(telescope, 1620);
|
||||
}
|
||||
|
||||
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
|
||||
|
||||
shoot_feedback_data.Motor_Temperature = friction_r->measure.temperature;
|
||||
//推送消息
|
||||
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
|
||||
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
|||
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
|
||||
}
|
||||
|
||||
static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
|
||||
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
|
||||
{
|
||||
memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
|
||||
motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
|
||||
|
|
@ -75,7 +75,7 @@ static void DMMotorLostCallback(void *motor_ptr)
|
|||
void DMMotorCaliEncoder(DMMotorInstance *motor)
|
||||
{
|
||||
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
|
||||
DWT_Delay(0.1);
|
||||
DWT_Delay(0.1f);
|
||||
}
|
||||
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref);
|
|||
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
|
||||
|
||||
void DMMotorEnable(DMMotorInstance *motor);
|
||||
|
||||
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
|
||||
void DMMotorStop(DMMotorInstance *motor);
|
||||
void DMMotorCaliEncoder(DMMotorInstance *motor);
|
||||
void DMMotorControlInit();
|
||||
|
|
|
|||
|
|
@ -79,9 +79,11 @@ void MyUIInit()
|
|||
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
|
||||
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
|
||||
UILineDraw(&UI_shoot_line[5], "sl5", UI_Graph_ADD, 7, UI_Color_Yellow, 2, shoot_line_location[5], 200, shoot_line_location[5], 800);
|
||||
|
||||
UIFloatDraw(&UI_shoot_line[6], "sl6", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 2, 650, 440, 5500000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 7, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4], UI_shoot_line[5],UI_shoot_line[6]);
|
||||
|
||||
UIFloatDraw(&UI_shoot_line[7], "sl7", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 2, 850, 440, 26000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id,1, UI_shoot_line[7]);
|
||||
|
||||
// 绘制车辆状态标志指示
|
||||
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
|
||||
|
|
@ -91,6 +93,7 @@ void MyUIInit()
|
|||
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
|
||||
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "frict:");
|
||||
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
|
||||
|
||||
|
||||
|
|
@ -104,6 +107,7 @@ void MyUIInit()
|
|||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
|
||||
UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
|
||||
|
||||
// UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open ");
|
||||
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
|
||||
|
||||
|
|
@ -263,6 +267,20 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
|
|||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[2]);
|
||||
_Interactive_data->Referee_Interactive_Flag.Cap_flag = 0;
|
||||
}
|
||||
//motor_speed
|
||||
if (_Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag == 1)
|
||||
{
|
||||
UIFloatDraw(&UI_shoot_line[6], "sl6", UI_Graph_Change, 7, UI_Color_Green, 18, 2, 2, 650, 440, _Interactive_data->Chassis_Power_Data.motor_speed * 1000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[6]);
|
||||
_Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag = 0;
|
||||
}
|
||||
//Motor_Temperature
|
||||
if (_Interactive_data->Referee_Interactive_Flag.Temperature_flag == 1)
|
||||
{
|
||||
UIFloatDraw(&UI_shoot_line[7], "sl7", UI_Graph_Change, 7, UI_Color_Green, 18, 2, 2, 850, 440, _Interactive_data->Motor_Temperature*1000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id,1, UI_shoot_line[7]);
|
||||
_Interactive_data->Referee_Interactive_Flag.Temperature_flag = 0;
|
||||
}
|
||||
|
||||
//line
|
||||
if (_Interactive_data->Referee_Interactive_Flag.Vision_flag == 1)
|
||||
|
|
@ -338,4 +356,14 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
|
|||
_Interactive_data->Referee_Interactive_Flag.Vision_flag = 1;
|
||||
_Interactive_data->last_Vision_fire = _Interactive_data->Vision_fire;
|
||||
}
|
||||
if (_Interactive_data->Chassis_Power_Data.motor_speed != _Interactive_data->Chassis_last_Power_Data.motor_speed)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.MotorSpeed_flag = 1;
|
||||
_Interactive_data->Chassis_last_Power_Data.motor_speed = _Interactive_data->Chassis_Power_Data.motor_speed;
|
||||
}
|
||||
if (_Interactive_data->Motor_Temperature != _Interactive_data->Last_Motor_Temperature)
|
||||
{
|
||||
_Interactive_data->Referee_Interactive_Flag.Temperature_flag = 1;
|
||||
_Interactive_data->Last_Motor_Temperature = _Interactive_data->Motor_Temperature;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -57,6 +57,8 @@ typedef struct
|
|||
uint32_t Power_flag : 1;
|
||||
uint32_t Cap_flag : 1;
|
||||
uint32_t Vision_flag :1;
|
||||
uint32_t MotorSpeed_flag:1;
|
||||
uint32_t Temperature_flag:1;
|
||||
} Referee_Interactive_Flag_t;
|
||||
|
||||
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
|
||||
|
|
@ -68,9 +70,10 @@ typedef struct
|
|||
gimbal_mode_e gimbal_mode; // 云台模式
|
||||
shoot_mode_e shoot_mode; // 发射模式设置
|
||||
friction_mode_e friction_mode; // 摩擦轮关闭
|
||||
tele_mode_e tele_mode; // 弹舱盖打开
|
||||
tele_mode_e tele_mode; // 倍镜打开关闭
|
||||
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
|
||||
uint8_t Vision_fire;
|
||||
uint8_t Motor_Temperature;
|
||||
|
||||
|
||||
// 上一次的模式，用于flag判断
|
||||
|
|
@ -81,6 +84,7 @@ typedef struct
|
|||
tele_mode_e tele_last_mode;
|
||||
Chassis_Power_Data_s Chassis_last_Power_Data;
|
||||
uint8_t last_Vision_fire;
|
||||
uint8_t Last_Motor_Temperature;
|
||||
|
||||
} Referee_Interactive_info_t;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue