完成了LKmotor模块的重构，优化了DJIMotor的反馈计算
This commit is contained in:
NeoZng
parent
78cc27ee1a
commit
2f41e67de0
|
|
@ -40,7 +40,8 @@
|
|||
"super_cap.h": "c",
|
||||
"motor_def.h": "c",
|
||||
"quaternionekf.h": "c",
|
||||
"shoot.h": "c"
|
||||
"shoot.h": "c",
|
||||
"lk9025.h": "c"
|
||||
},
|
||||
"C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
|
||||
}
|
||||
|
|
@ -65,13 +65,13 @@ void ChassisInit()
|
|||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.MaxOut = 4000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 1.2,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.MaxOut = 4000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
|
|
@ -83,23 +83,23 @@ void ChassisInit()
|
|||
.motor_type = M3508,
|
||||
};
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 1;
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_lf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 2,
|
||||
chassis_motor_config.can_init_config.tx_id = 3,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 3,
|
||||
chassis_motor_config.can_init_config.tx_id = 1,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_lb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 4,
|
||||
chassis_motor_config.can_init_config.tx_id = 2,
|
||||
chassis_motor_config.controller_setting_init_config.reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
referee_data = RefereeInit(&huart6); //裁判系统初始化
|
||||
referee_data = RefereeInit(&huart6); // 裁判系统初始化
|
||||
|
||||
SuperCap_Init_Config_s cap_conf = {
|
||||
.can_config = {
|
||||
|
|
@ -107,23 +107,23 @@ void ChassisInit()
|
|||
.tx_id = 0x302,
|
||||
.rx_id = 0x301,
|
||||
}};
|
||||
cap = SuperCapInit(&cap_conf); //超级电容初始化
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
|
||||
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
|
||||
#ifdef CHASSIS_BOARD
|
||||
Chassis_IMU_data=INS_Init(); // 底盘IMU初始化
|
||||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
|
||||
CANComm_Init_Config_s comm_conf = {
|
||||
.can_config={
|
||||
.can_handle=&hcan2,
|
||||
.tx_id=0x311,
|
||||
.rx_id=0x312,
|
||||
.can_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 0x311,
|
||||
.rx_id = 0x312,
|
||||
},
|
||||
.recv_data_len=sizeof(Chassis_Ctrl_Cmd_s),
|
||||
.send_data_len=sizeof(Chassis_Upload_Data_s),
|
||||
.recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
|
||||
.send_data_len = sizeof(Chassis_Upload_Data_s),
|
||||
};
|
||||
chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
|
||||
#endif // CHASSIS_BOARD
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
#ifdef ONE_BOARD
|
||||
chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
||||
|
|
@ -137,14 +137,14 @@ void ChassisInit()
|
|||
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
|
||||
/**
|
||||
* @brief 计算每个轮毂电机的输出,正运动学解算
|
||||
* 用宏进行预替换减小开销
|
||||
* 用宏进行预替换减小开销,运动解算具体过程参考教程
|
||||
*/
|
||||
static void MecanumCalculate()
|
||||
{
|
||||
vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
|
||||
vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
|
||||
vt_lb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
|
||||
vt_rb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
|
||||
vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
|
||||
vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -153,7 +153,7 @@ static void MecanumCalculate()
|
|||
*/
|
||||
static void LimitChassisOutput()
|
||||
{
|
||||
// 限制待添加
|
||||
// 功率限制待添加
|
||||
// referee_data->PowerHeatData.chassis_power;
|
||||
// referee_data->PowerHeatData.chassis_power_buffer;
|
||||
|
||||
|
|
@ -170,8 +170,8 @@ static void LimitChassisOutput()
|
|||
*/
|
||||
static void EstimateSpeed()
|
||||
{
|
||||
// 根据电机速度和imu的速度解算
|
||||
// chassis_feedback_data.vx vy wz
|
||||
// 根据电机速度和imu的速度解算,利用加速度计判断是否打滑(如果有)
|
||||
// chassis_feedback_data.vx vy wz =
|
||||
// ...
|
||||
}
|
||||
|
||||
|
|
@ -183,18 +183,18 @@ void ChassisTask()
|
|||
SubGetMessage(chassis_sub, &chassis_cmd_recv);
|
||||
#endif
|
||||
#ifdef CHASSIS_BOARD
|
||||
chassis_cmd_recv=*(Chassis_Ctrl_Cmd_s*)CANCommGet(chasiss_can_comm);
|
||||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode==CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
DJIMotorStop(motor_lf); // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
DJIMotorStop(motor_lf);
|
||||
DJIMotorStop(motor_rf);
|
||||
DJIMotorStop(motor_lb);
|
||||
DJIMotorStop(motor_rb);
|
||||
}
|
||||
else
|
||||
{
|
||||
{ // 正常工作
|
||||
DJIMotorEnable(motor_lf);
|
||||
DJIMotorEnable(motor_rf);
|
||||
DJIMotorEnable(motor_lb);
|
||||
|
|
@ -202,14 +202,13 @@ void ChassisTask()
|
|||
}
|
||||
|
||||
// 根据控制模式设定旋转速度
|
||||
// 后续增加不同状态的过渡模式?
|
||||
switch (chassis_cmd_recv.chassis_mode)
|
||||
{
|
||||
case CHASSIS_NO_FOLLOW:
|
||||
chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
|
||||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW:
|
||||
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角平方为速度输出
|
||||
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角度平方为速度输出
|
||||
break;
|
||||
case CHASSIS_ROTATE:
|
||||
// chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
|
|
@ -220,10 +219,11 @@ void ChassisTask()
|
|||
|
||||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||||
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
|
||||
chassis_vx = chassis_cmd_recv.vx * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
|
||||
chassis_cmd_recv.vy * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
|
||||
chassis_vy = chassis_cmd_recv.vx * arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD) -
|
||||
chassis_cmd_recv.vy * arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
|
||||
static float sin_theta, cos_theta;
|
||||
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
|
||||
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
|
||||
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||||
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||||
|
||||
// 根据控制模式进行正运动学解算,计算底盘输出
|
||||
MecanumCalculate();
|
||||
|
|
@ -243,9 +243,9 @@ void ChassisTask()
|
|||
|
||||
// 推送反馈消息
|
||||
#ifdef ONE_BOARD
|
||||
PubPushMessage(chassis_pub, &chassis_feedback_data);
|
||||
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
|
||||
#endif
|
||||
#ifdef CHASSIS_BOARD
|
||||
CANCommSend(chasiss_can_comm,(void*)&chassis_feedback_data);
|
||||
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
|
||||
#endif // CHASSIS_BOARD
|
||||
}
|
||||
|
|
@ -7,43 +7,42 @@
|
|||
#include "general_def.h"
|
||||
#include "dji_motor.h"
|
||||
|
||||
// 自动将编码器转换成角度值
|
||||
// 私有宏,自动将编码器转换成角度值
|
||||
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF)
|
||||
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF)
|
||||
|
||||
/* gimbal_cmd应用包含的模块实例指针和交互信息存储*/
|
||||
#ifdef GIMBAL_BOARD
|
||||
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
|
||||
#include "can_comm.h"
|
||||
static CANCommInstance *cmd_can_comm; // 双板通信
|
||||
#endif
|
||||
#ifdef ONE_BOARD
|
||||
static Publisher_t *chassis_cmd_pub
|
||||
static Subscriber_t *chassis_feed_sub;
|
||||
#endif // ONE_BOARD
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
|
||||
static Publisher_t *chassis_cmd_pub; // 底盘控制消息发布者
|
||||
static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
|
||||
#endif // ONE_BOARD
|
||||
|
||||
static Chassis_Ctrl_Cmd_s chassis_cmd_send; // 发送给底盘应用的信息,包括控制信息和UI绘制相关
|
||||
static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
|
||||
|
||||
static RC_ctrl_t *rc_data; // 遥控器数据,初始化时返回
|
||||
static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
|
||||
static Vision_Send_s vision_send_data; // 视觉发送数据
|
||||
|
||||
static Publisher_t *gimbal_cmd_pub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息
|
||||
static Subscriber_t *gimbal_feed_sub;
|
||||
static Publisher_t *gimbal_cmd_pub; // 云台控制消息发布者
|
||||
static Subscriber_t *gimbal_feed_sub; // 云台反馈信息订阅者
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; // 传递给云台的控制信息
|
||||
static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息
|
||||
|
||||
static Publisher_t *shoot_cmd_pub;
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
|
||||
static Subscriber_t *shoot_feed_sub;
|
||||
static Publisher_t *shoot_cmd_pub; // 发射控制消息发布者
|
||||
static Subscriber_t *shoot_feed_sub; // 发射反馈信息订阅者
|
||||
static Shoot_Ctrl_Cmd_s shoot_cmd_send; // 传递给发射的控制信息
|
||||
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
|
||||
|
||||
|
||||
|
||||
static Robot_Status_e robot_state;
|
||||
static Robot_Status_e robot_state; // 机器人整体工作状态
|
||||
|
||||
void GimbalCMDInit()
|
||||
{
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意dbus协议串口需加反相器
|
||||
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
|
||||
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
|
||||
|
||||
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
|
|
@ -51,24 +50,24 @@ void GimbalCMDInit()
|
|||
shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
|
||||
shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
|
||||
|
||||
#ifdef ONE_BOARD
|
||||
#ifdef ONE_BOARD // 双板兼容
|
||||
chassis_cmd_pub = PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
||||
chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
|
||||
#endif // ONE_BOARD
|
||||
#ifdef GIMBAL_BOARD
|
||||
CANComm_Init_Config_s comm_conf={
|
||||
.can_config={
|
||||
.can_handle=&hcan1,
|
||||
.tx_id=0x312,
|
||||
.rx_id=0x311,
|
||||
CANComm_Init_Config_s comm_conf = {
|
||||
.can_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 0x312,
|
||||
.rx_id = 0x311,
|
||||
},
|
||||
.recv_data_len=sizeof(Chassis_Upload_Data_s),
|
||||
.send_data_len=sizeof(Chassis_Ctrl_Cmd_s),
|
||||
.recv_data_len = sizeof(Chassis_Upload_Data_s),
|
||||
.send_data_len = sizeof(Chassis_Ctrl_Cmd_s),
|
||||
};
|
||||
cmd_can_comm=CANCommInit(&comm_conf);
|
||||
cmd_can_comm = CANCommInit(&comm_conf);
|
||||
#endif // GIMBAL_BOARD
|
||||
|
||||
robot_state=ROBOT_WORKING; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -98,53 +97,52 @@ static void CalcOffsetAngle()
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief 输入为遥控器(调试时)的模式和控制量设置
|
||||
* @brief 控制输入为遥控器(调试时)的模式和控制量设置
|
||||
*
|
||||
*/
|
||||
static void RemoteControlSet()
|
||||
{
|
||||
// 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
|
||||
if (switch_is_down(rc_data[TEMP].rc.s[0])) // 右侧开关状态[下],底盘跟随云台
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.s[0])) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
|
||||
else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
|
||||
|
||||
// 云台参数,确定云台控制数据
|
||||
if (switch_is_mid(rc_data[TEMP].rc.s[1])) // 左侧开关状态为[中],视觉模式
|
||||
if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
|
||||
{
|
||||
// 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
|
||||
// ...
|
||||
}
|
||||
// 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
|
||||
if (switch_is_down(rc_data[TEMP].rc.s[1]) || vision_recv_data->target_state == NO_TARGET)
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
|
||||
{ // 按照摇杆的输出大小进行角度增量,增益系数需调整
|
||||
gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.joystick[2];
|
||||
gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.joystick[3];
|
||||
gimbal_cmd_send.yaw += 0.0015f * (float)rc_data[TEMP].rc.rocker_l_;
|
||||
gimbal_cmd_send.pitch += 0.002f * (float)rc_data[TEMP].rc.rocker_l1;
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
|
||||
}
|
||||
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整
|
||||
chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.joystick[0];
|
||||
chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.joystick[1];
|
||||
chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_;
|
||||
chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1;
|
||||
|
||||
// 发射参数
|
||||
if (switch_is_up(rc_data[TEMP].rc.s[0])) // 右侧开关状态[上],弹舱打开
|
||||
{// 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
|
||||
}
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
||||
{ // 弹舱舵机控制,待添加servo_motor模块,开启
|
||||
}
|
||||
else
|
||||
; // 弹舱舵机控制,待添加servo_motor模块,关闭
|
||||
// 摩擦轮控制,后续可以根据左侧拨轮的值大小切换射频
|
||||
if (rc_data[TEMP].rc.joystick[4] < -100)
|
||||
if (rc_data[TEMP].rc.dial < -100)
|
||||
shoot_cmd_send.friction_mode = FRICTION_ON;
|
||||
else
|
||||
shoot_cmd_send.friction_mode = FRICTION_OFF;
|
||||
// 拨弹控制,目前固定为连发
|
||||
if (rc_data[TEMP].rc.joystick[4] <-500)
|
||||
if (rc_data[TEMP].rc.dial < -500)
|
||||
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
|
||||
else
|
||||
shoot_cmd_send.load_mode = LOAD_STOP;
|
||||
shoot_cmd_send.shoot_rate=1;
|
||||
shoot_cmd_send.shoot_rate = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -158,21 +156,22 @@ static void MouseKeySet()
|
|||
/**
|
||||
* @brief 紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
|
||||
* '300'待修改成合适的值,或改为开关控制
|
||||
* @todo 后续修改为遥控器离线则电机停止(关闭遥控器急停)
|
||||
*
|
||||
*/
|
||||
static void EmergencyHandler()
|
||||
{
|
||||
// 拨轮的向下拨超过一半,注意向下拨轮是正
|
||||
if (rc_data[TEMP].rc.joystick[4] > 300 || robot_state==ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
// 拨轮的向下拨超过一半,注意向打时下拨轮是正
|
||||
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
|
||||
{
|
||||
robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式
|
||||
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
|
||||
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
|
||||
shoot_cmd_send.shoot_mode = SHOOT_OFF;
|
||||
}
|
||||
if(switch_is_up(rc_data[TEMP].rc.s[0]))
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right))
|
||||
{
|
||||
robot_state = ROBOT_WORKING; // 遥控器右侧开关为[上],恢复正常运行
|
||||
robot_state = ROBOT_READY; // 遥控器右侧开关为[上],恢复正常运行
|
||||
shoot_cmd_send.shoot_mode = SHOOT_ON;
|
||||
}
|
||||
}
|
||||
|
|
@ -181,10 +180,10 @@ void GimbalCMDTask()
|
|||
{
|
||||
// 从其他应用获取回传数据
|
||||
#ifdef ONE_BOARD
|
||||
SubGetMessage(chassis_feed_sub, &chassis_fetch_data);
|
||||
SubGetMessage(chassis_feed_sub, (void*)&chassis_fetch_data);
|
||||
#endif // ONE_BOARD
|
||||
#ifdef GIMBAL_BOARD
|
||||
chassis_fetch_data=*(Chassis_Upload_Data_s*)CANCommGet(cmd_can_comm);
|
||||
chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
|
||||
#endif // GIMBAL_BOARD
|
||||
SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
|
||||
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
|
||||
|
|
@ -192,14 +191,14 @@ void GimbalCMDTask()
|
|||
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过私有变量完成
|
||||
CalcOffsetAngle();
|
||||
|
||||
if (switch_is_down(rc_data[TEMP].rc.s[1])) // 遥控器左侧开关状态为[下],遥控器控制
|
||||
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
|
||||
RemoteControlSet();
|
||||
else if (switch_is_up(rc_data[TEMP].rc.s[1])) // 遥控器左侧开关状态为[上],键盘控制
|
||||
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
|
||||
MouseKeySet();
|
||||
|
||||
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
|
||||
|
||||
// 设置视觉发送数据
|
||||
// 设置视觉发送数据,还需增加加速度和角速度数据
|
||||
vision_send_data.bullet_speed = chassis_fetch_data.bullet_speed;
|
||||
vision_send_data.enemy_color = chassis_fetch_data.enemy_color;
|
||||
vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
|
||||
|
|
@ -209,12 +208,12 @@ void GimbalCMDTask()
|
|||
// 推送消息,双板通信,视觉通信等
|
||||
// 应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
|
||||
#ifdef ONE_BOARD
|
||||
SubGetMessage(chassis_feed_sub, &chassis_fetch_data);
|
||||
PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
|
||||
#endif // ONE_BOARD
|
||||
#ifdef GIMBAL_BOARD
|
||||
CANCommSend(cmd_can_comm,(void*)&chassis_cmd_send);
|
||||
CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
|
||||
#endif // GIMBAL_BOARD
|
||||
PubPushMessage(shoot_cmd_pub, &shoot_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, &gimbal_cmd_send);
|
||||
PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
|
||||
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
|
||||
VisionSend(&vision_send_data);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -9,10 +9,10 @@ static attitude_t *Gimbal_IMU_data; // 云台IMU数据
|
|||
static DJIMotorInstance *yaw_motor; // yaw电机
|
||||
static DJIMotorInstance *pitch_motor; // pitch电机
|
||||
|
||||
static Publisher_t *gimbal_pub;
|
||||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给gimbal_cmd的云台状态信息
|
||||
static Subscriber_t *gimbal_sub;
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自gimbal_cmd的控制信息
|
||||
static Publisher_t *gimbal_pub; // 云台应用消息发布者(云台反馈给cmd)
|
||||
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
|
||||
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
|
||||
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
|
||||
|
||||
void GimbalInit()
|
||||
{
|
||||
|
|
@ -26,17 +26,17 @@ void GimbalInit()
|
|||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 10,
|
||||
.Kp = 20,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.DeadBand=0.3,
|
||||
.DeadBand = 0.3,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
.MaxOut = 4000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
|
||||
// 还需要增加角速度额外反馈指针
|
||||
|
|
@ -62,7 +62,7 @@ void GimbalInit()
|
|||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 4000,
|
||||
.DeadBand=0.3,
|
||||
.DeadBand = 0.3,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 10,
|
||||
|
|
@ -91,15 +91,6 @@ void GimbalInit()
|
|||
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
||||
}
|
||||
|
||||
// /**
|
||||
// * @brief
|
||||
// *
|
||||
// */
|
||||
// static void TransitionMode()
|
||||
// {
|
||||
|
||||
// }
|
||||
|
||||
void GimbalTask()
|
||||
{
|
||||
// 获取云台控制数据
|
||||
|
|
@ -107,7 +98,6 @@ void GimbalTask()
|
|||
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
||||
|
||||
// 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
|
||||
// 是否要增加不同模式之间的过渡?
|
||||
switch (gimbal_cmd_recv.gimbal_mode)
|
||||
{
|
||||
// 停止
|
||||
|
|
@ -140,25 +130,11 @@ void GimbalTask()
|
|||
default:
|
||||
break;
|
||||
}
|
||||
// 过渡示例:
|
||||
/* 需要给每个case增加如下判断,并添加一个过渡行为函数和过渡标志位
|
||||
case xxx:
|
||||
if(last_mode!=xxx)
|
||||
{
|
||||
transition_flag=1;
|
||||
}
|
||||
break;
|
||||
|
||||
void TransitMode()
|
||||
{
|
||||
motor_output=lpf_coef * last_output+(1 - lpf_coef) * concur_output;
|
||||
}
|
||||
*/
|
||||
|
||||
// 设置反馈数据
|
||||
gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data;
|
||||
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
|
||||
|
||||
// 推送消息
|
||||
PubPushMessage(gimbal_pub, &gimbal_feedback_data);
|
||||
// 推送消息
|
||||
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
||||
}
|
||||
|
|
@ -17,13 +17,13 @@
|
|||
#include "stdint-gcc.h"
|
||||
|
||||
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
|
||||
// #define ONE_BOARD // 单板控制整车
|
||||
#define ONE_BOARD // 单板控制整车
|
||||
// #define CHASSIS_BOARD //底盘板
|
||||
#define GIMBAL_BOARD //云台板
|
||||
// #define GIMBAL_BOARD //云台板
|
||||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
|
||||
// 云台参数
|
||||
#define YAW_CHASSIS_ALIGN_ECD 0 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
|
||||
#define YAW_ECD_GREATER_THAN_4096 0 // yaw电机的初始编码器值是否大于4096,是为1,否为0
|
||||
#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
// 发射参数
|
||||
|
|
@ -43,7 +43,7 @@
|
|||
(defined(ONE_BOARD) && defined(GIMBAL_BOARD)) || \
|
||||
(defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD))
|
||||
#error Conflict board definition! You can only define one type.
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#pragma pack(1) // 压缩结构体,取消字节对齐
|
||||
|
||||
|
|
@ -56,7 +56,7 @@
|
|||
typedef enum
|
||||
{
|
||||
ROBOT_STOP,
|
||||
ROBOT_WORKING,
|
||||
ROBOT_READY,
|
||||
} Robot_Status_e;
|
||||
|
||||
// 应用状态
|
||||
|
|
@ -88,13 +88,12 @@ typedef enum
|
|||
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
|
||||
} gimbal_mode_e;
|
||||
|
||||
|
||||
// 发射模式设置
|
||||
typedef enum
|
||||
{
|
||||
SHOOT_ON=0,
|
||||
SHOOT_ON = 0,
|
||||
SHOOT_OFF,
|
||||
}shoot_mode_e;
|
||||
} shoot_mode_e;
|
||||
typedef enum
|
||||
{
|
||||
FRICTION_OFF, // 摩擦轮关闭
|
||||
|
|
|
|||
|
|
@ -32,10 +32,10 @@ void ShootInit()
|
|||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 200,
|
||||
.MaxOut = 2000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 10,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
|
|
@ -61,10 +61,10 @@ void ShootInit()
|
|||
.Kp = 10,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 200,
|
||||
.MaxOut = 2000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 5,
|
||||
.Kp = 1,
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 2000,
|
||||
|
|
@ -200,8 +200,8 @@ void ShootTask()
|
|||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
default:
|
||||
DJIMotorSetRef(friction_l, 200);
|
||||
DJIMotorSetRef(friction_r, 200);
|
||||
DJIMotorSetRef(friction_l, 4000);
|
||||
DJIMotorSetRef(friction_r, 4000);
|
||||
break;
|
||||
} // 关闭摩擦轮
|
||||
if (shoot_cmd_recv.friction_mode==FRICTION_OFF)
|
||||
|
|
@ -210,8 +210,6 @@ void ShootTask()
|
|||
DJIMotorSetRef(friction_r, 0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 开关弹舱盖
|
||||
if (shoot_cmd_recv.lid_mode == LID_CLOSE)
|
||||
{
|
||||
|
|
@ -221,4 +219,7 @@ void ShootTask()
|
|||
{
|
||||
//...
|
||||
}
|
||||
|
||||
// 反馈数据
|
||||
PubPushMessage(shoot_pub,(void*)&shoot_feedback_data);
|
||||
}
|
||||
|
|
@ -23,6 +23,7 @@ typedef struct _
|
|||
uint8_t rx_len; // 接收长度,可能为0-8
|
||||
// 接收的回调函数,用于解析接收到的数据
|
||||
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
|
||||
void* id;
|
||||
} CANInstance;
|
||||
#pragma pack()
|
||||
|
||||
|
|
@ -33,6 +34,7 @@ typedef struct
|
|||
uint32_t tx_id;
|
||||
uint32_t rx_id;
|
||||
void (*can_module_callback)(CANInstance *);
|
||||
void* id;
|
||||
} CAN_Init_Config_s;
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#include "HT04.h"
|
||||
#include "memory.h"
|
||||
|
||||
joint_instance *joint_motor_info[HT_MOTOR_CNT];
|
||||
HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT];
|
||||
|
||||
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
|
||||
{
|
||||
|
|
@ -36,15 +36,15 @@ static void DecodeJoint(CANInstance *motor_instance)
|
|||
}
|
||||
}
|
||||
|
||||
joint_instance *HTMotorInit(CAN_Init_Config_s config)
|
||||
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config)
|
||||
{
|
||||
static uint8_t idx;
|
||||
joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance));
|
||||
joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t));
|
||||
joint_motor_info[idx]->motor_can_instace = CANRegister(&config);
|
||||
return joint_motor_info[idx++];
|
||||
}
|
||||
|
||||
void JointControl(joint_instance *_instance, float current)
|
||||
void JointControl(HKMotor_Measure_t *_instance, float current)
|
||||
{
|
||||
uint16_t tmp;
|
||||
LIMIT_MIN_MAX(current, T_MIN, T_MAX);
|
||||
|
|
@ -54,7 +54,7 @@ void JointControl(joint_instance *_instance, float current)
|
|||
CANTransmit(_instance->motor_can_instace);
|
||||
}
|
||||
|
||||
void SetJointMode(joint_mode cmd, joint_instance *_instance)
|
||||
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance)
|
||||
{
|
||||
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
|
||||
buf[7] = (uint8_t)cmd;
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ typedef struct // HT04
|
|||
|
||||
PIDInstance pid;
|
||||
CANInstance *motor_can_instace;
|
||||
} joint_instance;
|
||||
} HKMotor_Measure_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
|
|
@ -33,10 +33,10 @@ typedef enum
|
|||
CMD_ZERO_POSITION = 0xfe
|
||||
} joint_mode;
|
||||
|
||||
joint_instance *HTMotorInit(CAN_Init_Config_s config);
|
||||
HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config);
|
||||
|
||||
void JointControl(joint_instance *_instance, float current);
|
||||
void JointControl(HKMotor_Measure_t *_instance, float current);
|
||||
|
||||
void SetJointMode(joint_mode cmd, joint_instance *_instance);
|
||||
void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance);
|
||||
|
||||
#endif // !HT04_H#define HT04_H
|
||||
|
|
@ -1,47 +1,75 @@
|
|||
#include "LK9025.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
static driven_instance *driven_motor_info[LK_MOTOR_CNT];
|
||||
static uint8_t idx;
|
||||
static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL};
|
||||
|
||||
static void DecodeDriven(CANInstance *_instance)
|
||||
static void LKMotorDecode(CANInstance *_instance)
|
||||
{
|
||||
for (size_t i = 0; i < LK_MOTOR_CNT; i++)
|
||||
static LKMotor_Measure_t* measure;
|
||||
static uint8_t* rx_buff;
|
||||
rx_buff=_instance->rx_buff;
|
||||
measure=&((LKMotorInstance*)_instance)->measure;
|
||||
|
||||
measure->last_ecd=measure->ecd;
|
||||
measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
|
||||
measure->angle_single_round=ECD_ANGLE_COEF*measure->ecd;
|
||||
measure->speed_aps=(1-SPEED_SMOOTH_COEF)*measure->speed_aps+
|
||||
SPEED_SMOOTH_COEF*(float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
|
||||
measure->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+
|
||||
CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
|
||||
measure->temperate=rx_buff[1];
|
||||
|
||||
//计算多圈角度
|
||||
if (measure->ecd - measure->last_ecd > 32678)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -32678)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
|
||||
void LKMotorControl()
|
||||
{
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
if (driven_motor_info[i]->motor_can_instance == _instance)
|
||||
{
|
||||
driven_motor_info[i]->last_ecd = driven_motor_info[i]->ecd;
|
||||
driven_motor_info[i]->ecd = (uint16_t)((_instance->rx_buff[7] << 8) | _instance->rx_buff[6]);
|
||||
driven_motor_info[i]->speed_rpm = (uint16_t)(_instance->rx_buff[5] << 8 | _instance->rx_buff[4]);
|
||||
driven_motor_info[i]->real_current = (uint16_t)(_instance->rx_buff[3] << 8 | _instance->rx_buff[2]);
|
||||
driven_motor_info[i]->temperate = _instance->rx_buff[1];
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
driven_instance *LKMotroInit(CAN_Init_Config_s config)
|
||||
LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
static uint8_t idx;
|
||||
driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance));
|
||||
config.can_module_callback = DecodeDriven;
|
||||
driven_motor_info[idx]->motor_can_instance = CANRegister(&config);
|
||||
return driven_motor_info[idx++];
|
||||
lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance));
|
||||
memset(lkmotor_instance[idx],0,sizeof(LKMotorInstance));
|
||||
|
||||
lkmotor_instance[idx]->motor_settings=config->controller_setting_init_config;
|
||||
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
|
||||
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
|
||||
PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID);
|
||||
lkmotor_instance[idx]->other_angle_feedback_ptr=config->controller_param_init_config.other_angle_feedback_ptr;
|
||||
lkmotor_instance[idx]->other_speed_feedback_ptr=config->controller_param_init_config.other_speed_feedback_ptr;
|
||||
|
||||
config->can_init_config.can_module_callback=LKMotorDecode;
|
||||
config->can_init_config.rx_id=0x140+config->can_init_config.tx_id;
|
||||
config->can_init_config.tx_id=config->can_init_config.tx_id+0x240;
|
||||
lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config);
|
||||
|
||||
LKMotorEnable(lkmotor_instance[idx]);
|
||||
return lkmotor_instance[idx++];
|
||||
}
|
||||
|
||||
void DrivenControl(int16_t motor1_current, int16_t motor2_current)
|
||||
|
||||
void LKMotorStop(LKMotorInstance *motor)
|
||||
{
|
||||
LIMIT_MIN_MAX(motor1_current, I_MIN, I_MAX);
|
||||
LIMIT_MIN_MAX(motor2_current, I_MIN, I_MAX);
|
||||
driven_motor_info[0]->motor_can_instance->tx_buff[0] = motor1_current;
|
||||
driven_motor_info[0]->motor_can_instance->tx_buff[1] = motor1_current >> 8;
|
||||
driven_motor_info[0]->motor_can_instance->tx_buff[2] = motor2_current;
|
||||
driven_motor_info[0]->motor_can_instance->tx_buff[3] = motor2_current >> 8;
|
||||
CANTransmit(driven_motor_info[0]->motor_can_instance);
|
||||
motor->stop_flag=MOTOR_STOP;
|
||||
}
|
||||
|
||||
void SetDrivenMode(driven_mode cmd, uint16_t motor_id)
|
||||
void LKMotorEnable(LKMotorInstance *motor)
|
||||
{
|
||||
static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
|
||||
// code goes here ...
|
||||
|
||||
// CANTransmit(driven_mode)
|
||||
motor->stop_flag=MOTOR_ENALBED;
|
||||
}
|
||||
|
||||
void LKMotorSetRef(LKMotorInstance *motor, float ref)
|
||||
{
|
||||
motor->pid_ref=ref;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,32 +6,60 @@
|
|||
#include "controller.h"
|
||||
#include "motor_def.h"
|
||||
|
||||
#define LK_MOTOR_CNT 2
|
||||
#define LK_MOTOR_MX_CNT 4
|
||||
|
||||
#define I_MIN -2000
|
||||
#define I_MAX 2000
|
||||
#define CURRENT_SMOOTH_COEF 0.9f
|
||||
#define SPEED_SMOOTH_COEF 0.85f
|
||||
#define REDUCTION_RATIO_DRIVEN 1
|
||||
#define ECD_ANGLE_COEF (360.0f/65536.0f)
|
||||
|
||||
typedef struct // 9025
|
||||
{
|
||||
uint16_t last_ecd;
|
||||
uint16_t ecd;
|
||||
int16_t speed_rpm;
|
||||
int16_t real_current;
|
||||
uint8_t temperate;
|
||||
uint16_t last_ecd;// 上一次读取的编码器值
|
||||
uint16_t ecd; //
|
||||
float angle_single_round; // 单圈角度
|
||||
float speed_aps; // speed angle per sec(degree:°)
|
||||
int16_t real_current; // 实际电流
|
||||
uint8_t temperate; //温度,C°
|
||||
|
||||
PIDInstance *pid;
|
||||
CANInstance *motor_can_instance;
|
||||
float total_angle; // 总角度
|
||||
int32_t total_round; //总圈数
|
||||
|
||||
} driven_instance;
|
||||
} LKMotor_Measure_t;
|
||||
|
||||
typedef enum
|
||||
typedef struct
|
||||
{
|
||||
unused = 0,
|
||||
} driven_mode;
|
||||
LKMotor_Measure_t measure;
|
||||
|
||||
driven_instance *LKMotroInit(CAN_Init_Config_s config);
|
||||
Motor_Control_Setting_s motor_settings;
|
||||
|
||||
void DrivenControl(int16_t motor1_current, int16_t motor2_current);
|
||||
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
|
||||
float *other_speed_feedback_ptr;
|
||||
PIDInstance current_PID;
|
||||
PIDInstance speed_PID;
|
||||
PIDInstance angle_PID;
|
||||
float pid_ref;
|
||||
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
CANInstance* motor_can_ins;
|
||||
|
||||
}LKMotorInstance;
|
||||
|
||||
|
||||
LKMotorInstance *LKMotroInit(Motor_Init_Config_s* config);
|
||||
|
||||
void LKMotorSetRef(LKMotorInstance* motor,float ref);
|
||||
|
||||
void LKMotorControl();
|
||||
|
||||
void LKMotorStop(LKMotorInstance *motor);
|
||||
|
||||
void LKMotorEnable(LKMotorInstance *motor);
|
||||
|
||||
void LKMotorSetRef(LKMotorInstance *motor,float ref);
|
||||
|
||||
void SetDrivenMode(driven_mode cmd, uint16_t motor_id);
|
||||
|
||||
#endif // LK9025_H
|
||||
|
|
|
|||
|
|
@ -123,33 +123,26 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
|||
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
|
||||
static uint8_t *rxbuff;
|
||||
static DJI_Motor_Measure_s *measure;
|
||||
rxbuff = _instance->rx_buff;
|
||||
measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销
|
||||
|
||||
for (size_t i = 0; i < idx; i++)
|
||||
{
|
||||
if (dji_motor_info[i]->motor_can_instance == _instance)
|
||||
{
|
||||
rxbuff = _instance->rx_buff;
|
||||
measure = &dji_motor_info[i]->motor_measure; // measure要多次使用,保存指针减小访存开销
|
||||
uint8_t nice;
|
||||
// resolve data and apply filter to current and speed
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd;
|
||||
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC *
|
||||
SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ;
|
||||
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
|
||||
measure->temperate = rxbuff[6];
|
||||
uint8_t nice;
|
||||
// resolve data and apply filter to current and speed
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd;
|
||||
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC *
|
||||
SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ;
|
||||
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
|
||||
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
|
||||
measure->temperate = rxbuff[6];
|
||||
|
||||
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
|
||||
if (measure->ecd - measure->last_ecd > 4096)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -4096)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
break;
|
||||
}
|
||||
}
|
||||
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
|
||||
if (measure->ecd - measure->last_ecd > 4096)
|
||||
measure->total_round--;
|
||||
else if (measure->ecd - measure->last_ecd < -4096)
|
||||
measure->total_round++;
|
||||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
// 电机初始化,返回一个电机实例
|
||||
|
|
@ -174,6 +167,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
|||
|
||||
// register motor to CAN bus
|
||||
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
|
||||
config->can_init_config.id=dji_motor_info[idx]; //set id,eq to address(it is identity)
|
||||
dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config);
|
||||
|
||||
DJIMotorEnable(dji_motor_info[idx]);
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@
|
|||
#define DJI_MOTOR_CNT 12
|
||||
|
||||
/* 滤波系数设置为1的时候即关闭滤波 */
|
||||
#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.98f // this coef *must* be greater than 0.95
|
||||
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
|
||||
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
|
||||
#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制
|
||||
|
||||
/* DJI电机CAN反馈信息*/
|
||||
|
|
|
|||
|
|
@ -9,22 +9,19 @@ static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按
|
|||
// 遥控器拥有的串口实例
|
||||
static USARTInstance *rc_usart_instance;
|
||||
|
||||
|
||||
/**
|
||||
* @brief 矫正遥控器摇杆的值
|
||||
*
|
||||
*
|
||||
*/
|
||||
static void RectifyRCjoystick()
|
||||
{
|
||||
for (uint8_t i = 0; i < 5; i++)
|
||||
{
|
||||
if(rc_ctrl[TEMP].rc.joystick[i]>660 || rc_ctrl[TEMP].rc.joystick[i]<-660)
|
||||
rc_ctrl[TEMP].rc.joystick[i]=0;
|
||||
if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660)
|
||||
*(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief remote control protocol resolution
|
||||
* @param[in] sbus_buf: raw data point
|
||||
|
|
@ -35,15 +32,15 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
|
|||
{
|
||||
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
|
||||
// 摇杆,直接解算时减去偏置
|
||||
rc_ctrl[TEMP].rc.joystick[0] = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 0
|
||||
rc_ctrl[TEMP].rc.joystick[1] = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 1
|
||||
rc_ctrl[TEMP].rc.joystick[2] = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 2
|
||||
rc_ctrl[TEMP].rc.joystick[3] = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff)- RC_CH_VALUE_OFFSET ; //!< Channel 3
|
||||
rc_ctrl[TEMP].rc.joystick[4] = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF)- RC_CH_VALUE_OFFSET; // 左侧拨轮
|
||||
rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0
|
||||
rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1
|
||||
rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
|
||||
rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
|
||||
rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; // 左侧拨轮
|
||||
RectifyRCjoystick();
|
||||
// 开关,0左1右
|
||||
rc_ctrl[TEMP].rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
|
||||
rc_ctrl[TEMP].rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
|
||||
rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
|
||||
rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
|
||||
|
||||
// 鼠标解析
|
||||
rc_ctrl[TEMP].mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
|
||||
|
|
@ -98,5 +95,5 @@ RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
|
|||
conf.usart_handle = rc_usart_handle;
|
||||
conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
|
||||
rc_usart_instance = USARTRegister(&conf);
|
||||
return (RC_ctrl_t*)&rc_ctrl;
|
||||
return (RC_ctrl_t *)&rc_ctrl;
|
||||
}
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
#include "main.h"
|
||||
#include "usart.h"
|
||||
|
||||
//
|
||||
//
|
||||
#define LAST 1
|
||||
#define TEMP 0
|
||||
|
||||
|
|
@ -27,7 +27,7 @@
|
|||
#define KEY_PRESS_WITH_CTRL 2
|
||||
#define KEY_PRESS_WITH_SHIFT 3
|
||||
|
||||
// 检查接受值是否出错
|
||||
// 检查接收值是否出错
|
||||
#define RC_CH_VALUE_MIN ((uint16_t)364)
|
||||
#define RC_CH_VALUE_OFFSET ((uint16_t)1024)
|
||||
#define RC_CH_VALUE_MAX ((uint16_t)1684)
|
||||
|
|
@ -87,8 +87,14 @@ typedef struct
|
|||
{
|
||||
struct
|
||||
{
|
||||
int16_t joystick[5]; // 右|0 ,右-1 ,左-2 ,左|3 ,拨轮4
|
||||
uint8_t s[2]; //[0]:left [1]:right
|
||||
int16_t rocker_l_; // 左水平
|
||||
int16_t rocker_l1; // 左竖直
|
||||
int16_t rocker_r_; // 右水平
|
||||
int16_t rocker_r1; // 右竖直
|
||||
int16_t dial; // 侧边拨轮
|
||||
|
||||
uint8_t switch_left; // 左侧开关
|
||||
uint8_t switch_right; // 右侧开关
|
||||
} rc;
|
||||
struct
|
||||
{
|
||||
|
|
@ -101,12 +107,10 @@ typedef struct
|
|||
|
||||
uint16_t key_temp;
|
||||
uint8_t key[4][16]; // 当前使用的键盘索引
|
||||
Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
|
||||
// Key_t key_test[4]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
|
||||
|
||||
} RC_ctrl_t;
|
||||
|
||||
|
||||
|
||||
/* ------------------------- Internal Data ----------------------------------- */
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue