碳板步兵底盘基础运动调试完成，新增全向轮运动解算，vofa使用usb虚拟串口发送
This commit is contained in:
parent
eb0d9d8187
commit
fc6f91ac9a
|
|
@ -22,7 +22,7 @@
|
|||
#include "bsp_dwt.h"
|
||||
#include "referee_UI.h"
|
||||
#include "arm_math.h"
|
||||
|
||||
#include "vofa.h"
|
||||
/* 根据robot_def.h中的macro自动计算的参数 */
|
||||
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
|
||||
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
|
||||
|
|
@ -62,15 +62,15 @@ void ChassisInit()
|
|||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 10, // 4.5
|
||||
.Ki = 0, // 0
|
||||
.Kp = 3.0f, // 4.5
|
||||
.Ki = 0.8f, // 0
|
||||
.Kd = 0, // 0
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.MaxOut = 12000,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0.5, // 0.4
|
||||
.Kp = 0.5f, // 0.4
|
||||
.Ki = 0, // 0
|
||||
.Kd = 0,
|
||||
.IntegralLimit = 3000,
|
||||
|
|
@ -82,25 +82,26 @@ void ChassisInit()
|
|||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.close_loop_type = SPEED_LOOP,//| CURRENT_LOOP,
|
||||
},
|
||||
.motor_type = M3508,
|
||||
};
|
||||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||||
chassis_motor_config.can_init_config.tx_id = 1;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
chassis_motor_config.can_init_config.tx_id = 2;
|
||||
chassis_motor_config.controller_setting_init_config.motor_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 = 1;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_rf = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_lb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 3;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rb = DJIMotorInit(&chassis_motor_config);
|
||||
|
||||
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
|
|
@ -151,6 +152,19 @@ static void MecanumCalculate()
|
|||
vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
|
||||
}
|
||||
|
||||
static void OmniCalculate()
|
||||
{
|
||||
vt_rf = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz + chassis_vx - chassis_vy;
|
||||
vt_rb = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz - chassis_vx - chassis_vy;
|
||||
vt_lb = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz - chassis_vx + chassis_vy;
|
||||
vt_lf = -(HALF_TRACK_WIDTH+HALF_WHEEL_BASE)*chassis_cmd_recv.wz + chassis_vx + chassis_vy;
|
||||
|
||||
vt_rf/=(WHEEL_BASE*1.414f);
|
||||
vt_rb/=(WHEEL_BASE*1.414f);
|
||||
vt_lb/=(WHEEL_BASE*1.414f);
|
||||
vt_lf/=(WHEEL_BASE*1.414f);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
|
||||
*
|
||||
|
|
@ -226,16 +240,22 @@ void ChassisTask()
|
|||
// 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
|
||||
// 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
|
||||
static float sin_theta, cos_theta;
|
||||
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
// cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
// sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
sin_theta = 0; cos_theta = 1;
|
||||
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();
|
||||
//MecanumCalculate();
|
||||
OmniCalculate();
|
||||
|
||||
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
|
||||
LimitChassisOutput();
|
||||
float vofa_send_data[2];
|
||||
vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
|
||||
vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
|
||||
vofa_justfloat_output(vofa_send_data,8,&huart1);
|
||||
|
||||
// 根据电机的反馈速度和IMU(如果有)计算真实速度
|
||||
EstimateSpeed();
|
||||
|
|
|
|||
|
|
@ -136,8 +136,8 @@ static void RemoteControlSet()
|
|||
// 云台软件限位
|
||||
|
||||
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
|
||||
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; // 1数值方向
|
||||
chassis_cmd_send.vx = 8.0f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
|
||||
chassis_cmd_send.vy = 8.0f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
|
||||
|
||||
// 发射参数
|
||||
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
|
||||
|
|
|
|||
|
|
@ -31,8 +31,9 @@ void RobotInit()
|
|||
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
ShootInit();
|
||||
//暂时注释云台和发射任务 调试底盘
|
||||
// GimbalInit();
|
||||
// ShootInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
@ -49,8 +50,9 @@ void RobotTask()
|
|||
{
|
||||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDTask();
|
||||
GimbalTask();
|
||||
ShootTask();
|
||||
//暂时注释云台和发射任务 调试底盘
|
||||
// GimbalTask();
|
||||
// ShootTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
|
|||
|
|
@ -35,12 +35,12 @@
|
|||
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
|
||||
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
|
||||
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
|
||||
// 机器人底盘修改的参数,单位为mm(毫米)
|
||||
#define WHEEL_BASE 350 // 纵向轴距(前进后退方向)
|
||||
#define TRACK_WIDTH 300 // 横向轮距(左右平移方向)
|
||||
// 机器人底盘修改的参数,单位为m
|
||||
#define WHEEL_BASE 0.3f // 纵向轴距(前进后退方向)
|
||||
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
|
||||
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
|
||||
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
|
||||
#define RADIUS_WHEEL 60 // 轮子半径
|
||||
#define RADIUS_WHEEL 0.06f // 轮子半径
|
||||
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
|
|
|
|||
|
|
@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf)
|
|||
|
||||
void USBTransmit(uint8_t *buffer, uint16_t len)
|
||||
{
|
||||
CDC_Transmit_FS(buffer, len); // 发送
|
||||
//CDC_Transmit_FS(buffer, len); // 发送
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
* @LastEditTime: 2022-12-05 14:15:53
|
||||
*/
|
||||
#include "vofa.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
|
||||
/*VOFA浮点协议*/
|
||||
void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
|
||||
|
|
@ -29,5 +30,6 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
|
|||
send_data[4 * num + 2] = 0x80;
|
||||
send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴
|
||||
|
||||
HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
|
||||
//HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
|
||||
CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue