RM学习
CAN控制 电机学习
- 电机结构体
typedef struct
{
uint16_t ecd;//转子机械角度
int16_t speed_rpm;//转速
int16_t given_current;//实际转矩电流
uint8_t temperate;//电机温度
int16_t last_ecd;//上次转子机械角度
} motor_measure_t;
对于不同的电机会有不同的ID号码在解码中需要根据ID号进行解码
电机编号, 0:底盘电机1号3508电机, 1:底盘电机2号3508电机,2:底盘电机3号3508电机,3:底盘电机4号3508电机;
4:yaw云台电机6020电机; 5:pitch云台电机6020电机; 6:拨弹电机2006电机
2. CAN数据
掌握底盘控制代码
- 初始化
chassis_init(&chassis_move);
初始化电机以及PID - 检查是否掉线,若掉线将会重连
while (toe_is_error(CHASSIS_MOTOR1_TOE) ||toe_is_error(CHASSIS_MOTOR2_TOE) || toe_is_error(CHASSIS_MOTOR3_TOE) || toe_is_error(CHASSIS_MOTOR4_TOE) || toe_is_error(DBUS_TOE))
- 设置底盘工作方式并在模式切换保存数据
chassis_set_modechassis_mode_change_control_transit
(i//底盘会跟随云台相对角度(主要方式)ii//底盘有底盘角度控制闭环iii//底盘有旋转速度控制iiii//底盘速度直接由输入电流控制) - 更新一次数据
chassis_feedback_update
- 通过底盘设定方式算出
Vx_Set,Vy_Set,WZ_set
(选择模式i需要将底盘坐标轴转化到云台坐标轴)
具体转换公式说明可看底盘控制
chassis_move_control->vx_set = cos_yaw * vx_set + sin_yaw * vy_set;
chassis_move_control->vy_set = -sin_yaw * vx_set + cos_yaw * vy_set;
- 根据算出的设定值与真实值进行相关计算并更新到底盘电机结构体
chassis_control_loop
- 检查电机是否在线
toe_is_error
- 发送CAN命令控制底盘
CAN_cmd_chassis
掌握云台控制代码
- 初始化云台射击
gimbal_init(&gimbal_control);
shoot_init();
- 检查是否掉线,若掉线将会重连
while (toe_is_error(YAW_GIMBAL_MOTOR_TOE) || toe_is_error(PITCH_GIMBAL_MOTOR_TOE))
- 设置云台工作模式并在模式切换保存数据(与底盘识别方法近乎一致)
行为模式
控制模式
- 通过云台设定方式算出
- 算出云台P和Y角的增量
gimbal_behaviour_control_set(&add_yaw_angle, &add_pitch_angle, set_control);
- 设置P,Y的设置值
- 算出云台P和Y角的增量
- 根据算出的设定值与真实值进行相关计算并更新到云台电机结构体
- 检测电机是否在线
toe_is_error
- 发送CAN命令控制底盘
CAN_cmd_gimbal
掌握姿态解算代码
- 初始化姿态传感器与磁力计
BMI088_init() ist8310_init()
- 通过姿态传感器读取欧拉角,加速度与传感器温度
BMI088_read(bmi088_real_data.gyro, bmi088_real_data.accel, &bmi088_real_data.temp);
具体实现可看代码以及添加的注释 - 计算零漂并调整,减少误差并将调整之后的值赋予INS_gyro(角速度),INS_accel(角加速度),INS_mag(磁场)
imu_cali_slove(INS_gyro, INS_accel, INS_mag, &bmi088_real_data, &ist8310_real_data);
- 温度PID以及四元数初始化
PID_init(&imu_temp_pid, PID_POSITION, imu_temp_PID, TEMPERATURE_PID_MAX_OUT, TEMPERATURE_PID_MAX_IOUT); AHRS_init(INS_quat, INS_accel, INS_mag);
- 根据标志位调度SPI传输
//查询哪个已经准备完毕
if(gyro_update_flag & (1 << IMU_NOTIFY_SHFITS))//角速度准备完毕
{
gyro_update_flag &= ~(1 << IMU_NOTIFY_SHFITS);//清楚标志位
BMI088_gyro_read_over(gyro_dma_rx_buf + BMI088_GYRO_RX_BUF_DATA_OFFSET, bmi088_real_data.gyro);
}
if(accel_update_flag & (1 << IMU_UPDATE_SHFITS))//角加速度准备完毕
{
accel_update_flag &= ~(1 << IMU_UPDATE_SHFITS);//清楚标志位
BMI088_accel_read_over(accel_dma_rx_buf + BMI088_ACCEL_RX_BUF_DATA_OFFSET, bmi088_real_data.accel, &bmi088_real_data.time);
}
if(accel_temp_update_flag & (1 << IMU_UPDATE_SHFITS))//温度准备完毕
{
accel_temp_update_flag &= ~(1 << IMU_UPDATE_SHFITS);//清楚标志位
BMI088_temperature_read_over(accel_temp_dma_rx_buf + BMI088_ACCEL_RX_BUF_DATA_OFFSET, &bmi088_real_data.temp);
imu_temp_control(bmi088_real_data.temp);
}
具体中断函数可看该文件中的回调函数void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
在SPI的DMA运输中会有一个txbuffer负责spi对传感器各个模块的初始化,一开始我也找不到为什么不用初始化后面阅读了spi的dma传输的相关文档后搞清楚了这几个问题
6. 传输完成后唤醒主任务并更新欧拉角
AHRS_update(INS_quat, timing_time, INS_gyro, accel_fliter_3, INS_mag);
get_angle(INS_quat, INS_angle + INS_YAW_ADDRESS_OFFSET, INS_angle + INS_PITCH_ADDRESS_OFFSET, INS_angle + INS_ROLL_ADDRESS_OFFSET);
//获取欧拉角
了解控制器的使用
- 控制器结构体并初始化
typedef __packed struct
{
__packed struct
{
int16_t ch[5];
char s[2];
} rc;
__packed struct
{
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
} mouse;
__packed struct
{
uint16_t v;
} key;
} RC_ctrl_t;
通过串口3获得控制器数据
RC_Init(sbus_rx_buf[0], sbus_rx_buf[1], SBUS_RX_BUF_NUM);
值得注意的是在初始化前需要关闭dma因为不关闭dma将无法改写dma的相关设置导致初始化失败具体可看相关学习文档
2. 串口3中断后获得数据并重新设定缓冲区
//获取接收数据长度,长度 = 设定长度 - 剩余长度
this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;
//重新设定数据长度
hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;
//设定缓冲区1
hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;
3. 校验数据是否为一帧,若是则开始翻译
if(this_time_rx_len == RC_FRAME_LENGTH)
{
sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
//记录数据接收时间
detect_hook(DBUS_TOE);
sbus_to_usart1(sbus_rx_buf[0]);
}
- 翻译数据
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
{
if (sbus_buf == NULL || rc_ctrl == NULL)
{
return;
}
rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //将第一帧与第二帧组成16位数据后再截取前11位下面的步骤也类似
rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1
rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
(sbus_buf[4] << 10)) &0x07ff;
rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3
rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis
rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis
rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL
// 在while循环中,遥控器通道0的值是364-1684,那么就可以先减去1024,得到范围-660到660
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;//减1024
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
}
学习参考网址RM控制器