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",
"stdlib.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.intelliSenseEngineFallback": "enabled",

37
HAL_N_Middlewares/Src/main.c
View File

@ -40,6 +40,7 @@
#include "motor_task.h"
#include "referee.h"
#include "ins_task.h"
#include "can_comm.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -71,14 +72,21 @@ void MX_FREERTOS_Init(void);
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
typedef struct
{
float a;
float b;
float c;
}sdf;
volatile sdf* rx;
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void){
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
@ -125,9 +133,17 @@ int main(void){
.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_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);
/* USER CODE END 2 */
/* 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 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
INS_Init();
// INS_Init();
while (1)
{
/* USER CODE END WHILE */
DJIMotorSetRef(djimotor, 10);
MotorControlTask();
HAL_Delay(1);
INS_Task();
// DJIMotorSetRef(djimotor, 10);
// MotorControlTask();
HAL_Delay(10);
CANCommSend(in, (uint8_t*)&tx);
tx+=1;
rx=(sdf*)CANCommGet(in);
// INS_Task();
/* USER CODE BEGIN 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/Config \
-IHAL_N_Middlewares/Middlewares/ST/ARM/DSP/Inc \
-Iapplication/chassis \
-Iapplication/shoot \
-Iapplication/gimbal \
-Iapplication/cmd \
-Iapplication \
-Ibsp \
-Imodules/algorithm \
@ -219,7 +223,8 @@ C_INCLUDES = \
-Imodules/motor \
-Imodules/referee \
-Imodules/remote \
-Imodules/super_cap
-Imodules/super_cap \
-Imodules/can_comm
# compile gcc flags
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 <pub_sub.h>
void chassis_board_com_lost(void* obj) {
//暂时没有效用
}
chassis_board_cmd::chassis_board_cmd() : sender([&] {
can_send<chassis_board_send>::can_send_config config;
config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x003;
return config;
}()),
recver([&] {
can_recv<gimbal_board_send>::can_recv_config config;
config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x004;
config.lost_callback = chassis_board_com_lost;
return config;
}()),
board_buzzer([&] {
buzzer::buzzer_config config;
//底盘音乐2
config.music = &buzzer::buzzer_musics[2];
config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
return config;
}()) {
robot_mode = robot_stop;
robot_ready = 0;
chassis_upload_data = NULL;
board_recv = recver.recv_data;
memset(&chassis_control, 0, sizeof(cmd_chassis));
memset(&board_send, 0, sizeof(chassis_board_send));
}
// #include <chassis_board_cmd.h>
// #include <pub_sub.h>
// void chassis_board_com_lost(void* obj) {
// //暂时没有效用
// }
// chassis_board_cmd::chassis_board_cmd() : sender([&] {
// can_send<chassis_board_send>::can_send_config config;
// config.device = &BSP_CanTypeDef::can_devices[1];
// config.can_identifier = 0x003;
// return config;
// }()),
// recver([&] {
// can_recv<gimbal_board_send>::can_recv_config config;
// config.device = &BSP_CanTypeDef::can_devices[1];
// config.can_identifier = 0x004;
// config.lost_callback = chassis_board_com_lost;
// return config;
// }()),
// board_buzzer([&] {
// buzzer::buzzer_config config;
// //底盘音乐2
// config.music = &buzzer::buzzer_musics[2];
// config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
// return config;
// }()) {
// robot_mode = robot_stop;
// robot_ready = 0;
// chassis_upload_data = NULL;
// board_recv = recver.recv_data;
// memset(&chassis_control, 0, sizeof(cmd_chassis));
// memset(&board_send, 0, sizeof(chassis_board_send));
// }
void chassis_board_cmd::update() {
//初始化为RUN
robot_mode = robot_run;
// void chassis_board_cmd::update() {
// //初始化为RUN
// robot_mode = robot_run;
//判断板间通信在线
if (!recver.is_online()) {
robot_mode = robot_stop; //板间通信掉线，机器人停止
}
// //判断板间通信在线
// if (!recver.is_online()) {
// robot_mode = robot_stop; //板间通信掉线，机器人停止
// }
//接收底盘回传信息，判断底盘IMU在线且初始化完成
static subscriber<upload_chassis*> chassis_upload_suber("upload_chassis");
if (!chassis_upload_suber.empty()) {
chassis_upload_data = chassis_upload_suber.pop();
if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线
robot_mode = robot_stop;
}
}
// //接收底盘回传信息，判断底盘IMU在线且初始化完成
// static subscriber<upload_chassis*> chassis_upload_suber("upload_chassis");
// if (!chassis_upload_suber.empty()) {
// chassis_upload_data = chassis_upload_suber.pop();
// if (chassis_upload_data->chassis_status == module_lost) { //底盘模块掉线
// robot_mode = robot_stop;
// }
// }
if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成，第一次回传数据未收到
robot_mode = robot_stop;
} else {
board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2];
}
// if (chassis_upload_data == NULL) { //底盘模块初始化尚未完成，第一次回传数据未收到
// robot_mode = robot_stop;
// } else {
// board_send.gyro_yaw = chassis_upload_data->chassis_imu->euler[2];
// }
//判断裁判系统是否在线，并处理掉线情况（未实现）
board_send.heat_limit_remain = 30; //读取裁判系统
board_send.bullet_speed_max = 30; //读取裁判系统
// //判断裁判系统是否在线，并处理掉线情况（未实现）
// board_send.heat_limit_remain = 30; //读取裁判系统
// board_send.bullet_speed_max = 30; //读取裁判系统
//判断除了云台板stop之外，都已经上线，说明底盘板初始化完成，进入ready状态
if (robot_mode == robot_run) {
if (!robot_ready) {
robot_ready = 1;
board_buzzer.start(); //播放音乐
}
board_send.chassis_board_status = module_run;
} else {
board_send.chassis_board_status = module_lost;
}
// //判断除了云台板stop之外，都已经上线，说明底盘板初始化完成，进入ready状态
// if (robot_mode == robot_run) {
// if (!robot_ready) {
// robot_ready = 1;
// board_buzzer.start(); //播放音乐
// }
// board_send.chassis_board_status = module_run;
// } else {
// board_send.chassis_board_status = module_lost;
// }
//云台板进入stop模式
if (board_recv->robot_mode == robot_stop) {
robot_mode = robot_stop;
}
// //云台板进入stop模式
// if (board_recv->robot_mode == robot_stop) {
// robot_mode = robot_stop;
// }
if (robot_mode == robot_stop) {
// STOP模式
chassis_control.mode = chassis_stop;
} else {
// RUN模式
// 底盘控制指令
chassis_control.mode = board_recv->chassis_mode;
chassis_control.speed = board_recv->chassis_speed;
chassis_control.power.power_buffer = 0; //应该由裁判系统获得，未实现
chassis_control.power.power_limit = 50; //应该由裁判系统获得，未实现
}
// if (robot_mode == robot_stop) {
// // STOP模式
// chassis_control.mode = chassis_stop;
// } else {
// // RUN模式
// // 底盘控制指令
// chassis_control.mode = board_recv->chassis_mode;
// chassis_control.speed = board_recv->chassis_speed;
// chassis_control.power.power_buffer = 0; //应该由裁判系统获得，未实现
// chassis_control.power.power_limit = 50; //应该由裁判系统获得，未实现
// }
//发布指令
static publisher<cmd_chassis*> chassis_puber("cmd_chassis");
chassis_puber.push(&chassis_control);
// //发布指令
// static publisher<cmd_chassis*> chassis_puber("cmd_chassis");
// 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_
#define _CHASSIS_BOARD_CMD_H_
#include <buzzer.h>
#include <can_recv.h>
#include <can_send.h>
#include <robot_def.h>
#include <stdint.h>
class chassis_board_cmd {
private:
Robot_mode robot_mode; //机器人状态
cmd_chassis chassis_control; //发送给底盘的控制量
upload_chassis* chassis_upload_data; //底盘模块回传数据
chassis_board_send board_send; //发送给云台主控的数据
can_send<chassis_board_send> sender; //板间通信发送
can_recv<gimbal_board_send> recver; //板间通信接收
gimbal_board_send* board_recv; //板间通信接收数据指针
void* referee; //裁判系统（尚未完成）
buzzer board_buzzer; //蜂鸣器
uint8_t robot_ready; //底盘板是否准备好
public:
chassis_board_cmd();
void update();
};
// #ifndef _CHASSIS_BOARD_CMD_H_
// #define _CHASSIS_BOARD_CMD_H_
// #include <buzzer.h>
// #include <can_recv.h>
// #include <can_send.h>
// #include <robot_def.h>
// #include <stdint.h>
// class chassis_board_cmd {
// private:
// Robot_mode robot_mode; //机器人状态
// cmd_chassis chassis_control; //发送给底盘的控制量
// upload_chassis* chassis_upload_data; //底盘模块回传数据
// chassis_board_send board_send; //发送给云台主控的数据
// can_send<chassis_board_send> sender; //板间通信发送
// can_recv<gimbal_board_send> recver; //板间通信接收
// gimbal_board_send* board_recv; //板间通信接收数据指针
// void* referee; //裁判系统（尚未完成）
// buzzer board_buzzer; //蜂鸣器
// uint8_t robot_ready; //底盘板是否准备好
// public:
// chassis_board_cmd();
// 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_log.h"
#include "bsp_uart.h"
#include "common.h"
#include "pub_sub.h"
#include "string.h"
#define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值
// #include "bsp_can.h"
// #include "bsp_log.h"
// #include "bsp_uart.h"
// #include "common.h"
// #include "pub_sub.h"
// #include "string.h"
// #define INIT_FORWARD 3152 // 云台朝向底盘正前时云台yaw编码器值
void gimbal_board_com_lost(void* obj) {
//暂时没有效用
}
// void gimbal_board_com_lost(void* obj) {
// //暂时没有效用
// }
//此处涉及到lamdba表达式的应用
gimbal_board_cmd::gimbal_board_cmd() : sender([&] {
//板间通信：发
can_send<gimbal_board_send>::can_send_config config;
config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x004;
return config;
}()),
recver([&] {
//板间通信：收
can_recv<chassis_board_send>::can_recv_config config;
config.device = &BSP_CanTypeDef::can_devices[1];
config.can_identifier = 0x003;
config.notify_func = NULL;
config.lost_callback = gimbal_board_com_lost;
return config;
}()),
pc([&] {
//小电脑通信配置
canpc::canpc_config config;
config.device = &BSP_CanTypeDef::can_devices[0];
config.recv_identifer = 0x001;
return config;
}()),
remote([&] {
dt7Remote::dt7_config config;
config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT];
return config;
}()),
board_buzzer([&] {
buzzer::buzzer_config config;
config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
config.music = &buzzer::buzzer_musics[1];
return config;
}()) {
robot_mode = robot_stop;
robot_ready = 0;
autoaim_mode = auto_aim_off;
gimbal_upload_data = NULL;
//指针指向接收的实际数据
board_recv = recver.recv_data;
memset(&gimbal_control, 0, sizeof(cmd_gimbal));
memset(&shoot_control, 0, sizeof(cmd_shoot));
memset(&board_send, 0, sizeof(gimbal_board_send));
}
// //此处涉及到lamdba表达式的应用
// gimbal_board_cmd::gimbal_board_cmd() : sender([&] {
// //板间通信：发
// can_send<gimbal_board_send>::can_send_config config;
// config.device = &BSP_CanTypeDef::can_devices[1];
// config.can_identifier = 0x004;
// return config;
// }()),
// recver([&] {
// //板间通信：收
// can_recv<chassis_board_send>::can_recv_config config;
// config.device = &BSP_CanTypeDef::can_devices[1];
// config.can_identifier = 0x003;
// config.notify_func = NULL;
// config.lost_callback = gimbal_board_com_lost;
// return config;
// }()),
// pc([&] {
// //小电脑通信配置
// canpc::canpc_config config;
// config.device = &BSP_CanTypeDef::can_devices[0];
// config.recv_identifer = 0x001;
// return config;
// }()),
// remote([&] {
// dt7Remote::dt7_config config;
// config.uart_device = &BSP_UART_Typedef::uart_ports[UART_REMOTE_PORT];
// return config;
// }()),
// board_buzzer([&] {
// buzzer::buzzer_config config;
// config.pwm_device = &BSP_PWM_Typedef::pwm_ports[PWM_BUZZER_PORT];
// config.music = &buzzer::buzzer_musics[1];
// return config;
// }()) {
// robot_mode = robot_stop;
// robot_ready = 0;
// autoaim_mode = auto_aim_off;
// gimbal_upload_data = NULL;
// //指针指向接收的实际数据
// board_recv = recver.recv_data;
// memset(&gimbal_control, 0, sizeof(cmd_gimbal));
// memset(&shoot_control, 0, sizeof(cmd_shoot));
// memset(&board_send, 0, sizeof(gimbal_board_send));
// }
float get_offset_angle(short init_forward, short now_encoder) {
short tmp = 0;
if (init_forward < 4096) {
if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) {
tmp = now_encoder - init_forward;
} else if (now_encoder > 4096 + init_forward) {
tmp = -8192 + now_encoder - init_forward;
} else {
tmp = now_encoder - init_forward;
}
} else {
if (now_encoder > init_forward) {
tmp = now_encoder - init_forward;
} else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) {
tmp = now_encoder - init_forward;
} else {
tmp = now_encoder + 8192 - init_forward;
}
}
return tmp * 360.0 / 8192.0;
}
// float get_offset_angle(short init_forward, short now_encoder) {
// short tmp = 0;
// if (init_forward < 4096) {
// if (now_encoder > init_forward && now_encoder <= 4096 + init_forward) {
// tmp = now_encoder - init_forward;
// } else if (now_encoder > 4096 + init_forward) {
// tmp = -8192 + now_encoder - init_forward;
// } else {
// tmp = now_encoder - init_forward;
// }
// } else {
// if (now_encoder > init_forward) {
// tmp = now_encoder - init_forward;
// } else if (now_encoder <= init_forward && now_encoder >= init_forward - 4096) {
// tmp = now_encoder - init_forward;
// } else {
// tmp = now_encoder + 8192 - init_forward;
// }
// }
// return tmp * 360.0 / 8192.0;
// }
void gimbal_board_cmd::update() {
//第一步，判断机器人工作模式
// void gimbal_board_cmd::update() {
// //第一步，判断机器人工作模式
//初始化为RUN
robot_mode = robot_run;
// //初始化为RUN
// robot_mode = robot_run;
//判断板间通信在线
if (!recver.is_online()) {
robot_mode = robot_stop; //板间通信掉线，机器人停止
}
// //判断板间通信在线
// if (!recver.is_online()) {
// robot_mode = robot_stop; //板间通信掉线，机器人停止
// }
//接收云台回传信息，判断云台IMU在线且初始化完成
static subscriber<upload_gimbal*> gimbal_upload_suber("upload_gimbal");
if (!gimbal_upload_suber.empty()) {
gimbal_upload_data = gimbal_upload_suber.pop();
if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线
robot_mode = robot_stop;
}
//计算云台和底盘的夹角 offset_angle
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[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.auto_mode_flag = auto_aim_normal;
}
// //接收云台回传信息，判断云台IMU在线且初始化完成
// static subscriber<upload_gimbal*> gimbal_upload_suber("upload_gimbal");
// if (!gimbal_upload_suber.empty()) {
// gimbal_upload_data = gimbal_upload_suber.pop();
// if (gimbal_upload_data->gimbal_status == module_lost) { //云台模块掉线
// robot_mode = robot_stop;
// }
// //计算云台和底盘的夹角 offset_angle
// 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[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.auto_mode_flag = auto_aim_normal;
// }
if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成，第一次回传数据未收到
robot_mode = robot_stop;
}
// if (gimbal_upload_data == NULL) { //云台模块初始化尚未完成，第一次回传数据未收到
// robot_mode = robot_stop;
// }
//如果除遥控器之外都已经上线，说明机器人初始化完成，进入ready状态
if (robot_mode == robot_run) {
if (!robot_ready) {
robot_ready = 1;
board_buzzer.start(); //蜂鸣器播放初始化提示音
}
}
// //如果除遥控器之外都已经上线，说明机器人初始化完成，进入ready状态
// if (robot_mode == robot_run) {
// if (!robot_ready) {
// robot_ready = 1;
// board_buzzer.start(); //蜂鸣器播放初始化提示音
// }
// }
//遥控器判断
if (!remote.monitor_item.is_online()) {
//遥控器掉线
robot_mode = robot_stop;
} else if (remote.data.input_mode == dt7Remote::RC_Stop) {
//遥控器处于stop档位
robot_mode = robot_stop;
}
// //遥控器判断
// if (!remote.monitor_item.is_online()) {
// //遥控器掉线
// robot_mode = robot_stop;
// } else if (remote.data.input_mode == dt7Remote::RC_Stop) {
// //遥控器处于stop档位
// robot_mode = robot_stop;
// }
//第二步，机器人控制主要逻辑
// STOP模式
if (robot_mode == robot_stop) {
stop_mode_update();
}
// 运行模式
if (robot_mode == robot_run) {
//遥控器控制模式
if (remote.data.input_mode == dt7Remote::RC_Remote) {
remote_mode_update();
} else if (remote.data.input_mode == dt7Remote::RC_MouseKey) {
mouse_key_mode_update();
}
}
//发布命令，板间通信
send_cmd_and_data();
}
// //第二步，机器人控制主要逻辑
// // STOP模式
// if (robot_mode == robot_stop) {
// stop_mode_update();
// }
// // 运行模式
// if (robot_mode == robot_run) {
// //遥控器控制模式
// if (remote.data.input_mode == dt7Remote::RC_Remote) {
// remote_mode_update();
// } else if (remote.data.input_mode == dt7Remote::RC_MouseKey) {
// mouse_key_mode_update();
// }
// }
// //发布命令，板间通信
// send_cmd_and_data();
// }
void gimbal_board_cmd::stop_mode_update() {
board_send.robot_mode = robot_stop;
board_send.chassis_mode = chassis_stop;
gimbal_control.gimbal_mode = gimbal_stop;
shoot_control.shoot_mode = shoot_stop;
shoot_control.bullet_mode = bullet_holdon;
}
// void gimbal_board_cmd::stop_mode_update() {
// board_send.robot_mode = robot_stop;
// board_send.chassis_mode = chassis_stop;
// gimbal_control.gimbal_mode = gimbal_stop;
// shoot_control.shoot_mode = shoot_stop;
// shoot_control.bullet_mode = bullet_holdon;
// }
void gimbal_board_cmd::remote_mode_update() {
//云台控制参数
gimbal_control.gimbal_mode = gimbal_run;
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.rotate_feedforward = 0; //目前没有使用小陀螺前馈
// void gimbal_board_cmd::remote_mode_update() {
// //云台控制参数
// gimbal_control.gimbal_mode = gimbal_run;
// 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.rotate_feedforward = 0; //目前没有使用小陀螺前馈
//底盘控制参数
if (remote.data.rc.s1 == 1) {
//小陀螺模式
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.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS);
} else {
//底盘跟随模式
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.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS);
}
// //底盘控制参数
// if (remote.data.rc.s1 == 1) {
// //小陀螺模式
// 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.vx = 16.0 * 0.6 * (float)(remote.data.rc.ch0 - CHx_BIAS);
// } else {
// //底盘跟随模式
// 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.vx = 16.0 * (float)(remote.data.rc.ch0 - CHx_BIAS);
// }
//发射机构控制
if (remote.data.rc.s1 == 2) {
shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮
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_close;
} else {
//发弹控制
shoot_control.shoot_mode = shoot_run; //开摩擦轮
shoot_control.bullet_mode = bullet_continuous; //连续发射
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.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
}
}
// //发射机构控制
// if (remote.data.rc.s1 == 2) {
// shoot_control.bullet_mode = bullet_holdon; //在发射停止位关闭播弹轮
// 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_close;
// } else {
// //发弹控制
// shoot_control.shoot_mode = shoot_run; //开摩擦轮
// shoot_control.bullet_mode = bullet_continuous; //连续发射
// 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.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
// }
// }
void gimbal_board_cmd::mouse_key_mode_update() {
// 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) {
board_send.chassis_mode = chassis_rotate_run; //小陀螺模式
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
}
}
// 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) {
board_send.chassis_mode = chassis_run; //底盘独立模式
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
} else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
}
}
// 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) {
board_send.chassis_mode = chassis_run; //底盘独立模式
gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式
} else {
board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
}
}
// z:爬坡模式（待添加）
// f:正常自瞄模式
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_normal) {
autoaim_mode = auto_aim_normal;
} else {
autoaim_mode = auto_aim_off;
}
}
// g:小符
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_buff_small) {
autoaim_mode = auto_aim_buff_small;
} else {
autoaim_mode = auto_aim_off;
}
}
// b:大符
if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
if (autoaim_mode != auto_aim_buff_big) {
autoaim_mode = auto_aim_buff_big;
} else {
autoaim_mode = auto_aim_off;
}
}
pc.pc_send_data.auto_mode_flag = autoaim_mode;
// c:开关弹仓
if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) {
if (shoot_control.mag_mode == magazine_open) {
shoot_control.mag_mode = magazine_close;
} else {
shoot_control.mag_mode = magazine_open;
}
}
//底盘控制参数
// wasd
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.d) board_send.chassis_speed.vx = 8000;
if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000;
// void gimbal_board_cmd::mouse_key_mode_update() {
// // 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) {
// board_send.chassis_mode = chassis_rotate_run; //小陀螺模式
// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
// } else {
// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
// }
// }
// // 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) {
// board_send.chassis_mode = chassis_run; //底盘独立模式
// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
// } else {
// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
// }
// }
// // 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) {
// board_send.chassis_mode = chassis_run; //底盘独立模式
// gimbal_control.gimbal_mode = gimbal_run; //云台跟随底盘模式
// } else {
// board_send.chassis_mode = chassis_run_follow_offset; //底盘跟随云台
// gimbal_control.gimbal_mode = gimbal_run; //云台正常模式
// }
// }
// // z:爬坡模式（待添加）
// // f:正常自瞄模式
// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
// if (autoaim_mode != auto_aim_normal) {
// autoaim_mode = auto_aim_normal;
// } else {
// autoaim_mode = auto_aim_off;
// }
// }
// // g:小符
// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
// if (autoaim_mode != auto_aim_buff_small) {
// autoaim_mode = auto_aim_buff_small;
// } else {
// autoaim_mode = auto_aim_off;
// }
// }
// // b:大符
// if (remote.data.key_single_press_cnt.f != remote.last_data.key_single_press_cnt.f) {
// if (autoaim_mode != auto_aim_buff_big) {
// autoaim_mode = auto_aim_buff_big;
// } else {
// autoaim_mode = auto_aim_off;
// }
// }
// pc.pc_send_data.auto_mode_flag = autoaim_mode;
// // c:开关弹仓
// if (remote.data.key_single_press_cnt.c != remote.last_data.key_single_press_cnt.c) {
// if (shoot_control.mag_mode == magazine_open) {
// shoot_control.mag_mode = magazine_close;
// } else {
// shoot_control.mag_mode = magazine_open;
// }
// }
// //底盘控制参数
// // wasd
// 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.d) board_send.chassis_speed.vx = 8000;
// if (remote.data.key_down.a) board_send.chassis_speed.vx = -8000;
//按住ctrl减速
if (remote.data.key_down.ctrl) {
board_send.chassis_speed.vx /= 3.0;
board_send.chassis_speed.vy /= 3.0;
}
// //按住ctrl减速
// if (remote.data.key_down.ctrl) {
// board_send.chassis_speed.vx /= 3.0;
// board_send.chassis_speed.vy /= 3.0;
// }
//按住shift加速
if (remote.data.key_down.shift) {
board_send.chassis_speed.vx *= 3.0;
board_send.chassis_speed.vy *= 3.0;
}
// //按住shift加速
// if (remote.data.key_down.shift) {
// board_send.chassis_speed.vx *= 3.0;
// board_send.chassis_speed.vy *= 3.0;
// }
// q和e
if (remote.data.key_down.q) {
if (board_send.chassis_mode == chassis_run) {
board_send.chassis_speed.rotate = -90;
} else if (board_send.chassis_mode == chassis_run_follow_offset) {
gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘
}
}
// // q和e
// if (remote.data.key_down.q) {
// if (board_send.chassis_mode == chassis_run) {
// board_send.chassis_speed.rotate = -90;
// } else if (board_send.chassis_mode == chassis_run_follow_offset) {
// gimbal_control.yaw -= 15; //通过控制云台yaw轴带动底盘
// }
// }
if (remote.data.key_down.e) {
if (board_send.chassis_mode == chassis_run) {
board_send.chassis_speed.rotate = 90;
} else if (board_send.chassis_mode == chassis_run_follow_offset) {
gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘
}
}
// if (remote.data.key_down.e) {
// if (board_send.chassis_mode == chassis_run) {
// board_send.chassis_speed.rotate = 90;
// } else if (board_send.chassis_mode == chassis_run_follow_offset) {
// gimbal_control.yaw += 15; //通过控制云台yaw轴带动底盘
// }
// }
//云台控制参数
if (gimbal_control.gimbal_mode == gimbal_run) {
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.pitch -= -0.1f * ((float)remote.data.mouse.y);
} else {
//计算真实yaw值
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;
gimbal_control.yaw = yaw_target;
gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定，pitch轴反馈为陀螺仪roll
}
} else if (gimbal_control.gimbal_mode == gimbal_middle) {
//云台跟随底盘模式，待完善
}
// //云台控制参数
// if (gimbal_control.gimbal_mode == gimbal_run) {
// 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.pitch -= -0.1f * ((float)remote.data.mouse.y);
// } else {
// //计算真实yaw值
// 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;
// gimbal_control.yaw = yaw_target;
// gimbal_control.pitch = pc.pc_recv_data->roll * 8192.0 / 2 / pi; // 根据当前情况决定，pitch轴反馈为陀螺仪roll
// }
// } else if (gimbal_control.gimbal_mode == gimbal_middle) {
// //云台跟随底盘模式，待完善
// }
//发射机构控制参数
if (remote.data.rc.s1 == 2) {
shoot_control.bullet_mode = bullet_holdon;
shoot_control.shoot_mode = shoot_stop;
} else {
//发弹控制，单发，双发, 射频和小电脑控制待完善
shoot_control.shoot_mode = shoot_run; //开摩擦轮
shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余
shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
shoot_control.fire_rate = 3; //固定射频
if (remote.data.mouse.press_l) {
shoot_control.bullet_mode = bullet_continuous;
} else {
shoot_control.bullet_mode = bullet_holdon;
}
}
}
// //发射机构控制参数
// if (remote.data.rc.s1 == 2) {
// shoot_control.bullet_mode = bullet_holdon;
// shoot_control.shoot_mode = shoot_stop;
// } else {
// //发弹控制，单发，双发, 射频和小电脑控制待完善
// shoot_control.shoot_mode = shoot_run; //开摩擦轮
// shoot_control.heat_limit_remain = board_recv->heat_limit_remain; //下板传回的热量剩余
// shoot_control.bullet_speed = board_recv->bullet_speed_max; //下板传回的子弹速度上限
// shoot_control.fire_rate = 3; //固定射频
// if (remote.data.mouse.press_l) {
// shoot_control.bullet_mode = bullet_continuous;
// } else {
// shoot_control.bullet_mode = bullet_holdon;
// }
// }
// }
void gimbal_board_cmd::send_cmd_and_data() {
static publisher<cmd_gimbal*> gimbal_puber("cmd_gimbal");
static publisher<cmd_shoot*> shoot_puber("cmd_shoot");
gimbal_puber.push(&gimbal_control);
shoot_puber.push(&shoot_control);
sender.send(board_send);
pc.send(pc.pc_send_data);
}
// void gimbal_board_cmd::send_cmd_and_data() {
// static publisher<cmd_gimbal*> gimbal_puber("cmd_gimbal");
// static publisher<cmd_shoot*> shoot_puber("cmd_shoot");
// gimbal_puber.push(&gimbal_control);
// shoot_puber.push(&shoot_control);
// sender.send(board_send);
// pc.send(pc.pc_send_data);
// }

62
application/cmd/gimbal_cmd.h
View File

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

12
bsp/bsp_can.c
View File

@ -57,7 +57,7 @@ static void CANServiceInit()
/* -----------------------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;
if (!idx)
@ -66,15 +66,15 @@ void CANRegister(can_instance *ins, can_instance_config_s config)
}
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.RTR = CAN_RTR_DATA;
instance[idx]->txconf.DLC = 0x08;
instance[idx]->can_handle = config.can_handle;
instance[idx]->tx_id = config.tx_id;
instance[idx]->rx_id = config.rx_id;
instance[idx]->can_module_callback = config.can_module_callback;
instance[idx]->can_handle = config->can_handle;
instance[idx]->tx_id = config->tx_id;
instance[idx]->rx_id = config->rx_id;
instance[idx]->can_module_callback = config->can_module_callback;
CANAddFilter(instance[idx++]);
}

11
bsp/bsp_can.h
View File

@ -9,7 +9,9 @@
#define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless
#define DEVICE_CAN_CNT 2 // CAN1,CAN2
/* can instance typedef, every module registered to CAN should have this variable */
#pragma pack(1)
typedef struct _
{
CAN_HandleTypeDef *can_handle;
@ -22,6 +24,7 @@ typedef struct _
uint8_t rx_len;
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
} can_instance;
#pragma pack()
/* this structure is used as initialization*/
typedef struct
@ -32,8 +35,8 @@ typedef struct
void (*can_module_callback)(can_instance *);
} can_instance_config_s;
/* module callback,which resolve protocol when new mesg arrives*/
typedef void (*can_callback)(can_instance *);
/* module callback,which resolve protocol when new mesg arrives */
typedef void (*can_callback)(can_instance*);
/**
* @brief transmit mesg through CAN device
@ -48,7 +51,7 @@ void CANTransmit(can_instance *_instance);
* @param config init config
* @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
@ -56,6 +59,6 @@ void CANRegister(can_instance *instance, can_instance_config_s config);
* @param _instance 要修改长度的can实例
* @param length 设定长度
*/
void CANSetDLC(can_instance *_instance,uint8_t length);
void CANSetDLC(can_instance *_instance, uint8_t length);
#endif

93
modules/can_comm/can_comm.c
View File

@ -5,7 +5,19 @@
/* can_comm用于保存每个实例的指针数组,用于回调函数区分实例 */
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 重置CAN comm的接收状态和buffer
*
* @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
@ -18,75 +30,84 @@ static void CANCommRxCallback(can_instance *_instance)
{
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
return ; //直接跳过即可
return; // 直接跳过即可
}
if(can_comm_instance[i]->recv_state) //已经开始接收
{ // 直接拷贝到当前的接收buffer后面
memcpy(can_comm_instance[i]->raw_recvbuf+can_comm_instance[i]->cur_recv_len,_instance->rx_buff,8);
can_comm_instance[i]->cur_recv_len+=8;
// 当前已经收满
if(can_comm_instance[i]->cur_recv_len>=can_comm_instance[i]->recv_buf_len)
{ // 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]->recv_state) // 已经收到过帧头
{ // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
if (can_comm_instance[i]->cur_recv_len + _instance->rx_len > can_comm_instance[i]->recv_buf_len)
{
memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len);
can_comm_instance[i]->recv_state=0;
can_comm_instance[i]->cur_recv_len=0;
return ; // 重置状态然后返回
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)
{ // buff里本该是tail的位置不等于CAN_COMM_TAIL
if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 1] != CAN_COMM_TAIL)
{
CANCommResetRx(can_comm_instance[i]);
return; // 重置状态然后返回
}
else // tail正确, 对数据进行crc8校验
{
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))
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))
{ // 通过校验,复制数据到unpack_data中
memcpy(can_comm_instance[i]->raw_recvbuf+2,can_comm_instance[i]->unpacked_recv_data,can_comm_instance[i]->recv_data_len);
can_comm_instance[i]->update_flag=1; //数据更新flag置为1
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
}
memset(can_comm_instance[i]->raw_recvbuf,0,can_comm_instance[i]->recv_buf_len); //整个buff置零
can_comm_instance[i]->recv_state=0;
can_comm_instance[i]->cur_recv_len=0;
return ; // 重置状态然后返回
CANCommResetRx(can_comm_instance[i]);
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));
memset(can_comm_instance[idx], 0, sizeof(CANCommInstance));
can_comm_instance[idx]->recv_data_len = config.recv_data_len;
can_comm_instance[idx]->send_data_len = config.send_data_len;
can_comm_instance[idx]->send_buf_len = config.send_data_len + CAN_COMM_OFFSET_BYTES;
can_comm_instance[idx]->recv_data_len = comm_config->recv_data_len;
can_comm_instance[idx]->recv_buf_len = comm_config->recv_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[1] = 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[1] = comm_config->send_data_len;
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;
CANRegister(&can_comm_instance[idx]->can_ins, config.can_config);
comm_config->can_config.can_module_callback = CANCommRxCallback;
CANRegister(&can_comm_instance[idx]->can_ins, &comm_config->can_config);
return can_comm_instance[idx++];
}
void CANCommSend(CANCommInstance *instance, uint8_t *data)
{
static uint8_t crc8;
static uint8_t send_len;
memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len);
crc8 = crc_8(data, instance->send_data_len);
instance->raw_sendbuf[2 + instance->send_data_len] = crc8;
for (size_t i = 0; i < instance->send_buf_len; i += 8)
{
memcpy(instance->can_ins.tx_buff, instance->raw_sendbuf[i], 8);
{ // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可
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);
}
}

26
modules/can_comm/can_comm.h
View File

@ -11,40 +11,43 @@
#ifndef CAN_COMM_H
#define CAN_COMM_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_TAIL 'e'
#define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8
#pragma pack(1)
/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */
typedef struct
{
can_instance can_ins;
/* 发送部分 */
uint8_t send_data_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_buf_len;
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 raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
/* 接收和更新标志位*/
uint8_t recv_state;
uint8_t cur_recv_len;
uint8_t update_flag;
} CANCommInstance;
#pragma pack()
/* CAN comm 初始化结构体 */
typedef struct
{
can_instance_config_s can_config;
uint8_t send_data_len;
uint8_t recv_data_len;
can_instance_config_s can_config;
}CANComm_Init_Config_s;
} CANComm_Init_Config_s;
/**
* @brief
@ -52,7 +55,7 @@ typedef struct
* @param config
* @return CANCommInstance*
*/
CANCommInstance* CANCommInit(CANComm_Init_Config_s config);
CANCommInstance *CANCommInit(CANComm_Init_Config_s* comm_config);
/**
* @brief 发送数据
@ -60,7 +63,7 @@ CANCommInstance* CANCommInit(CANComm_Init_Config_s config);
* @param instance can comm实例
* @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同
*/
void CANCommSend(CANCommInstance* instance,uint8_t* data);
void CANCommSend(CANCommInstance *instance, uint8_t *data);
/**
* @brief 获取CAN COMM接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型
@ -69,7 +72,6 @@ void CANCommSend(CANCommInstance* instance,uint8_t* data);
* @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n)
* CAN COMM接收到的数据可以看作是pack(1)之后的,是连续存放的
*/
void* CANCommGet(CANCommInstance* instance);
void *CANCommGet(CANCommInstance *instance);
#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;
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++];
}

2
modules/motor/LK9025.c
View File

@ -23,7 +23,7 @@ driven_instance *LKMotroInit(can_instance_config_s config)
static uint8_t idx;
driven_motor_info[idx] = (driven_instance *)malloc(sizeof(driven_instance));
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++];
}

25
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)
{
// 由于需要多次变址访存,直接将buff和measure地址保存在寄存器里避免多次存取
static uint8_t *rxbuff;
static dji_motor_measure *measure;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
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
measure->last_ecd = measure->ecd;
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->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
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));
memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance));
// motor basic setting
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_type = config->motor_type;
dji_motor_info[idx]->motor_settings = config->controller_setting_init_config;
// 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.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);
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;
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.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_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
// 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
config.can_init_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, config.can_init_config);
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
CANRegister(&dji_motor_info[idx]->motor_can_instance, &config->can_init_config);
return dji_motor_info[idx++];
}

5
modules/motor/dji_motor.h
View File

@ -20,6 +20,8 @@
#include "motor_def.h"
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#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
@ -77,7 +79,7 @@ typedef struct
*
* @return dji_motor_instance*
*/
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s config);
dji_motor_instance *DJIMotorInit(Motor_Init_Config_s *config);
/**
* @brief 被application层的应用调用,给电机设定参考值.
@ -103,4 +105,5 @@ void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedba
*/
void DJIMotorControl();
#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)))
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: 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);
float JudgeGetChassisPower(void);
#pragma pack()
#endif // !REFEREE_H