HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1.初版can comm已经实现; 2.修改了电机\bsp_can\usart的初始化实现

This commit is contained in:
NeoZeng 2022-11-28 16:17:30 +08:00
parent cb3f5d92d1
commit 8332422eac
17 changed files with 615 additions and 552 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -14,7 +14,10 @@
"string.h": "c", "string.h": "c",
"stdlib.h": "c", "stdlib.h": "c",
"crc8.h": "c", "crc8.h": "c",
"main.h": "c" "main.h": "c",
"can.h": "c",
"bsp_can.h": "c",
"dji_motor.h": "c"
}, },
"C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
"C_Cpp.intelliSenseEngineFallback": "enabled", "C_Cpp.intelliSenseEngineFallback": "enabled",

37
HAL_N_Middlewares/Src/main.c
View File

@ -40,6 +40,7 @@
#include "motor_task.h" #include "motor_task.h"
#include "referee.h" #include "referee.h"
#include "ins_task.h" #include "ins_task.h"
#include "can_comm.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@ -71,14 +72,21 @@ void MX_FREERTOS_Init(void);
/* Private user code ---------------------------------------------------------*/ /* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */ /* USER CODE BEGIN 0 */
typedef struct
{
float a;
float b;
float c;
}sdf;
volatile sdf* rx;
/* USER CODE END 0 */ /* USER CODE END 0 */
/** /**
* @brief The application entry point. * @brief The application entry point.
* @retval int * @retval int
*/ */
int main(void){ int main(void)
{
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* USER CODE END 1 */ /* USER CODE END 1 */
@ -125,9 +133,17 @@ int main(void){
.tx_id = 6}, .tx_id = 6},
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL}, .controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL},
.controller_param_init_config = {.angle_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}}; .controller_param_init_config = {.angle_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}};
dji_motor_instance *djimotor = DJIMotorInit(config); dji_motor_instance *djimotor = DJIMotorInit(&config);
CANComm_Init_Config_s cconfig = {
.can_config = {.can_handle=&hcan1,.tx_id=0x02,.rx_id=0x03},
.send_data_len = 4,
.recv_data_len = 12};
CANCommInstance* in = CANCommInit(&cconfig);
volatile float tx = 32;
RefereeInit(&huart6); RefereeInit(&huart6);
/* USER CODE END 2 */ /* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */ /* Call init function for freertos objects (in freertos.c) */
@ -139,15 +155,20 @@ int main(void){
/* We should never get here as control is now taken by the scheduler */ /* We should never get here as control is now taken by the scheduler */
/* Infinite loop */ /* Infinite loop */
/* USER CODE BEGIN WHILE */ /* USER CODE BEGIN WHILE */
INS_Init(); // INS_Init();
while (1) while (1)
{ {
/* USER CODE END WHILE */ /* USER CODE END WHILE */
DJIMotorSetRef(djimotor, 10);
MotorControlTask(); // DJIMotorSetRef(djimotor, 10);
HAL_Delay(1); // MotorControlTask();
INS_Task(); HAL_Delay(10);
CANCommSend(in, (uint8_t*)&tx);
tx+=1;
rx=(sdf*)CANCommGet(in);
// INS_Task();
/* USER CODE BEGIN 3 */ /* USER CODE BEGIN 3 */
} }
/* USER CODE END 3 */ /* USER CODE END 3 */

7
Makefile
View File

@ -210,6 +210,10 @@ C_INCLUDES = \
-IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/RTT \ -IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/RTT \
-IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/Config \ -IHAL_N_Middlewares/Middlewares/Third_Party/SEGGER/Config \
-IHAL_N_Middlewares/Middlewares/ST/ARM/DSP/Inc \ -IHAL_N_Middlewares/Middlewares/ST/ARM/DSP/Inc \
-Iapplication/chassis \
-Iapplication/shoot \
-Iapplication/gimbal \
-Iapplication/cmd \
-Iapplication \ -Iapplication \
-Ibsp \ -Ibsp \
-Imodules/algorithm \ -Imodules/algorithm \
@ -219,7 +223,8 @@ C_INCLUDES = \
-Imodules/motor \ -Imodules/motor \
-Imodules/referee \ -Imodules/referee \
-Imodules/remote \ -Imodules/remote \
-Imodules/super_cap -Imodules/super_cap \
-Imodules/can_comm
# compile gcc flags # compile gcc flags
ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections

172
application/cmd/chassis_cmd.c
View File

@ -1,96 +1,96 @@
#include <chassis_board_cmd.h> // #include <chassis_board_cmd.h>
#include <pub_sub.h> // #include <pub_sub.h>
void chassis_board_com_lost(void* obj) { // void chassis_board_com_lost(void* obj) {
//暂时没有效用 // //暂时没有效用
} // }
chassis_board_cmd::chassis_board_cmd() : sender([&] { // chassis_board_cmd::chassis_board_cmd() : sender([&] {
can_send<chassis_board_send>::can_send_config config; // can_send<chassis_board_send>::can_send_config config;
config.device = &BSP_CanTypeDef::can_devices[1]; // config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x003; // config.can_identifier = 0x003;
return config; // return config;
}()), // }()),
recver([&] { // recver([&] {
can_recv<gimbal_board_send>::can_recv_config config; // can_recv<gimbal_board_send>::can_recv_config config;
config.device = &BSP_CanTypeDef::can_devices[1]; // config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x004; // config.can_identifier = 0x004;
config.lost_callback = chassis_board_com_lost; // config.lost_callback = chassis_board_com_lost;
return config; // return config;
}()), // }()),
board_buzzer([&] { // board_buzzer([&] {
buzzer::buzzer_config config; // buzzer::buzzer_config config;
//底盘音乐2 // //底盘音乐2
config.music = &buzzer::buzzer_musics[2]; // config.music = &buzzer::buzzer_musics[2];
config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; // config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
return config; // return config;
}()) { // }()) {
robot_mode = robot_stop; // robot_mode = robot_stop;
robot_ready = 0; // robot_ready = 0;
chassis_upload_data = NULL; // chassis_upload_data = NULL;
board_recv = recver.recv_data; // board_recv = recver.recv_data;
memset(&chassis_control, 0, sizeof(cmd_chassis)); // memset(&chassis_control, 0, sizeof(cmd_chassis));
memset(&board_send, 0, sizeof(chassis_board_send)); // memset(&board_send, 0, sizeof(chassis_board_send));
} // }
void chassis_board_cmd::update() { // void chassis_board_cmd::update() {
//初始化为RUN // //初始化为RUN
robot_mode = robot_run; // robot_mode = robot_run;
//判断板间通信在线 // //判断板间通信在线
if (!recver.is_online()) { // if (!recver.is_online()) {
robot_mode = robot_stop; //板间通信掉线，机器人停止 // robot_mode = robot_stop; //板间通信掉线，机器人停止
} // }
//接收底盘回传信息，判断底盘IMU在线且初始化完成 // //接收底盘回传信息，判断底盘IMU在线且初始化完成
static subscriber<upload_chassis*> chassis_upload_suber("upload_chassis"); // static subscriber<upload_chassis*> chassis_upload_suber("upload_chassis");
if (!chassis_upload_suber.empty()) { // if (!chassis_upload_suber.empty()) {
chassis_upload_data = chassis_upload_suber.pop(); // chassis_upload_data = chassis_upload_suber.pop();
if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线 // if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线
robot_mode = robot_stop; // robot_mode = robot_stop;
} // }
} // }
if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成，第一次回传数据未收到 // if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成，第一次回传数据未收到
robot_mode = robot_stop; // robot_mode = robot_stop;
} else { // } else {
board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2]; // board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2];
} // }
//判断裁判系统是否在线，并处理掉线情况（未实现） // //判断裁判系统是否在线，并处理掉线情况（未实现）
board_send.heat_limit_remain = 30; //读取裁判系统 // board_send.heat_limit_remain = 30; //读取裁判系统
board_send.bullet_speed_max = 30; //读取裁判系统 // board_send.bullet_speed_max = 30; //读取裁判系统
//判断除了云台板stop之外，都已经上线，说明底盘板初始化完成，进入ready状态 // //判断除了云台板stop之外，都已经上线，说明底盘板初始化完成，进入ready状态
if (robot_mode == robot_run) { // if (robot_mode == robot_run) {
if (!robot_ready) { // if (!robot_ready) {
robot_ready = 1; // robot_ready = 1;
board_buzzer.start(); //播放音乐 // board_buzzer.start(); //播放音乐
} // }
board_send.chassis_board_status = module_run; // board_send.chassis_board_status = module_run;
} else { // } else {
board_send.chassis_board_status = module_lost; // board_send.chassis_board_status = module_lost;
} // }
//云台板进入stop模式 // //云台板进入stop模式
if (board_recv->robot_mode == robot_stop) { // if (board_recv->robot_mode == robot_stop) {
robot_mode = robot_stop; // robot_mode = robot_stop;
} // }
if (robot_mode == robot_stop) { // if (robot_mode == robot_stop) {
// STOP模式 // // STOP模式
chassis_control.mode = chassis_stop; // chassis_control.mode = chassis_stop;
} else { // } else {
// RUN模式 // // RUN模式
// 底盘控制指令 // // 底盘控制指令
chassis_control.mode = board_recv->chassis_mode; // chassis_control.mode = board_recv->chassis_mode;
chassis_control.speed = board_recv->chassis_speed; // chassis_control.speed = board_recv->chassis_speed;
chassis_control.power.power_buffer = 0; //应该由裁判系统获得，未实现 // chassis_control.power.power_buffer = 0; //应该由裁判系统获得，未实现
chassis_control.power.power_limit = 50; //应该由裁判系统获得，未实现 // chassis_control.power.power_limit = 50; //应该由裁判系统获得，未实现
} // }
//发布指令 // //发布指令
static publisher<cmd_chassis*> chassis_puber("cmd_chassis"); // static publisher<cmd_chassis*> chassis_puber("cmd_chassis");
chassis_puber.push(&chassis_control); // chassis_puber.push(&chassis_control);
//板间通信 // //板间通信
sender.send(board_send); // sender.send(board_send);
} // }

48
application/cmd/chassis_cmd.h
View File

@ -1,25 +1,25 @@
#ifndef _CHASSIS_BOARD_CMD_H_ // #ifndef _CHASSIS_BOARD_CMD_H_
#define _CHASSIS_BOARD_CMD_H_ // #define _CHASSIS_BOARD_CMD_H_
#include <buzzer.h> // #include <buzzer.h>
#include <can_recv.h> // #include <can_recv.h>
#include <can_send.h> // #include <can_send.h>
#include <robot_def.h> // #include <robot_def.h>
#include <stdint.h> // #include <stdint.h>
class chassis_board_cmd { // class chassis_board_cmd {
private: // private:
Robot_mode robot_mode; //机器人状态 // Robot_mode robot_mode; //机器人状态
cmd_chassis chassis_control; //发送给底盘的控制量 // cmd_chassis chassis_control; //发送给底盘的控制量
upload_chassis* chassis_upload_data; //底盘模块回传数据 // upload_chassis* chassis_upload_data; //底盘模块回传数据
chassis_board_send board_send; //发送给云台主控的数据 // chassis_board_send board_send; //发送给云台主控的数据
can_send<chassis_board_send> sender; //板间通信发送 // can_send<chassis_board_send> sender; //板间通信发送
can_recv<gimbal_board_send> recver; //板间通信接收 // can_recv<gimbal_board_send> recver; //板间通信接收
gimbal_board_send* board_recv; //板间通信接收数据指针 // gimbal_board_send* board_recv; //板间通信接收数据指针
void* referee; //裁判系统（尚未完成） // void* referee; //裁判系统（尚未完成）
buzzer board_buzzer; //蜂鸣器 // buzzer board_buzzer; //蜂鸣器
uint8_t robot_ready; //底盘板是否准备好 // uint8_t robot_ready; //底盘板是否准备好
public: // public:
chassis_board_cmd(); // chassis_board_cmd();
void update(); // void update();
}; // };
#endif // #endif

624
application/cmd/gimbal_cmd.c
View File

@ -1,336 +1,336 @@
#include "gimbal_board_cmd.h" // #include "gimbal_board_cmd.h"
#include "bsp_can.h" // #include "bsp_can.h"
#include "bsp_log.h" // #include "bsp_log.h"
#include "bsp_uart.h" // #include "bsp_uart.h"
#include "common.h" // #include "common.h"
#include "pub_sub.h" // #include "pub_sub.h"
#include "string.h" // #include "string.h"
#define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值 // #define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值
void gimbal_board_com_lost(void* obj) { // void gimbal_board_com_lost(void* obj) {
//暂时没有效用 // //暂时没有效用
} // }
//此处涉及到lamdba表达式的应用 // //此处涉及到lamdba表达式的应用
gimbal_board_cmd::gimbal_board_cmd() : sender([&] { // gimbal_board_cmd::gimbal_board_cmd() : sender([&] {
//板间通信：发 // //板间通信：发
can_send<gimbal_board_send>::can_send_config config; // can_send<gimbal_board_send>::can_send_config config;
config.device = &BSP_CanTypeDef::can_devices[1]; // config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x004; // config.can_identifier = 0x004;
return config; // return config;
}()), // }()),
recver([&] { // recver([&] {
//板间通信：收 // //板间通信：收
can_recv<chassis_board_send>::can_recv_config config; // can_recv<chassis_board_send>::can_recv_config config;
config.device = &BSP_CanTypeDef::can_devices[1]; // config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x003; // config.can_identifier = 0x003;
config.notify_func = NULL; // config.notify_func = NULL;
config.lost_callback = gimbal_board_com_lost; // config.lost_callback = gimbal_board_com_lost;
return config; // return config;
}()), // }()),
pc([&] { // pc([&] {
//小电脑通信配置 // //小电脑通信配置
canpc::canpc_config config; // canpc::canpc_config config;
config.device = &BSP_CanTypeDef::can_devices[0]; // config.device = &BSP_CanTypeDef::can_devices[0];
config.recv_identifer = 0x001; // config.recv_identifer = 0x001;
return config; // return config;
}()), // }()),
remote([&] { // remote([&] {
dt7Remote::dt7_config config; // dt7Remote::dt7_config config;
config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT]; // config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT];
return config; // return config;
}()), // }()),
board_buzzer([&] { // board_buzzer([&] {
buzzer::buzzer_config config; // buzzer::buzzer_config config;
config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT]; // config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
config.music = &buzzer::buzzer_musics[1]; // config.music = &buzzer::buzzer_musics[1];
return config; // return config;
}()) { // }()) {
robot_mode = robot_stop; // robot_mode = robot_stop;
robot_ready = 0; // robot_ready = 0;
autoaim_mode = auto_aim_off; // autoaim_mode = auto_aim_off;
gimbal_upload_data = NULL; // gimbal_upload_data = NULL;
//指针指向接收的实际数据 // //指针指向接收的实际数据
board_recv = recver.recv_data; // board_recv = recver.recv_data;
memset(&gimbal_control, 0, sizeof(cmd_gimbal)); // memset(&gimbal_control, 0, sizeof(cmd_gimbal));
memset(&shoot_control, 0, sizeof(cmd_shoot)); // memset(&shoot_control, 0, sizeof(cmd_shoot));
memset(&board_send, 0, sizeof(gimbal_board_send)); // memset(&board_send, 0, sizeof(gimbal_board_send));
} // }
float get_offset_angle(short init_forward, short now_encoder) { // float get_offset_angle(short init_forward, short now_encoder) {
short tmp = 0; // short tmp = 0;
if (init_forward < 4096) { // if (init_forward < 4096) {
if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) { // if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) {
tmp = now_encoder - init_forward; // tmp = now_encoder - init_forward;
} else if (now_encoder > 4096 + init_forward) { // } else if (now_encoder > 4096 + init_forward) {
tmp = -8192 + now_encoder - init_forward; // tmp = -8192 + now_encoder - init_forward;
} else { // } else {
tmp = now_encoder - init_forward; // tmp = now_encoder - init_forward;
} // }
} else { // } else {
if (now_encoder > init_forward) { // if (now_encoder > init_forward) {
tmp = now_encoder - init_forward; // tmp = now_encoder - init_forward;
} else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) { // } else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) {
tmp = now_encoder - init_forward; // tmp = now_encoder - init_forward;
} else { // } else {
tmp = now_encoder + 8192 - init_forward; // tmp = now_encoder + 8192 - init_forward;
} // }
} // }
return tmp * 360.0 / 8192.0; // return tmp * 360.0 / 8192.0;
} // }
void gimbal_board_cmd::update() { // void gimbal_board_cmd::update() {
//第一步，判断机器人工作模式 // //第一步，判断机器人工作模式
//初始化为RUN // //初始化为RUN
robot_mode = robot_run; // robot_mode = robot_run;
//判断板间通信在线 // //判断板间通信在线
if (!recver.is_online()) { // if (!recver.is_online()) {
robot_mode = robot_stop; //板间通信掉线，机器人停止 // robot_mode = robot_stop; //板间通信掉线，机器人停止
} // }
//接收云台回传信息，判断云台IMU在线且初始化完成 // //接收云台回传信息，判断云台IMU在线且初始化完成
static subscriber<upload_gimbal*> gimbal_upload_suber("upload_gimbal"); // static subscriber<upload_gimbal*> gimbal_upload_suber("upload_gimbal");
if (!gimbal_upload_suber.empty()) { // if (!gimbal_upload_suber.empty()) {
gimbal_upload_data = gimbal_upload_suber.pop(); // gimbal_upload_data = gimbal_upload_suber.pop();
if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线 // if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线
robot_mode = robot_stop; // robot_mode = robot_stop;
} // }
//计算云台和底盘的夹角 offset_angle // //计算云台和底盘的夹角 offset_angle
board_send.chassis_speed.offset_angle = get_offset_angle(INIT_FORWARD, *gimbal_upload_data->yaw_encoder); // board_send.chassis_speed.offset_angle = get_offset_angle(INIT_FORWARD, *gimbal_upload_data->yaw_encoder);
pc.pc_send_data.euler[0] = gimbal_upload_data->gimbal_imu->euler[0]; // pc.pc_send_data.euler[0] = gimbal_upload_data->gimbal_imu->euler[0];
pc.pc_send_data.euler[1] = gimbal_upload_data->gimbal_imu->euler[1]; // pc.pc_send_data.euler[1] = gimbal_upload_data->gimbal_imu->euler[1];
pc.pc_send_data.euler[2] = gimbal_upload_data->gimbal_imu->euler[2]; // pc.pc_send_data.euler[2] = gimbal_upload_data->gimbal_imu->euler[2];
pc.pc_send_data.auto_mode_flag = auto_aim_normal; // pc.pc_send_data.auto_mode_flag = auto_aim_normal;
} // }
if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成，第一次回传数据未收到 // if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成，第一次回传数据未收到
robot_mode = robot_stop; // robot_mode = robot_stop;
} // }
//如果除遥控器之外都已经上线，说明机器人初始化完成，进入ready状态 // //如果除遥控器之外都已经上线，说明机器人初始化完成，进入ready状态
if (robot_mode == robot_run) { // if (robot_mode == robot_run) {
if (!robot_ready) { // if (!robot_ready) {
robot_ready = 1; // robot_ready = 1;
board_buzzer.start(); //蜂鸣器播放初始化提示音 // board_buzzer.start(); //蜂鸣器播放初始化提示音
} // }
} // }
//遥控器判断 // //遥控器判断
if (!remote.monitor_item.is_online()) { // if (!remote.monitor_item.is_online()) {
//遥控器掉线 // //遥控器掉线
robot_mode = robot_stop; // robot_mode = robot_stop;
} else if (remote.data.input_mode == dt7Remote::RC_Stop) { // } else if (remote.data.input_mode == dt7Remote::RC_Stop) {
//遥控器处于stop档位 // //遥控器处于stop档位
robot_mode = robot_stop; // robot_mode = robot_stop;
} // }
//第二步，机器人控制主要逻辑 // //第二步，机器人控制主要逻辑
// STOP模式 // // STOP模式
if (robot_mode == robot_stop) { // if (robot_mode == robot_stop) {
stop_mode_update(); // stop_mode_update();
} // }
// 运行模式 // // 运行模式
if (robot_mode == robot_run) { // if (robot_mode == robot_run) {
//遥控器控制模式 // //遥控器控制模式
if (remote.data.input_mode == dt7Remote::RC_Remote) { // if (remote.data.input_mode == dt7Remote::RC_Remote) {
remote_mode_update(); // remote_mode_update();
} else if (remote.data.input_mode == dt7Remote::RC_MouseKey) { // } else if (remote.data.input_mode == dt7Remote::RC_MouseKey) {
mouse_key_mode_update(); // mouse_key_mode_update();
} // }
} // }
//发布命令，板间通信 // //发布命令，板间通信
send_cmd_and_data(); // send_cmd_and_data();
} // }
void gimbal_board_cmd::stop_mode_update() { // void gimbal_board_cmd::stop_mode_update() {
board_send.robot_mode = robot_stop; // board_send.robot_mode = robot_stop;
board_send.chassis_mode = chassis_stop; // board_send.chassis_mode = chassis_stop;
gimbal_control.gimbal_mode = gimbal_stop; // gimbal_control.gimbal_mode = gimbal_stop;
shoot_control.shoot_mode = shoot_stop; // shoot_control.shoot_mode = shoot_stop;
shoot_control.bullet_mode = bullet_holdon; // shoot_control.bullet_mode = bullet_holdon;
} // }
void gimbal_board_cmd::remote_mode_update() { // void gimbal_board_cmd::remote_mode_update() {
//云台控制参数 // //云台控制参数
gimbal_control.gimbal_mode = gimbal_run; // gimbal_control.gimbal_mode = gimbal_run;
gimbal_control.yaw -= 0.04f * ((float)remote.data.rc.ch2 - CHx_BIAS); // gimbal_control.yaw -= 0.04f * ((float)remote.data.rc.ch2 - CHx_BIAS);
gimbal_control.pitch = -1.0f * ((float)remote.data.rc.ch3 - CHx_BIAS); // gimbal_control.pitch = -1.0f * ((float)remote.data.rc.ch3 - CHx_BIAS);
gimbal_control.rotate_feedforward = 0; //目前没有使用小陀螺前馈 // gimbal_control.rotate_feedforward = 0; //目前没有使用小陀螺前馈
//底盘控制参数 // //底盘控制参数
if (remote.data.rc.s1 == 1) { // if (remote.data.rc.s1 == 1) {
//小陀螺模式 // //小陀螺模式
board_send.chassis_mode = chassis_rotate_run; // board_send.chassis_mode = chassis_rotate_run;
board_send.chassis_speed.vy = 16.0 * 0.6 * (float)(remote.data.rc.ch1 - CHx_BIAS); // board_send.chassis_speed.vy = 16.0 * 0.6 * (float)(remote.data.rc.ch1 - CHx_BIAS);
board_send.chassis_speed.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS); // board_send.chassis_speed.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS);
} else { // } else {
//底盘跟随模式 // //底盘跟随模式
board_send.chassis_mode = chassis_run_follow_offset; // board_send.chassis_mode = chassis_run_follow_offset;
board_send.chassis_speed.vy = 16.0 * (float)(remote.data.rc.ch1 - CHx_BIAS); // board_send.chassis_speed.vy = 16.0 * (float)(remote.data.rc.ch1 - CHx_BIAS);
board_send.chassis_speed.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS); // board_send.chassis_speed.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS);
} // }
//发射机构控制 // //发射机构控制
if (remote.data.rc.s1 == 2) { // if (remote.data.rc.s1 == 2) {
shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮 // shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮
shoot_control.shoot_mode = shoot_stop; //在发射停止位关闭摩擦轮 // shoot_control.shoot_mode = shoot_stop; //在发射停止位关闭摩擦轮
//弹仓盖控制 // //弹仓盖控制
if (remote.data.rc.ch4 > CHx_BIAS + 400) shoot_control.mag_mode = magazine_open; // if (remote.data.rc.ch4 > CHx_BIAS + 400) shoot_control.mag_mode = magazine_open;
if (remote.data.rc.ch4 < CHx_BIAS - 400) shoot_control.mag_mode = magazine_close; // if (remote.data.rc.ch4 < CHx_BIAS - 400) shoot_control.mag_mode = magazine_close;
} else { // } else {
//发弹控制 // //发弹控制
shoot_control.shoot_mode = shoot_run; //开摩擦轮 // shoot_control.shoot_mode = shoot_run; //开摩擦轮
shoot_control.bullet_mode = bullet_continuous; //连续发射 // shoot_control.bullet_mode = bullet_continuous; //连续发射
shoot_control.fire_rate = 0.01f * (float)(remote.data.rc.ch4 - CHx_BIAS); //射频 // shoot_control.fire_rate = 0.01f * (float)(remote.data.rc.ch4 - CHx_BIAS); //射频
shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 // shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余
shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 // shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
} // }
} // }
void gimbal_board_cmd::mouse_key_mode_update() { // void gimbal_board_cmd::mouse_key_mode_update() {
// r:小陀螺模式 // // r:小陀螺模式
if (remote.data.key_single_press_cnt.r != remote.last_data.key_single_press_cnt.r) { // if (remote.data.key_single_press_cnt.r != remote.last_data.key_single_press_cnt.r) {
if (board_send.chassis_mode != chassis_rotate_run) { // if (board_send.chassis_mode != chassis_rotate_run) {
board_send.chassis_mode = chassis_rotate_run; //小陀螺模式 // board_send.chassis_mode = chassis_rotate_run; //小陀螺模式
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 // gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} else { // } else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 // board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 // gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} // }
} // }
// v:云台底盘独立模式 // // v:云台底盘独立模式
if (remote.data.key_single_press_cnt.v != remote.last_data.key_single_press_cnt.v) { // if (remote.data.key_single_press_cnt.v != remote.last_data.key_single_press_cnt.v) {
if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_run) { // if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_run) {
board_send.chassis_mode = chassis_run; //底盘独立模式 // board_send.chassis_mode = chassis_run; //底盘独立模式
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 // gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} else { // } else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 // board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 // gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} // }
} // }
// x:云台跟随底盘模式 // // x:云台跟随底盘模式
if (remote.data.key_single_press_cnt.x != remote.last_data.key_single_press_cnt.x) { // if (remote.data.key_single_press_cnt.x != remote.last_data.key_single_press_cnt.x) {
if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_middle) { // if (board_send.chassis_mode != chassis_run || gimbal_control.gimbal_mode != gimbal_middle) {
board_send.chassis_mode = chassis_run; //底盘独立模式 // board_send.chassis_mode = chassis_run; //底盘独立模式
gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式 // gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式
} else { // } else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台 // board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式 // gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} // }
} // }
// z:爬坡模式（待添加） // // z:爬坡模式（待添加）
// f:正常自瞄模式 // // f:正常自瞄模式
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { // if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_normal) { // if (autoaim_mode != auto_aim_normal) {
autoaim_mode = auto_aim_normal; // autoaim_mode = auto_aim_normal;
} else { // } else {
autoaim_mode = auto_aim_off; // autoaim_mode = auto_aim_off;
} // }
} // }
// g:小符 // // g:小符
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { // if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_buff_small) { // if (autoaim_mode != auto_aim_buff_small) {
autoaim_mode = auto_aim_buff_small; // autoaim_mode = auto_aim_buff_small;
} else { // } else {
autoaim_mode = auto_aim_off; // autoaim_mode = auto_aim_off;
} // }
} // }
// b:大符 // // b:大符
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) { // if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_buff_big) { // if (autoaim_mode != auto_aim_buff_big) {
autoaim_mode = auto_aim_buff_big; // autoaim_mode = auto_aim_buff_big;
} else { // } else {
autoaim_mode = auto_aim_off; // autoaim_mode = auto_aim_off;
} // }
} // }
pc.pc_send_data.auto_mode_flag = autoaim_mode; // pc.pc_send_data.auto_mode_flag = autoaim_mode;
// c:开关弹仓 // // c:开关弹仓
if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) { // if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) {
if (shoot_control.mag_mode == magazine_open) { // if (shoot_control.mag_mode == magazine_open) {
shoot_control.mag_mode = magazine_close; // shoot_control.mag_mode = magazine_close;
} else { // } else {
shoot_control.mag_mode = magazine_open; // shoot_control.mag_mode = magazine_open;
} // }
} // }
//底盘控制参数 // //底盘控制参数
// wasd // // wasd
if (remote.data.key_down.w) board_send.chassis_speed.vy = 8000; // if (remote.data.key_down.w) board_send.chassis_speed.vy = 8000;
if (remote.data.key_down.s) board_send.chassis_speed.vy = -8000; // if (remote.data.key_down.s) board_send.chassis_speed.vy = -8000;
if (remote.data.key_down.d) board_send.chassis_speed.vx = 8000; // if (remote.data.key_down.d) board_send.chassis_speed.vx = 8000;
if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000; // if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000;
//按住ctrl减速 // //按住ctrl减速
if (remote.data.key_down.ctrl) { // if (remote.data.key_down.ctrl) {
board_send.chassis_speed.vx /= 3.0; // board_send.chassis_speed.vx /= 3.0;
board_send.chassis_speed.vy /= 3.0; // board_send.chassis_speed.vy /= 3.0;
} // }
//按住shift加速 // //按住shift加速
if (remote.data.key_down.shift) { // if (remote.data.key_down.shift) {
board_send.chassis_speed.vx *= 3.0; // board_send.chassis_speed.vx *= 3.0;
board_send.chassis_speed.vy *= 3.0; // board_send.chassis_speed.vy *= 3.0;
} // }
// q和e // // q和e
if (remote.data.key_down.q) { // if (remote.data.key_down.q) {
if (board_send.chassis_mode == chassis_run) { // if (board_send.chassis_mode == chassis_run) {
board_send.chassis_speed.rotate = -90; // board_send.chassis_speed.rotate = -90;
} else if (board_send.chassis_mode == chassis_run_follow_offset) { // } else if (board_send.chassis_mode == chassis_run_follow_offset) {
gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘 // gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘
} // }
} // }
if (remote.data.key_down.e) { // if (remote.data.key_down.e) {
if (board_send.chassis_mode == chassis_run) { // if (board_send.chassis_mode == chassis_run) {
board_send.chassis_speed.rotate = 90; // board_send.chassis_speed.rotate = 90;
} else if (board_send.chassis_mode == chassis_run_follow_offset) { // } else if (board_send.chassis_mode == chassis_run_follow_offset) {
gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘 // gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘
} // }
} // }
//云台控制参数 // //云台控制参数
if (gimbal_control.gimbal_mode == gimbal_run) { // if (gimbal_control.gimbal_mode == gimbal_run) {
if (autoaim_mode == auto_aim_off) { // if (autoaim_mode == auto_aim_off) {
gimbal_control.yaw -= 0.5f * (0.7f * (remote.data.mouse.x) + 0.3f * (remote.last_data.mouse.x)); // gimbal_control.yaw -= 0.5f * (0.7f * (remote.data.mouse.x) + 0.3f * (remote.last_data.mouse.x));
gimbal_control.pitch -= -0.1f * ((float)remote.data.mouse.y); // gimbal_control.pitch -= -0.1f * ((float)remote.data.mouse.y);
} else { // } else {
//计算真实yaw值 // //计算真实yaw值
float yaw_target = pc.pc_recv_data->yaw * 8192.0 / 2 / pi + gimbal_upload_data->gimbal_imu->round * 8192.0; // float yaw_target = pc.pc_recv_data->yaw * 8192.0 / 2 / pi + gimbal_upload_data->gimbal_imu->round * 8192.0;
if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] > pi) yaw_target -= 8192; // if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] > pi) yaw_target -= 8192;
if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] < -pi) yaw_target += 8192; // if (pc.pc_recv_data->yaw - gimbal_upload_data->gimbal_imu->euler[2] < -pi) yaw_target += 8192;
gimbal_control.yaw = yaw_target; // gimbal_control.yaw = yaw_target;
gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定，pitch轴反馈为陀螺仪roll // gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定，pitch轴反馈为陀螺仪roll
} // }
} else if (gimbal_control.gimbal_mode == gimbal_middle) { // } else if (gimbal_control.gimbal_mode == gimbal_middle) {
//云台跟随底盘模式，待完善 // //云台跟随底盘模式，待完善
} // }
//发射机构控制参数 // //发射机构控制参数
if (remote.data.rc.s1 == 2) { // if (remote.data.rc.s1 == 2) {
shoot_control.bullet_mode = bullet_holdon; // shoot_control.bullet_mode = bullet_holdon;
shoot_control.shoot_mode = shoot_stop; // shoot_control.shoot_mode = shoot_stop;
} else { // } else {
//发弹控制，单发，双发, 射频和小电脑控制待完善 // //发弹控制，单发，双发, 射频和小电脑控制待完善
shoot_control.shoot_mode = shoot_run; //开摩擦轮 // shoot_control.shoot_mode = shoot_run; //开摩擦轮
shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余 // shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余
shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限 // shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
shoot_control.fire_rate = 3; //固定射频 // shoot_control.fire_rate = 3; //固定射频
if (remote.data.mouse.press_l) { // if (remote.data.mouse.press_l) {
shoot_control.bullet_mode = bullet_continuous; // shoot_control.bullet_mode = bullet_continuous;
} else { // } else {
shoot_control.bullet_mode = bullet_holdon; // shoot_control.bullet_mode = bullet_holdon;
} // }
} // }
} // }
void gimbal_board_cmd::send_cmd_and_data() { // void gimbal_board_cmd::send_cmd_and_data() {
static publisher<cmd_gimbal*> gimbal_puber("cmd_gimbal"); // static publisher<cmd_gimbal*> gimbal_puber("cmd_gimbal");
static publisher<cmd_shoot*> shoot_puber("cmd_shoot"); // static publisher<cmd_shoot*> shoot_puber("cmd_shoot");
gimbal_puber.push(&gimbal_control); // gimbal_puber.push(&gimbal_control);
shoot_puber.push(&shoot_control); // shoot_puber.push(&shoot_control);
sender.send(board_send); // sender.send(board_send);
pc.send(pc.pc_send_data); // pc.send(pc.pc_send_data);
} // }

62
application/cmd/gimbal_cmd.h
View File

@ -1,35 +1,35 @@
#ifndef _GIMBAL_ROBOT_CMD_H // #ifndef _GIMBAL_ROBOT_CMD_H
#define _GIMBAL_ROBOT_CMD_H // #define _GIMBAL_ROBOT_CMD_H
#include <DT7_DR16.h> // #include <DT7_DR16.h>
#include <buzzer.h> // #include <buzzer.h>
#include <can_pc.h> // #include <can_pc.h>
#include <pub_sub.h> // #include <pub_sub.h>
#include <robot_def.h> // #include <robot_def.h>
class gimbal_board_cmd { // class gimbal_board_cmd {
private: // private:
gimbal_board_send board_send; //需要发送给下板的板间通信数据 // gimbal_board_send board_send; //需要发送给下板的板间通信数据
cmd_gimbal gimbal_control; //发送给云台的控制量 // cmd_gimbal gimbal_control; //发送给云台的控制量
cmd_shoot shoot_control; //发送给发射机构的控制量 // cmd_shoot shoot_control; //发送给发射机构的控制量
can_send<gimbal_board_send> sender; //板间通信发送 // can_send<gimbal_board_send> sender; //板间通信发送
can_recv<chassis_board_send> recver; //板间通信接收 // can_recv<chassis_board_send> recver; //板间通信接收
chassis_board_send* board_recv; //板间通信接收数据指针 // chassis_board_send* board_recv; //板间通信接收数据指针
canpc pc; //小电脑视觉数据 // canpc pc; //小电脑视觉数据
dt7Remote remote; //遥控器 // dt7Remote remote; //遥控器
Robot_mode robot_mode; //机器人模式 // Robot_mode robot_mode; //机器人模式
uint8_t robot_ready; //机器人准备好标志位 // uint8_t robot_ready; //机器人准备好标志位
AutoAim_mode autoaim_mode; //机器人自瞄模式 // AutoAim_mode autoaim_mode; //机器人自瞄模式
buzzer board_buzzer; //蜂鸣器 // buzzer board_buzzer; //蜂鸣器
upload_gimbal* gimbal_upload_data; //云台模块回传的数据 // upload_gimbal* gimbal_upload_data; //云台模块回传的数据
void stop_mode_update(); //机器人停止模式更新函数 // void stop_mode_update(); //机器人停止模式更新函数
void remote_mode_update(); //机器人遥控器模式更新函数 // void remote_mode_update(); //机器人遥控器模式更新函数
void mouse_key_mode_update(); //机器人键鼠模式更新函数 // void mouse_key_mode_update(); //机器人键鼠模式更新函数
void send_cmd_and_data(); //发布指令和板间通信 // void send_cmd_and_data(); //发布指令和板间通信
public: // public:
gimbal_board_cmd(); // gimbal_board_cmd();
void update(); // void update();
}; // };
#endif // #endif

12
bsp/bsp_can.c
View File

@ -57,7 +57,7 @@ static void CANServiceInit()
/* -----------------------two extern callable function -----------------------*/ /* -----------------------two extern callable function -----------------------*/
void CANRegister(can_instance *ins, can_instance_config_s config) void CANRegister(can_instance *ins, can_instance_config_s* config)
{ {
static uint8_t idx; static uint8_t idx;
if (!idx) if (!idx)
@ -66,15 +66,15 @@ void CANRegister(can_instance *ins, can_instance_config_s config)
} }
instance[idx] = ins; instance[idx] = ins;
instance[idx]->txconf.StdId = config.tx_id; instance[idx]->txconf.StdId = config->tx_id;
instance[idx]->txconf.IDE = CAN_ID_STD; instance[idx]->txconf.IDE = CAN_ID_STD;
instance[idx]->txconf.RTR = CAN_RTR_DATA; instance[idx]->txconf.RTR = CAN_RTR_DATA;
instance[idx]->txconf.DLC = 0x08; instance[idx]->txconf.DLC = 0x08;
instance[idx]->can_handle = config.can_handle; instance[idx]->can_handle = config->can_handle;
instance[idx]->tx_id = config.tx_id; instance[idx]->tx_id = config->tx_id;
instance[idx]->rx_id = config.rx_id; instance[idx]->rx_id = config->rx_id;
instance[idx]->can_module_callback = config.can_module_callback; instance[idx]->can_module_callback = config->can_module_callback;
CANAddFilter(instance[idx++]); CANAddFilter(instance[idx++]);
} }

13
bsp/bsp_can.h
View File

@ -9,7 +9,9 @@
#define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless #define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless
#define DEVICE_CAN_CNT 2 // CAN1,CAN2 #define DEVICE_CAN_CNT 2 // CAN1,CAN2
/* can instance typedef, every module registered to CAN should have this variable */ /* can instance typedef, every module registered to CAN should have this variable */
#pragma pack(1)
typedef struct _ typedef struct _
{ {
CAN_HandleTypeDef *can_handle; CAN_HandleTypeDef *can_handle;
@ -22,6 +24,7 @@ typedef struct _
uint8_t rx_len; uint8_t rx_len;
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
} can_instance; } can_instance;
#pragma pack()
/* this structure is used as initialization*/ /* this structure is used as initialization*/
typedef struct typedef struct
@ -32,8 +35,8 @@ typedef struct
void (*can_module_callback)(can_instance *); void (*can_module_callback)(can_instance *);
} can_instance_config_s; } can_instance_config_s;
/* module callback,which resolve protocol when new mesg arrives*/ /* module callback,which resolve protocol when new mesg arrives */
typedef void (*can_callback)(can_instance *); typedef void (*can_callback)(can_instance*);
/** /**
* @brief transmit mesg through CAN device * @brief transmit mesg through CAN device
@ -48,14 +51,14 @@ void CANTransmit(can_instance *_instance);
* @param config init config * @param config init config
* @return can_instance* can instance owned by module * @return can_instance* can instance owned by module
*/ */
void CANRegister(can_instance *instance, can_instance_config_s config); void CANRegister(can_instance *instance, can_instance_config_s *config);
/** /**
* @brief 修改CAN发送报文的数据帧长度;注意最大长度为8,在没有进行修改的时候,默认长度为8 * @brief 修改CAN发送报文的数据帧长度;注意最大长度为8,在没有进行修改的时候,默认长度为8
* *
* @param _instance 要修改长度的can实例 * @param _instance 要修改长度的can实例
* @param length 设定长度 * @param length 设定长度
*/ */
void CANSetDLC(can_instance *_instance,uint8_t length); void CANSetDLC(can_instance *_instance, uint8_t length);
#endif #endif

103
modules/can_comm/can_comm.c
View File

@ -1,16 +1,28 @@
#include "can_comm.h" #include "can_comm.h"
#include "memory.h" #include "memory.h"
#include "stdlib.h" #include "stdlib.h"
#include "crc8.h" #include "crc8.h"
/* can_comm用于保存每个实例的指针数组,用于回调函数区分实例 */ /* can_comm用于保存每个实例的指针数组,用于回调函数区分实例 */
static CANCommInstance *can_comm_instance[MX_CAN_COMM_COUNT] = {NULL}; static CANCommInstance *can_comm_instance[MX_CAN_COMM_COUNT] = {NULL};
static uint8_t idx; //配合can_comm_instance的初始化使用,标识当前初始化的是哪一个实例 static uint8_t idx; // 配合can_comm_instance的初始化使用,标识当前初始化的是哪一个实例
/** /**
* @brief * @brief 重置CAN comm的接收状态和buffer
* *
* @param _instance * @param ins 需要重置的实例
*/
static void CANCommResetRx(CANCommInstance *ins)
{
memset(ins->raw_recvbuf, 0, ins->cur_recv_len);
ins->recv_state = 0;
ins->cur_recv_len = 0;
}
/**
* @brief
*
* @param _instance
*/ */
static void CANCommRxCallback(can_instance *_instance) static void CANCommRxCallback(can_instance *_instance)
{ {
@ -18,75 +30,84 @@ static void CANCommRxCallback(can_instance *_instance)
{ {
if (&can_comm_instance[i]->can_ins == _instance) // 遍历,找到对应的接收CAN COMM实例 if (&can_comm_instance[i]->can_ins == _instance) // 遍历,找到对应的接收CAN COMM实例
{ {
if(_instance->rx_buff[0]==CAN_COMM_HEADER && can_comm_instance[i]->recv_state==0) //尚未开始接收且新的一包里有帧头 /* 接收状态判断 */
if (_instance->rx_buff[0] == CAN_COMM_HEADER && can_comm_instance[i]->recv_state == 0) // 尚未开始接收且新的一包里有帧头
{ {
if(_instance->rx_buff[1]==can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度 if (_instance->rx_buff[1] == can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度
{ {
can_comm_instance[i]->recv_state=1; can_comm_instance[i]->recv_state = 1;
} }
else else
return ; //直接跳过即可 return; // 直接跳过即可
} }
if(can_comm_instance[i]->recv_state) //已经开始接收 if (can_comm_instance[i]->recv_state) // 已经收到过帧头
{ // 直接拷贝到当前的接收buffer后面 { // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
memcpy(can_comm_instance[i]->raw_recvbuf+can_comm_instance[i]->cur_recv_len,_instance->rx_buff,8); if (can_comm_instance[i]->cur_recv_len + _instance->rx_len > can_comm_instance[i]->recv_buf_len)
can_comm_instance[i]->cur_recv_len+=8; {
CANCommResetRx(can_comm_instance[i]);
return; // 重置状态然后返回
}
// 直接拷贝到当前的接收buffer后面
memcpy(can_comm_instance[i]->raw_recvbuf + can_comm_instance[i]->cur_recv_len, _instance->rx_buff, _instance->rx_len);
can_comm_instance[i]->cur_recv_len += _instance->rx_len;
// 当前已经收满 // 当前已经收满
if(can_comm_instance[i]->cur_recv_len>=can_comm_instance[i]->recv_buf_len) if (can_comm_instance[i]->cur_recv_len == can_comm_instance[i]->recv_buf_len)
{ // buff里本该是tail的位置不等于CAN_COMM_TAIL { // buff里本该是tail的位置不等于CAN_COMM_TAIL
if(can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len-1]!=CAN_COMM_TAIL) if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 1] != CAN_COMM_TAIL)
{ {
memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len); CANCommResetRx(can_comm_instance[i]);
can_comm_instance[i]->recv_state=0; return; // 重置状态然后返回
can_comm_instance[i]->cur_recv_len=0;
return ; // 重置状态然后返回
} }
else // tail正确, 对数据进行crc8校验 else // tail正确, 对数据进行crc8校验
{ {
if(can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len-2] == if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 2] ==
crc_8(can_comm_instance[i]->raw_recvbuf+2,can_comm_instance[i]->recv_data_len)) crc_8(can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len))
{ // 通过校验,复制数据到unpack_data中 { // 通过校验,复制数据到unpack_data中
memcpy(can_comm_instance[i]->raw_recvbuf+2,can_comm_instance[i]->unpacked_recv_data,can_comm_instance[i]->recv_data_len); memcpy(can_comm_instance[i]->unpacked_recv_data,can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len);
can_comm_instance[i]->update_flag=1; //数据更新flag置为1 can_comm_instance[i]->update_flag = 1; // 数据更新flag置为1
} }
memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len); //整个buff置零 CANCommResetRx(can_comm_instance[i]);
can_comm_instance[i]->recv_state=0; return; // 重置状态然后返回
can_comm_instance[i]->cur_recv_len=0;
return ; // 重置状态然后返回
} }
} }
} }
return ; return; // 访问完一个can comm直接退出,一次中断只会也只可能会处理一个实例的回调
} }
} }
} }
CANCommInstance *CANCommInit(CANComm_Init_Config_s config) CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
{ {
can_comm_instance[idx] = (CANCommInstance *)malloc(sizeof(CANCommInstance)); can_comm_instance[idx] = (CANCommInstance *)malloc(sizeof(CANCommInstance));
memset(can_comm_instance[idx], 0, sizeof(CANCommInstance)); memset(can_comm_instance[idx], 0, sizeof(CANCommInstance));
can_comm_instance[idx]->recv_data_len = config.recv_data_len; can_comm_instance[idx]->recv_data_len = comm_config->recv_data_len;
can_comm_instance[idx]->send_data_len = config.send_data_len; can_comm_instance[idx]->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
can_comm_instance[idx]->send_buf_len = config.send_data_len + CAN_COMM_OFFSET_BYTES; can_comm_instance[idx]->send_data_len = comm_config->send_data_len;
can_comm_instance[idx]->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
can_comm_instance[idx]->raw_sendbuf[0] = CAN_COMM_HEADER; can_comm_instance[idx]->raw_sendbuf[0] = CAN_COMM_HEADER;
can_comm_instance[idx]->raw_sendbuf[1] = config.send_data_len; can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len;
can_comm_instance[idx]->raw_sendbuf[config.send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL; can_comm_instance[idx]->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
config.can_config.can_module_callback = CANCommRxCallback; comm_config->can_config.can_module_callback = CANCommRxCallback;
CANRegister(&can_comm_instance[idx]->can_ins, config.can_config); CANRegister(&can_comm_instance[idx]->can_ins, &comm_config->can_config);
return can_comm_instance[idx++]; return can_comm_instance[idx++];
} }
void CANCommSend(CANCommInstance *instance, uint8_t *data) void CANCommSend(CANCommInstance *instance, uint8_t *data)
{ {
static uint8_t crc8; static uint8_t crc8;
static uint8_t send_len;
memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len); memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len);
crc8 = crc_8(data, instance->send_data_len); crc8 = crc_8(data, instance->send_data_len);
instance->raw_sendbuf[2 + instance->send_data_len] = crc8; instance->raw_sendbuf[2 + instance->send_data_len] = crc8;
for (size_t i = 0; i < instance->send_buf_len; i += 8) for (size_t i = 0; i < instance->send_buf_len; i += 8)
{ { // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可
memcpy(instance->can_ins.tx_buff, instance->raw_sendbuf[i], 8); send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i;
CANSetDLC(&instance->can_ins, send_len);
memcpy(instance->can_ins.tx_buff, instance->raw_sendbuf+i, send_len);
CANTransmit(&instance->can_ins); CANTransmit(&instance->can_ins);
} }
} }

46
modules/can_comm/can_comm.h
View File

@ -4,72 +4,74 @@
* @brief 用于多机CAN通信的收发模块 * @brief 用于多机CAN通信的收发模块
* @version 0.1 * @version 0.1
* @date 2022-11-27 * @date 2022-11-27
* *
* @copyright Copyright (c) 2022 HNUYueLu EC all rights reserved * @copyright Copyright (c) 2022 HNUYueLu EC all rights reserved
* *
*/ */
#ifndef CAN_COMM_H #ifndef CAN_COMM_H
#define CAN_COMM_H #define CAN_COMM_H
#include "bsp_can.h" #include "bsp_can.h"
#define MX_CAN_COMM_COUNT 4 //注意均衡负载,一条总线上不要挂载过多的外设 #define MX_CAN_COMM_COUNT 4 // 注意均衡负载,一条总线上不要挂载过多的外设
#define CAN_COMM_MAX_BUFFSIZE 68 // 最大发送/接收字节数 #define CAN_COMM_MAX_BUFFSIZE 60 // 最大发送/接收字节数,如果不够可以增加此数值
#define CAN_COMM_HEADER 's' #define CAN_COMM_HEADER 's'
#define CAN_COMM_TAIL 'e' #define CAN_COMM_TAIL 'e'
#define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8 #define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8
typedef struct #pragma pack(1)
/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */
typedef struct
{ {
can_instance can_ins; can_instance can_ins;
/* 发送部分 */ /* 发送部分 */
uint8_t send_data_len; uint8_t send_data_len;
uint8_t send_buf_len; uint8_t send_buf_len;
uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE+CAN_COMM_OFFSET_BYTES]; //额外4个bytes保存帧头帧尾和校验和 uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
/* 接收部分 */ /* 接收部分 */
uint8_t recv_data_len; uint8_t recv_data_len;
uint8_t recv_buf_len; uint8_t recv_buf_len;
uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE+CAN_COMM_OFFSET_BYTES]; //额外4个bytes保存帧头帧尾和校验和 uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
/* 接收和更新标志位*/ /* 接收和更新标志位*/
uint8_t recv_state; uint8_t recv_state;
uint8_t cur_recv_len; uint8_t cur_recv_len;
uint8_t update_flag; uint8_t update_flag;
} CANCommInstance; } CANCommInstance;
#pragma pack()
/* CAN comm 初始化结构体 */
typedef struct typedef struct
{ {
can_instance_config_s can_config;
uint8_t send_data_len; uint8_t send_data_len;
uint8_t recv_data_len; uint8_t recv_data_len;
can_instance_config_s can_config; } CANComm_Init_Config_s;
}CANComm_Init_Config_s;
/** /**
* @brief * @brief
* *
* @param config * @param config
* @return CANCommInstance* * @return CANCommInstance*
*/ */
CANCommInstance* CANCommInit(CANComm_Init_Config_s config); CANCommInstance *CANCommInit(CANComm_Init_Config_s* comm_config);
/** /**
* @brief 发送数据 * @brief 发送数据
* *
* @param instance can comm实例 * @param instance can comm实例
* @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同 * @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同
*/ */
void CANCommSend(CANCommInstance* instance,uint8_t* data); void CANCommSend(CANCommInstance *instance, uint8_t *data);
/** /**
* @brief 获取CAN COMM接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型 * @brief 获取CAN COMM接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型
* *
* @return void* 返回的数据指针 * @return void* 返回的数据指针
* @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n) * @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n)
* CAN COMM接收到的数据可以看作是pack(1)之后的,是连续存放的 * CAN COMM接收到的数据可以看作是pack(1)之后的,是连续存放的
*/ */
void* CANCommGet(CANCommInstance* instance); void *CANCommGet(CANCommInstance *instance);
#endif // !CAN_COMM_H #endif // !CAN_COMM_H

2
modules/motor/HT04.c
View File

@ -40,7 +40,7 @@ joint_instance *HTMotorInit(can_instance_config_s config)
{ {
static uint8_t idx; static uint8_t idx;
joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance)); joint_motor_info[idx] = (joint_instance *)malloc(sizeof(joint_instance));
CANRegister(joint_motor_info[idx++]->motor_can_instace, config); CANRegister(joint_motor_info[idx++]->motor_can_instace, &config);
return joint_motor_info[idx++]; return joint_motor_info[idx++];
} }

2
modules/motor/LK9025.c
View File

@ -23,7 +23,7 @@ driven_instance *LKMotroInit(can_instance_config_s config)
static uint8_t idx; static uint8_t idx;
driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance)); driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance));
config.can_module_callback = DecodeDriven; config.can_module_callback = DecodeDriven;
CANRegister(driven_motor_info[idx]->motor_can_instance, config); CANRegister(driven_motor_info[idx]->motor_can_instance, &config);
return driven_motor_info[idx++]; return driven_motor_info[idx++];
} }

27
modules/motor/dji_motor.c
View File

@ -120,8 +120,10 @@ static void MotorSenderGrouping(can_instance_config_s *config)
*/ */
static void DecodeDJIMotor(can_instance *_instance) static void DecodeDJIMotor(can_instance *_instance)
{ {
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
static uint8_t *rxbuff; static uint8_t *rxbuff;
static dji_motor_measure *measure; static dji_motor_measure *measure;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++) for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{ {
if (&dji_motor_info[i]->motor_can_instance == _instance) if (&dji_motor_info[i]->motor_can_instance == _instance)
@ -131,6 +133,7 @@ static void DecodeDJIMotor(can_instance *_instance)
// resolve data and apply filter to current and speed // resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd; measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
// 增加滤波
measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]); measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
measure->temperate = rxbuff[6]; measure->temperate = rxbuff[6];
@ -146,26 +149,26 @@ static void DecodeDJIMotor(can_instance *_instance)
} }
// 电机初始化,返回一个电机实例 // 电机初始化,返回一个电机实例
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config) dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config)
{ {
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance)); dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance)); memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance));
// motor basic setting // motor basic setting
dji_motor_info[idx]->motor_type = config.motor_type; dji_motor_info[idx]->motor_type = config->motor_type;
dji_motor_info[idx]->motor_settings = config.controller_setting_init_config; dji_motor_info[idx]->motor_settings = config->controller_setting_init_config;
// motor controller init // motor controller init
PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config.controller_param_init_config.current_PID); PID_Init(&dji_motor_info[idx]->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config.controller_param_init_config.speed_PID); PID_Init(&dji_motor_info[idx]->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config.controller_param_init_config.angle_PID); PID_Init(&dji_motor_info[idx]->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config.controller_param_init_config.other_angle_feedback_ptr; dji_motor_info[idx]->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config.controller_param_init_config.other_speed_feedback_ptr; dji_motor_info[idx]->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
// group motors, because 4 motors share the same CAN control message // group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(&config.can_init_config); MotorSenderGrouping(&config->can_init_config);
// register motor to CAN bus // register motor to CAN bus
config.can_init_config.can_module_callback = DecodeDJIMotor; // set callback config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, config.can_init_config); CANRegister(&dji_motor_info[idx]->motor_can_instance, &config->can_init_config);
return dji_motor_info[idx++]; return dji_motor_info[idx++];
} }

5
modules/motor/dji_motor.h
View File

@ -20,6 +20,8 @@
#include "motor_def.h" #include "motor_def.h"
#define DJI_MOTOR_CNT 12 #define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8 #define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8
#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95 #define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
@ -77,7 +79,7 @@ typedef struct
* *
* @return dji_motor_instance* * @return dji_motor_instance*
*/ */
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config); dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config);
/** /**
* @brief 被application层的应用调用,给电机设定参考值. * @brief 被application层的应用调用,给电机设定参考值.
@ -103,4 +105,5 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
*/ */
void DJIMotorControl(); void DJIMotorControl();
#endif // !DJI_MOTOR_H #endif // !DJI_MOTOR_H

1
modules/motor/motor_def.h
View File

@ -17,6 +17,7 @@
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x))) #define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
/** /**
* @brief 闭环类型,如果需要多个闭环,则使用或运算 * @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP * 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP

1
modules/referee/referee.h
View File

@ -346,4 +346,5 @@ void RefereeInit(UART_HandleTypeDef *referee_usart_handle);
void JudgeReadData(uint8_t *ReadFromUsart); void JudgeReadData(uint8_t *ReadFromUsart);
float JudgeGetChassisPower(void); float JudgeGetChassisPower(void);
#pragma pack()
#endif // !REFEREE_H #endif // !REFEREE_H