diff --git a/.vscode/launch.json b/.vscode/launch.json index 482e0ac..311c392 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -33,6 +33,6 @@ "interface": "swd", "svdFile": ".\\STM32F407.svd", // "preLaunchTask": "build task",//先运行Build任务,取消注释即可使用 - } + }, ] } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 944a8bb..94e9aae 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -7,7 +7,10 @@ "usart.h": "c", "controller.h": "c", "ins_task.h": "c", - "task.h": "c" + "task.h": "c", + "user_lib.h": "c", + "chassis.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", + "C_Cpp.intelliSenseEngineFallback": "enabled", } \ No newline at end of file diff --git a/HAL_N_Middlewares/Src/main.c b/HAL_N_Middlewares/Src/main.c index 3478879..02f63e2 100644 --- a/HAL_N_Middlewares/Src/main.c +++ b/HAL_N_Middlewares/Src/main.c @@ -39,6 +39,7 @@ #include "dji_motor.h" #include "motor_task.h" #include "referee.h" +#include "ins_task.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -138,13 +139,15 @@ int main(void){ /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ + INS_Init(); while (1) { /* USER CODE END WHILE */ DJIMotorSetRef(djimotor, 10); MotorControlTask(); - HAL_Delay(100); + HAL_Delay(1); + INS_Task(); /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ diff --git a/Makefile b/Makefile index efcbe1c..53ebedb 100644 --- a/Makefile +++ b/Makefile @@ -108,11 +108,15 @@ bsp/bsp_init.c \ modules/algorithm/controller.c \ modules/algorithm/kalman_filter.c \ modules/algorithm/QuaternionEKF.c \ +modules/algorithm/crc8.c \ +modules/algorithm/crc16.c \ +modules/algorithm/user_lib.c \ modules/imu/BMI088driver.c \ modules/imu/BMI088Middleware.c \ modules/imu/ins_task.c \ modules/led_light/led_task.c \ modules/master_machine/master_process.c \ +modules/master_machine/seasky_protocol.c \ modules/motor/dji_motor.c \ modules/motor/HT04.c \ modules/motor/LK9025.c \ @@ -124,14 +128,13 @@ modules/referee/referee_UI.c \ modules/referee/referee_communication.c \ modules/remote/remote_control.c \ modules/super_cap/super_cap.c \ -modules/master_machine/seasky_protocol.c \ -modules/algorithm/crc8.c \ -modules/algorithm/crc16.c \ modules/can_comm/can_comm.c \ -application/gimbal.c \ -application/chassis.c \ -application/shoot.c \ -application/robot_cmd.c +application/gimbal/gimbal.c \ +application/chassis/chassis.c \ +application/shoot/shoot.c \ +application/cmd/chassis_cmd.c \ +application/cmd/gimbal_cmd.c \ +application/robot.c # ASM sources diff --git a/application/chassis.c b/application/chassis.c deleted file mode 100644 index e69de29..0000000 diff --git a/application/chassis.h b/application/chassis.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c new file mode 100644 index 0000000..6fbbc3c --- /dev/null +++ b/application/chassis/chassis.c @@ -0,0 +1,14 @@ +#include "chassis.h" + + + + +void ChassisInit() +{ + +} + +void ChassisTask() +{ + +} \ No newline at end of file diff --git a/application/chassis/chassis.h b/application/chassis/chassis.h new file mode 100644 index 0000000..446e952 --- /dev/null +++ b/application/chassis/chassis.h @@ -0,0 +1,29 @@ +#ifndef CHASSIS_H +#define CHASSIS_H + +#include "robot_def.h" +#include "dji_motor.h" +#include "super_cap.h" + +#ifdef CHASSIS_BOARD //使用板载IMU获取底盘转动角速度 +#include "ins_task.h" +#endif // CHASSIS_BOARD + +typedef struct +{ +#ifdef CHASSIS_BOARD + IMU_Data_t Chassis_IMU_data; +#endif // CHASSIS_BOARD + // SuperCAP cap; + dji_motor_instance lf; //left right forward + dji_motor_instance rf; + dji_motor_instance lb; + dji_motor_instance rb; + +} chassis; + +void ChassisInit(); + +void ChassisTask(); + +#endif //CHASSIS_H \ No newline at end of file diff --git a/application/chassis.md b/application/chassis/chassis.md similarity index 100% rename from application/chassis.md rename to application/chassis/chassis.md diff --git a/application/cmd/chassis_cmd.c b/application/cmd/chassis_cmd.c new file mode 100644 index 0000000..ea34c7e --- /dev/null +++ b/application/cmd/chassis_cmd.c @@ -0,0 +1,96 @@ +#include +#include +void chassis_board_com_lost(void* obj) { + //暂时没有效用 +} +chassis_board_cmd::chassis_board_cmd() : sender([&] { + can_send::can_send_config config; + config.device = &BSP_CanTypeDef::can_devices[1]; + config.can_identifier = 0x003; + return config; + }()), + recver([&] { + can_recv::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; + + //判断板间通信在线 + if (!recver.is_online()) { + robot_mode = robot_stop; //板间通信掉线,机器人停止 + } + + //接收底盘回传信息,判断底盘IMU在线且初始化完成 + static subscriber 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]; + } + + //判断裁判系统是否在线,并处理掉线情况(未实现) + 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模式 + 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; //应该由裁判系统获得,未实现 + } + + //发布指令 + static publisher chassis_puber("cmd_chassis"); + chassis_puber.push(&chassis_control); + + //板间通信 + sender.send(board_send); +} \ No newline at end of file diff --git a/application/cmd/chassis_cmd.h b/application/cmd/chassis_cmd.h new file mode 100644 index 0000000..c79e676 --- /dev/null +++ b/application/cmd/chassis_cmd.h @@ -0,0 +1,25 @@ +#ifndef _CHASSIS_BOARD_CMD_H_ +#define _CHASSIS_BOARD_CMD_H_ +#include +#include +#include +#include +#include +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 sender; //板间通信发送 + can_recv recver; //板间通信接收 + gimbal_board_send* board_recv; //板间通信接收数据指针 + void* referee; //裁判系统(尚未完成) + buzzer board_buzzer; //蜂鸣器 + uint8_t robot_ready; //底盘板是否准备好 + public: + chassis_board_cmd(); + void update(); +}; + +#endif \ No newline at end of file diff --git a/application/cmd/gimbal_cmd.c b/application/cmd/gimbal_cmd.c new file mode 100644 index 0000000..adffaf6 --- /dev/null +++ b/application/cmd/gimbal_cmd.c @@ -0,0 +1,336 @@ +#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编码器值 + +void gimbal_board_com_lost(void* obj) { + //暂时没有效用 +} + +//此处涉及到lamdba表达式的应用 +gimbal_board_cmd::gimbal_board_cmd() : sender([&] { + //板间通信:发 + can_send::can_send_config config; + config.device = &BSP_CanTypeDef::can_devices[1]; + config.can_identifier = 0x004; + return config; + }()), + recver([&] { + //板间通信:收 + can_recv::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; +} + +void gimbal_board_cmd::update() { + //第一步,判断机器人工作模式 + + //初始化为RUN + robot_mode = robot_run; + + //判断板间通信在线 + if (!recver.is_online()) { + robot_mode = robot_stop; //板间通信掉线,机器人停止 + } + + //接收云台回传信息,判断云台IMU在线且初始化完成 + static subscriber 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; + } + + //如果除遥控器之外都已经上线,说明机器人初始化完成,进入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; + } + + //第二步,机器人控制主要逻辑 + // 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::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 == 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; + + //按住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; + } + + // 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 (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; + } + } +} + +void gimbal_board_cmd::send_cmd_and_data() { + static publisher gimbal_puber("cmd_gimbal"); + static publisher shoot_puber("cmd_shoot"); + gimbal_puber.push(&gimbal_control); + shoot_puber.push(&shoot_control); + sender.send(board_send); + pc.send(pc.pc_send_data); +} \ No newline at end of file diff --git a/application/cmd/gimbal_cmd.h b/application/cmd/gimbal_cmd.h new file mode 100644 index 0000000..90ad4e8 --- /dev/null +++ b/application/cmd/gimbal_cmd.h @@ -0,0 +1,35 @@ +#ifndef _GIMBAL_ROBOT_CMD_H +#define _GIMBAL_ROBOT_CMD_H +#include +#include +#include +#include +#include +class gimbal_board_cmd { + private: + gimbal_board_send board_send; //需要发送给下板的板间通信数据 + cmd_gimbal gimbal_control; //发送给云台的控制量 + cmd_shoot shoot_control; //发送给发射机构的控制量 + + can_send sender; //板间通信发送 + can_recv 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(); //发布指令和板间通信 + + public: + gimbal_board_cmd(); + void update(); +}; + +#endif \ No newline at end of file diff --git a/application/gimbal.c b/application/gimbal.c deleted file mode 100644 index e69de29..0000000 diff --git a/application/gimbal.h b/application/gimbal.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c new file mode 100644 index 0000000..050ea37 --- /dev/null +++ b/application/gimbal/gimbal.c @@ -0,0 +1,11 @@ +#include "gimbal.h" + +void GimbalInit() +{ + +} + +void GimbalTask() +{ + +} \ No newline at end of file diff --git a/application/gimbal/gimbal.h b/application/gimbal/gimbal.h new file mode 100644 index 0000000..b66b72a --- /dev/null +++ b/application/gimbal/gimbal.h @@ -0,0 +1,19 @@ +#ifndef GIMBAL_H +#define GIMBAL_H + +#include "robot_def.h" +#include "dji_motor.h" +#include "ins_task.h" + +typedef struct +{ + IMU_Data_t Gimbal_IMU_data; + dji_motor_instance yaw; + dji_motor_instance pitch; +} gimbal; + +void GimbalInit(); + +void GimbalTask(); + +#endif //GIMBAL_H \ No newline at end of file diff --git a/application/gimbal.md b/application/gimbal/gimbal.md similarity index 100% rename from application/gimbal.md rename to application/gimbal/gimbal.md diff --git a/application/robot.c b/application/robot.c index e69de29..5d342f8 100644 --- a/application/robot.c +++ b/application/robot.c @@ -0,0 +1,33 @@ +#include "robot.h" +#include "robot_def.h" + +#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) +#include "chassis.h" +#endif +#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) +#include "gimbal.h" +#include "shoot.h" +#endif + +void RobotInit() +{ +#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) + +#endif + +#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) + +#endif +} + +void RobotTask() +{ +#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) + +#endif + +#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) + +#endif +} + diff --git a/application/robot.h b/application/robot.h index edf2f9f..07e78ea 100644 --- a/application/robot.h +++ b/application/robot.h @@ -1,8 +1,6 @@ #ifndef ROBOT_H #define ROBOT_H -#define GIMBAL_BOARD - void RobotInit(); void RobotTask(); diff --git a/application/robot_cmd.c b/application/robot_cmd.c deleted file mode 100644 index e69de29..0000000 diff --git a/application/robot_cmd.h b/application/robot_cmd.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/robot_cmd.md b/application/robot_cmd.md deleted file mode 100644 index e69de29..0000000 diff --git a/application/robot_def.h b/application/robot_def.h new file mode 100644 index 0000000..4287804 --- /dev/null +++ b/application/robot_def.h @@ -0,0 +1,11 @@ +#ifndef ROBOT_DEF_H +#define ROBOT_DEF_H + +/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译 */ +#define ONE_BOARD //单板控制整车 +#define CHASSIS_BOARD //底盘板 +#define GIMBAL_BOARD //云台板 + + + +#endif // !ROBOT_DEF_H \ No newline at end of file diff --git a/application/shoot.c b/application/shoot.c deleted file mode 100644 index e69de29..0000000 diff --git a/application/shoot.h b/application/shoot.h deleted file mode 100644 index e69de29..0000000 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c new file mode 100644 index 0000000..3e83dfc --- /dev/null +++ b/application/shoot/shoot.c @@ -0,0 +1,11 @@ +#include "shoot.h" + +void ShootInit() +{ + +} + +void ShootTask() +{ + +} \ No newline at end of file diff --git a/application/shoot/shoot.h b/application/shoot/shoot.h new file mode 100644 index 0000000..e90084f --- /dev/null +++ b/application/shoot/shoot.h @@ -0,0 +1,19 @@ +#ifndef SHOOT_H +#define SHOOT_H + +#include "robot_def.h" +#include "dji_motor.h" + +typedef struct +{ + dji_motor_instance friction_l; + dji_motor_instance friction_r; + dji_motor_instance loader; + +} shoot; + +void ShootInit(); + +void ShootTask(); + +#endif //SHOOT_H \ No newline at end of file diff --git a/application/shoot.md b/application/shoot/shoot.md similarity index 100% rename from application/shoot.md rename to application/shoot/shoot.md