From a9a7db1e19b560be87cbad1f651787437605b89d Mon Sep 17 00:00:00 2001 From: TonyZhangZTN Date: Wed, 13 Mar 2024 09:42:20 +0800 Subject: [PATCH] add param adjust for hero, set default baud 230400 --- CMakeLists.txt | 23 +- config/RM_Board_F3X.cfg | 7 + .../param_adjusts/pa_hero/README.md | 67 +++ .../pa_hero/can_motor_config.cpp | 48 +++ .../param_adjusts/pa_hero/can_motor_config.h | 35 ++ .../param_adjusts/pa_hero/hardware_conf.h | 33 ++ .../param_adjusts/pa_hero/pa_hero.cpp | 393 ++++++++++++++++++ .../param_adjusts/pa_hero/thread_priorities.h | 40 ++ dev/interface/shell/shell.cpp | 2 +- 9 files changed, 641 insertions(+), 7 deletions(-) create mode 100644 config/RM_Board_F3X.cfg create mode 100644 dev/application/param_adjusts/pa_hero/README.md create mode 100644 dev/application/param_adjusts/pa_hero/can_motor_config.cpp create mode 100644 dev/application/param_adjusts/pa_hero/can_motor_config.h create mode 100644 dev/application/param_adjusts/pa_hero/hardware_conf.h create mode 100644 dev/application/param_adjusts/pa_hero/pa_hero.cpp create mode 100644 dev/application/param_adjusts/pa_hero/thread_priorities.h diff --git a/CMakeLists.txt b/CMakeLists.txt index fae94c24..6adc3397 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -417,16 +417,27 @@ target_compile_definitions(ut_damiao_motor PRIVATE ut_damiao_motor) # Gimbal: include CANInterface, GimbalInterface, GimbalController, GimbalFeedbackThread and adjustment modules. -set(pa_gimbal_dir dev/application/param_adjusts/pa_infantry/) -add_executable(pa_gimbal +set(pa_infantry_dir dev/application/param_adjusts/pa_infantry) +add_executable(pa_infantry ${COMMON_SRC} ${AHRS_SRC} ${CAN_SRC} dev/interface/remote/remote_interpreter.cpp - ${pa_gimbal_dir}can_motor_config.cpp - ${pa_gimbal_dir}pa_infantry.cpp) -target_link_libraries(pa_gimbal ${AHRS_LIB}) -target_include_directories(pa_gimbal PRIVATE ${pa_gimbal_dir}) + ${pa_infantry_dir}/can_motor_config.cpp + ${pa_infantry_dir}/pa_infantry.cpp) +target_link_libraries(pa_infantry ${AHRS_LIB}) +target_include_directories(pa_infantry PRIVATE ${pa_infantry_dir}) + +set(pa_hero_dir dev/application/param_adjusts/pa_hero) +add_executable(pa_hero + ${COMMON_SRC} + ${AHRS_SRC} + ${CAN_SRC} + dev/interface/remote/remote_interpreter.cpp + ${pa_hero_dir}/can_motor_config.cpp + ${pa_hero_dir}/pa_hero.cpp) +target_link_libraries(pa_hero ${AHRS_LIB}) +target_include_directories(pa_hero PRIVATE ${pa_hero_dir}) add_executable(ca_ahrs_infantry dev/interface/ahrs/ahrs_math.hpp diff --git a/config/RM_Board_F3X.cfg b/config/RM_Board_F3X.cfg new file mode 100644 index 00000000..d852c5e6 --- /dev/null +++ b/config/RM_Board_F3X.cfg @@ -0,0 +1,7 @@ +source [find interface/stlink.cfg] + + +transport select hla_swd + + +source [find target/stm32f3x.cfg] diff --git a/dev/application/param_adjusts/pa_hero/README.md b/dev/application/param_adjusts/pa_hero/README.md new file mode 100644 index 00000000..730b6913 --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/README.md @@ -0,0 +1,67 @@ +# PA INFANTRY步兵调参固件 +此target是为步兵编写的调参固件。这个固件取消了电机之间的界限，将每个电机看作独立的轴，每个电机的PID参数都可单独调整。你甚至可以为步兵底盘电机调整角度PID，对底盘M3508电机进行角度闭环控制(虽然不知道有什么用，但这听上去很酷)。 + +## 程序结构 +本程序基本沿用了infantry application内的文件， 包含 `hardware_conf.h`,`can_motor_config`,`thread_priorities.h`，以及`pa_infantry.cpp`。 + +### pa_infantry.cpp +`pa_infantry.cpp`是调参的主程序文件，包含了调参的各类shell commands以及remote controller测试程序。 + +在遥控器左上拨钮拨到上方时，所有电机关闭并失去动力。 + +在遥控器左上拨钮拨到中间时，整机进入调试状态，用户可以通过发送shell command调试各个电机的角度或者速度PID。 + +在遥控器左上拨钮拨到下方时，整机进入测试状态，用户可以通过遥控器验证PID参数效果。 + +pa_infantry包含的shell command API会在文尾列出。 + +### hardware_conf.h +`hardware_conf.h`通过配置文件内TRUE/FALSE开启、关闭特定功能。 +目前，用户可以改变 `#define ENABLE_USB_SHELL` 宏定义切换Shell显示方式。 + +若`ENABLE_USB_SHELL`为`TRUE`，你可以通过USB虚拟串口接入Shell。但是，由于USB虚拟串口包含接收验证，在USB断开时，调用Shell发送数据的线程会被Shell printf命令卡住导致机器停止接收命令。 + +若`ENABLE_USB_SHELL`为`FALSE`, 你可以通过UART6口，连接USB转TTL模块接入shell。此种方式不会导致某些调用Shell printf命令的线程卡死。 + +不过，由于大部分线程已经对Shell printf进行了分离，不管是使用USB虚拟串口还是UART6口，断开都只会影响命令接收而不会影响主功能（电机控制PID，遥控器）卡死。 + +### thread_priorities.h +`thread_priorities.h`包含了线程的优先级，由于作者较懒和考虑到未来开发可能在调参程序中加入的功能，这个文件是直接从infantry拷贝过来的，有较多冗余的priorities。 + +### can_motor_config +`can_motor_config` 类决定了电机的数量、以及对应电机的型号和`SID`。此外，`can_motor_config`中还包含了一组电机的初始PID参数。在每次重启后，即使你不设置pid电机仍然是可以动起来的，这不是灵异现象。 +关于详细的`can_motor_config`介绍，请阅读`dev/interface/can_motor`下的`README`文档。 + +## pa_infantry Shell 命令一览 +| 命令头 | 数据 | 详细描述 | +|:------------------|:-----:|:-----------------------:| +| set_enable_a | motor_id | 启动角度PID闭环控制（串级PID） | +| set_disable_a | motor_id | 关闭角度PID闭环控制（并将力矩输出调整为0） | +| set_enable_v | motor_id | 启动速度PID闭环控制 | +| set_disable_v | motor_id | 关闭速度PID闭环控制 | +| get_sid | motor_id | 得到某个电机的CAN SID | +| fb_enable | motor_id | 将某个电机的反馈数据显示在Shell上 | +| fb_disable | motor_id | 停止把某个电机的反馈数据显示在Shell上 | +| set_pid | motor_id pid_id(0: angle_to_v, 1: v_to_i) ki kp kd i_limit out_limit | 设置PID参数 | +| echo_pid | motor_id pid_id(0: angle_to_v, 1: v_to_i) | 读取正在使用的PID参数 | +| set_target_angle | motor_id target_angle | 设置电机目标角度 | +| set_target_vel | motor_id target_vel | 设置电机目标速度 | +| echo_actual_angle | motor_id | 反馈电机累计角度 | +| echo_raw_angle | motor_id | 反馈电机原始转子角度（-8192~8192） | + +关于Shell反馈数据格式，在CANMotorController的DOXYGEN documentation中有解释: +``` + !fb, %u, %u, %.2f, %.2f, %.2f, %.2f, %d, %d\r\n + | | | | | | | | + | | actual target actual target actual target + time id angle angle velocity velocity current current +``` +其中，`!fb`是行头，标志这行是反馈数据。 + +`time`为反馈数据的时间戳，从STM32开机开始计时，单位为毫秒。 + +`id`为电机ID，和`can_motor_config`中的`motor_id_t` enumerator相对应，**并非SID！** + +其余的就不一一阐述了。 + +希望有兴趣的同学可以通过此写出自己的GUI调参软件 :D \ No newline at end of file diff --git a/dev/application/param_adjusts/pa_hero/can_motor_config.cpp b/dev/application/param_adjusts/pa_hero/can_motor_config.cpp new file mode 100644 index 00000000..06fca718 --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/can_motor_config.cpp @@ -0,0 +1,48 @@ +// +// Created by 钱晨 on 11/14/21. +// + +#include "can_motor_config.h" + +CANMotorBase CANMotorCFG::CANMotorProfile[MOTOR_COUNT] = { + {CANMotorBase::can_channel_2, 0x202, CANMotorBase::M3508, 3572},//Front Left + {CANMotorBase::can_channel_2, 0x201, CANMotorBase::M3508, 3572},//Front Right + {CANMotorBase::can_channel_2, 0x204, CANMotorBase::M3508, 3572},//Back Right + {CANMotorBase::can_channel_2, 0x203, CANMotorBase::M3508, 3572},//Back Left + {CANMotorBase::can_channel_2, 0x208, CANMotorBase::GM6020, 1994},//Yaw + {CANMotorBase::can_channel_1, 0x205, CANMotorBase::GM6020, 3301},//PITCH 侧面的6020 + {CANMotorBase::can_channel_1, 0x201, CANMotorBase::M3508_without_deceleration, 3344}, + {CANMotorBase::can_channel_2, 0x206, CANMotorBase::M3508, 3572}, //Bullet Loader + {CANMotorBase::can_channel_1, 0x203, CANMotorBase::M3508_without_deceleration, 3572},//FW_UP(左边) + {CANMotorBase::can_channel_1, 0x204, CANMotorBase::M3508_without_deceleration, 3572}//FW_DOWN +}; + +PIDController::pid_params_t CANMotorCFG::a2vParams[MOTOR_COUNT] = { + {10, 0.0f, 0.2, 100, 500}, + {10, 0.0f, 0.2, 100, 500}, + {10, 0.0f, 0.2, 100, 500}, + {10, 0.0f, 0.2, 100, 500}, + {10, 0.0f, 0.1, 70, 90}, + {8.5, 0.0f, 0.1, 70, 90}, + {10, 0.0f, 0.2, 100, 500}, + {50, 0.0f, 0.18, 100, 250},//Bullet Loader temprarily to 2.5x to surpass the friction + {10, 0.0f, 0.2, 100, 500}, + {10, 0.0f, 0.2, 100, 500}, +}; + +PIDController::pid_params_t CANMotorCFG::v2iParams[MOTOR_COUNT] = { + {26.0f,0.1f,0.02f,2000.0,6000.0}, + {26.0f,0.1f,0.02f,2000.0,6000.0}, + {26.0f,0.1f,0.02f,2000.0,6000.0}, + {26.0f,0.1f,0.02f,2000.0,6000.0}, + {50.0f, 0.0f, 0.0f, 5000.0f, 15000.0f}, + { 50.0f, 0.0f, 0.00f, 5000.0f, 15000.0f}, + {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, + {35.0f, 2.1f, 0.0f, 3000.0f, 16383.0f},//Bullet Loader + {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f}, + {26.0f, 0.1f, 0.02f, 2000.0f, 6000.0f} +}; + +/// Will assigned by each scheduler. +bool CANMotorCFG::enable_a2v[MOTOR_COUNT] {false}; +bool CANMotorCFG::enable_v2i[MOTOR_COUNT] {false}; \ No newline at end of file diff --git a/dev/application/param_adjusts/pa_hero/can_motor_config.h b/dev/application/param_adjusts/pa_hero/can_motor_config.h new file mode 100644 index 00000000..37ecdf59 --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/can_motor_config.h @@ -0,0 +1,35 @@ +// +// Created by 钱晨 on 11/14/21. +// + +#ifndef META_INFANTRY_CANBUS_MOTOR_CFG_H +#define META_INFANTRY_CANBUS_MOTOR_CFG_H + +#include "can_motor_feedback.h" +#include "pid_controller.hpp" + +class CANMotorCFG { +public: + enum motor_id_t { + FL, + FR, + BR, + BL, + YAW, + PITCH, + SUB_PITCH, + BULLET_LOADER, + FW_UP, + FW_DOWN, + MOTOR_COUNT + }; + static CANMotorBase CANMotorProfile[MOTOR_COUNT]; + + // Parameters for double loop PID control. + static PIDController::pid_params_t a2vParams [MOTOR_COUNT]; + static PIDController::pid_params_t v2iParams [MOTOR_COUNT]; + static bool enable_a2v[MOTOR_COUNT]; + static bool enable_v2i[MOTOR_COUNT]; +}; + +#endif //META_INFANTRY_CANBUS_MOTOR_CFG_H diff --git a/dev/application/param_adjusts/pa_hero/hardware_conf.h b/dev/application/param_adjusts/pa_hero/hardware_conf.h new file mode 100644 index 00000000..d1986f0f --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/hardware_conf.h @@ -0,0 +1,33 @@ +// +// Created by 钱晨 on 3/16/22. +// This file contains some feature enabling masks. +// + +#ifndef META_INFANTRY_HARDWARE_CONF_H +#define META_INFANTRY_HARDWARE_CONF_H + +#if !defined(ENABLE_VISION) || defined(__DOXYGEN__) +#define ENABLE_VISION FALSE +#endif + +#if !defined(ENABLE_REFEREE) || defined(__DOXYGEN__) +#define ENABLE_REFEREE FALSE +#endif + +#if !defined(ENABLE_AHRS) || defined(__DOXYGEN__) +#define ENABLE_AHRS TRUE +#endif + +#if !defined(ENABLE_SUBPITCH) || defined(__DOXYGEN__) +#define ENABLE_SUBPITCH FALSE +#endif + +#if !defined(ENABLE_CAPACITOR) || defined(__DOXYGEN__) +#define ENABLE_CAPACITOR TRUE +#endif + +#if !defined(ENABLE_USB_SHELL) || defined(__DOXYGEN__) +#define ENABLE_USB_SHELL FALSE +#endif + +#endif //META_INFANTRY_HARDWARE_CONF_H diff --git a/dev/application/param_adjusts/pa_hero/pa_hero.cpp b/dev/application/param_adjusts/pa_hero/pa_hero.cpp new file mode 100644 index 00000000..0a58698d --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/pa_hero.cpp @@ -0,0 +1,393 @@ +// +// Created by kerui on 2021/7/14. +// + +#include "ch.hpp" +#include "hal.h" + +#include "led.h" +#include "shell.h" + +#include "thread_priorities.h" + +#include "can_interface.h" +#include "can_motor_controller.h" + +#include "ahrs.h" +#include "remote_interpreter.h" + +#include "buzzer_scheduler.h" + +using namespace chibios_rt; + +const char *motor_name[10] = { + "Front Left", + "Front Right", + "Back Right", + "Back Left", + "Yaw", + "PITCH", + "SUB_PITCH", + "BULLET_LOADER", + "FW_UP", + "FW_DOWN"}; + +/// Define constants +constexpr unsigned USER_THREAD_INTERVAL = 7; // [ms] + +float gimbal_yaw_target_angle_ = 0; +float gimbal_rc_yaw_max_speed = 180; // [degree/s] + +float gimbal_pitch_min_angle = -30; // down range for pitch [degree] +float gimbal_pitch_max_angle = 10; // up range for pitch [degree] + +bool is_shooting = false; +float loader_velocity = 20.0f; +float friction_velocity = 100.0f; + +/// AHRS PARAMETERS +/// Depends on the installation direction of the board +#define ON_BOARD_AHRS_MATRIX {{0.0f, -1.0f, 0.0f}, \ + {1.0f, 0.0f, 0.0f}, \ + {0.0f, 0.0f, 1.0f}} + +#define GIMBAL_ANGLE_INSTALLATION_MATRIX {{1.0f, 0.0f, 0.0f}, \ + {0.0f, 1.0f, 0.0f}, \ + {0.0f, 0.0f, -1.0f}} + + +#define GIMBAL_GYRO_INSTALLATION_MATRIX {{0.0f, -1.0f, 0.0f}, \ + {0.0f, 0.0f, 1.0f}, \ + {1.0f, 0.0f, 0.0f}} + +static const Matrix33 ON_BOARD_AHRS_MATRIX_ = ON_BOARD_AHRS_MATRIX; +static const Matrix33 GIMBAL_ANGLE_INSTALLATION_MATRIX_ = GIMBAL_ANGLE_INSTALLATION_MATRIX; +static const Matrix33 GIMBAL_GYRO_INSTALLATION_MATRIX_ = GIMBAL_GYRO_INSTALLATION_MATRIX; +#define MPU6500_STORED_GYRO_BIAS {0.491007685, -1.024637818, -0.371067702} + +CANInterface can1(&CAND1); +CANInterface can2(&CAND2); +AHRSOnBoard ahrs; + +DEF_SHELL_CMD_START(cmd_motor_angle_enable) + (void) argv; + if(argc != 1){ + return false; + } + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + CANMotorCFG::enable_a2v[motor_id] = true; + CANMotorCFG::enable_v2i[motor_id] = true; + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_motor_angle_disable) + (void) argv; + if(argc != 1){ + return false; + } + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + CANMotorCFG::enable_a2v[motor_id] = false; + CANMotorCFG::enable_v2i[motor_id] = false; + CANMotorController::set_target_current((CANMotorCFG::motor_id_t)motor_id, 0); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_motor_velocity_enable) + (void) argv; + if(argc != 1){ + return false; + } + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return true; + } + CANMotorCFG::enable_a2v[motor_id] = false; + CANMotorCFG::enable_v2i[motor_id] = true; + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_motor_velocity_disable) + (void) argv; + if(argc != 1){ + return false; + } + unsigned motor_id = Shell::atoi(argv[0]); + CANMotorCFG::enable_a2v[motor_id] = false; + CANMotorCFG::enable_v2i[motor_id] = false; + CANMotorController::set_target_current((CANMotorCFG::motor_id_t)motor_id, 0); + return true; // command executed successfully +DEF_SHELL_CMD_END + + +DEF_SHELL_CMD_START(cmd_get_sid) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + Shell::printf("motor type: %d, motor sid: %x" SHELL_NEWLINE_STR, + CANMotorCFG::CANMotorProfile[motor_id].motor_type, + CANMotorCFG::CANMotorProfile[motor_id].CAN_SID); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_enable_feedback) + if(argc != 1){ + return false; + } + unsigned feedback_motor_id = Shell::atoi(argv[0]); + if (feedback_motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid feedback ID %d" SHELL_NEWLINE_STR, feedback_motor_id); + return false; + } + Shell::printf("%s feedback enabled" SHELL_NEWLINE_STR, motor_name[feedback_motor_id]); + CANMotorController::shell_display((CANMotorCFG::motor_id_t) feedback_motor_id, true); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_disable_feedback) + (void) argv; + if (argc != 1) { + return false; + } + unsigned feedback_motor_id = Shell::atoi(argv[0]); + if (feedback_motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid feedback ID %d" SHELL_NEWLINE_STR, feedback_motor_id); + return false; + } + Shell::printf("%s feedback disabled" SHELL_NEWLINE_STR, motor_name[feedback_motor_id]); + CANMotorController::shell_display((CANMotorCFG::motor_id_t) feedback_motor_id, false); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_set_param) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + unsigned pid_id = Shell::atoi(argv[1]); + if (pid_id > 1) { + Shell::printf("Invalid pid ID %d" SHELL_NEWLINE_STR, pid_id); + return false; + } + PIDController::pid_params_t pid_param = {Shell::atof(argv[2]), + Shell::atof(argv[3]), + Shell::atof(argv[4]), + Shell::atof(argv[5]), + Shell::atof(argv[6])}; + + CANMotorController::load_PID_params((CANMotorCFG::motor_id_t) motor_id, pid_id == 0, pid_param); + + Shell::printf("ps!" SHELL_NEWLINE_STR); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_echo_param) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + + unsigned pid_id = Shell::atoi(argv[1]); + if (pid_id > 1) { + Shell::printf("Invalid pid ID %d" SHELL_NEWLINE_STR, pid_id); + return false; + } + + PIDController::pid_params_t pid_param = {0,0,0,0,0}; + pid_param = CANMotorController::getPIDParams((CANMotorCFG::motor_id_t)motor_id, (pid_id == 0)); + Shell::printf("kp: %.2f, ki: %.2f, kd: %.2f, i_limit: %.2f, out_limit: %.2f" SHELL_NEWLINE_STR, + pid_param.kp, pid_param.ki, pid_param.kd, pid_param.i_limit, pid_param.out_limit); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_set_target_angle) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + float angle = Shell::atof(argv[1]); + CANMotorController::set_target_angle((CANMotorCFG::motor_id_t)motor_id, angle); + Shell::printf("ps!" SHELL_NEWLINE_STR); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_set_target_velocity) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + float velocity = Shell::atof(argv[1]); + CANMotorController::set_target_vel((CANMotorCFG::motor_id_t)motor_id, velocity); + Shell::printf("ps!" SHELL_NEWLINE_STR); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_echo_actual_angle) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + float angle = 0; + angle = CANMotorIF::motor_feedback[motor_id].accumulate_angle(); + Shell::printf("%.2f" SHELL_NEWLINE_STR, angle); + return true; // command executed successfully +DEF_SHELL_CMD_END + +DEF_SHELL_CMD_START(cmd_echo_raw_angle) + (void) argv; + unsigned motor_id = Shell::atoi(argv[0]); + if (motor_id >= CANMotorCFG::MOTOR_COUNT) { + Shell::printf("Invalid motor ID %d" SHELL_NEWLINE_STR, motor_id); + return false; + } + Shell::printf("%d" SHELL_NEWLINE_STR, CANMotorIF::motor_feedback[motor_id].rotor_angle_raw); + return true; // command executed successfully +DEF_SHELL_CMD_END + + +class TopControlThread : public BaseStaticThread<512> { +private: + void main() final { + setName("TopControlThread"); + Remote::rc_status_t previous_rcs1_state = Remote::rc.s1; + CANMotorCFG::motor_id_t vel_group[] = {CANMotorCFG::YAW, CANMotorCFG::PITCH}; + CANMotorCFG::motor_id_t angle_group[] = {CANMotorCFG::FL, CANMotorCFG::FR, + CANMotorCFG::BL, CANMotorCFG::BR, + CANMotorCFG::FW_UP, CANMotorCFG::FW_DOWN, + CANMotorCFG::BULLET_LOADER}; + while (!shouldTerminate()) { + + if (Remote::rc.s1 == Remote::S_DOWN) { + + + } + + if(Remote::rc.s1 == Remote::S_UP){ + /// Safe Mode + for (int i = 0; i < CANMotorCFG::MOTOR_COUNT; i++) { + CANMotorCFG::enable_a2v[i] = false; + CANMotorCFG::enable_v2i[i] = false; + CANMotorController::set_target_current((CANMotorCFG::motor_id_t)i, 0); + } + } + + previous_rcs1_state = Remote::rc.s1; + + /// Final, since user thread is not time sensitive, we use fixed time sleep interval. (No need to adjust sleep time for constant interval) + sleep(TIME_MS2I(USER_THREAD_INTERVAL)); + } + } +} topControlThread; + +// Command lists for gimbal controller test and adjustments +Shell::Command mainProgramCommands[] = { + {"set_enable_a", "motor_id", cmd_motor_angle_enable, nullptr}, + {"set_disable_a", "motor_id", cmd_motor_angle_disable, nullptr}, + {"set_enable_v", "motor_id", cmd_motor_velocity_enable, nullptr}, + {"set_disable_v", "motor_id", cmd_motor_velocity_disable,nullptr}, + {"get_sid", "motor_id", cmd_get_sid, nullptr}, + {"fb_enable", "motor_id", cmd_enable_feedback, nullptr}, + {"fb_disable", "motor_id", cmd_disable_feedback, nullptr}, + {"set_pid", "motor_id pid_id(0: angle_to_v, 1: v_to_i) kp ki kd i_limit out_limit", cmd_set_param, nullptr}, + {"echo_pid", "motor_id pid_id(0: angle_to_v, 1: v_to_i)", cmd_echo_param, nullptr}, + {"set_target_angle", "motor_id target_angle", cmd_set_target_angle, nullptr}, + {"set_target_vel", "motor_id target_vel", cmd_set_target_velocity, nullptr}, + {"echo_actual_angle", "motor_id", cmd_echo_actual_angle, nullptr}, + {"echo_raw_angle", "motor_id", cmd_echo_raw_angle, nullptr}, + {nullptr, nullptr, nullptr, nullptr} +}; + + + + +int main() { + /*** --------------------------- Period 0. Fundamental Setup --------------------------- ***/ + + halInit(); + chibios_rt::System::init(); + + // Enable power of bullet loader motor + palSetPadMode(GPIOH, GPIOH_POWER1_CTRL, PAL_MODE_OUTPUT_PUSHPULL); + palSetPad(GPIOH, GPIOH_POWER1_CTRL); + + /*** ---------------------- Period 1. Modules Setup and Self-Check ---------------------- ***/ + + /// Preparation of Period 1 + LED::all_off(); + + /// Setup Shell + Shell::start(THREAD_SHELL_PRIO); + Shell::addCommands(mainProgramCommands); + chThdSleepMilliseconds(50); // wait for logo to print :) + + BuzzerSKD::init(THREAD_BUZZER_SKD_PRIO); + + /// Setup CAN1 & CAN2 + can1.start(THREAD_CAN1_RX_PRIO); + can2.start(THREAD_CAN2_RX_PRIO); + chThdSleepMilliseconds(5); + + /// Setup On-Board AHRS + ahrs.load_calibration_data(MPU6500_STORED_GYRO_BIAS); + ahrs.start(ON_BOARD_AHRS_MATRIX_, THREAD_AHRS_PRIO); + chThdSleepMilliseconds(5); + + /// Setup Remote + Remote::start(); + + + CANMotorController::start(THREAD_MOTOR_SKD_PRIO, THREAD_FEEDBACK_SKD_PRIO, &can1, &can2); + chThdSleepMilliseconds(2000); // wait for C610 to be online and friction wheel to reset + + /// Setup Red Spot Laser + palSetPad(GPIOG, GPIOG_RED_SPOT_LASER); // enable the red spot laser + + + /*** ------------ Period 2. Calibration and Start Logic Control Thread ----------- ***/ + + /// Echo Gimbal Raws and Converted Angles + LOG("Gimbal Yaw: %u, %f, Pitch: %u, %f" SHELL_NEWLINE_STR, + CANMotorIF::motor_feedback[CANMotorCFG::YAW].rotor_angle_raw, CANMotorIF::motor_feedback[CANMotorCFG::YAW].accumulate_angle(), + CANMotorIF::motor_feedback[CANMotorCFG::PITCH].rotor_angle_raw, CANMotorIF::motor_feedback[CANMotorCFG::PITCH].accumulate_angle()); + + /// Start SKDs + topControlThread.start(THREAD_USER_PRIO); + + /// Complete Period 2 + BuzzerSKD::play_sound(BuzzerSKD::sound_startup_intel); // Now play the startup sound + + + /*** ------------------------ Period 3. End of main thread ----------------------- ***/ + + // Entering empty loop with low priority +#if CH_CFG_NO_IDLE_THREAD // See chconf.h for what this #define means. + // ChibiOS idle thread has been disabled, main() should implement infinite loop + while (true) {} +#else + // When vehicle() quits, the vehicle thread will somehow enter an infinite loop, so we set the + // priority to lowest before quitting, to let other threads run normally + chibios_rt::BaseThread::setPriority(IDLEPRIO); +#endif + return 0; +} \ No newline at end of file diff --git a/dev/application/param_adjusts/pa_hero/thread_priorities.h b/dev/application/param_adjusts/pa_hero/thread_priorities.h new file mode 100644 index 00000000..56ac1e37 --- /dev/null +++ b/dev/application/param_adjusts/pa_hero/thread_priorities.h @@ -0,0 +1,40 @@ +// +// Created by liuzikai on 7/29/21. +// + +#ifndef META_INFANTRY_THREAD_PRIORITIES_H +#define META_INFANTRY_THREAD_PRIORITIES_H + +/// Thread Priority List +#define THREAD_CAN1_RX_PRIO (HIGHPRIO - 1) +#define THREAD_CAN2_RX_PRIO (HIGHPRIO - 2) + +#define THREAD_GIMBAL_SKD_PRIO (HIGHPRIO - 3) +#define THREAD_CHASSIS_SKD_PRIO (HIGHPRIO - 4) +#define THREAD_SHOOT_SKD_PRIO (HIGHPRIO - 5) +#define THREAD_AHRS_PRIO (HIGHPRIO - 6) +#define THREAD_MOTOR_SKD_PRIO (HIGHPRIO - 7) +#define THREAD_FEEDBACK_SKD_PRIO (HIGHPRIO - 8) +#define THREAD_BUTTON_DETECT_PRIO (HIGHPRIO - 9) + +#define THREAD_CHASSIS_LG_PRIO (HIGHPRIO - 10) +#define THREAD_GIMBAL_LG_VISION_PRIO (HIGHPRIO - 11) +#define THREAD_GIMBAL_BALLISTIC_PRIO (HIGHPRIO - 12) +#define THREAD_MOTOR_LG_VISION_PRIO (HIGHPRIO - 13) +#define THREAD_USER_PRIO (HIGHPRIO - 14) +#define THREAD_USER_ACTION_PRIO (HIGHPRIO - 15) + +#define THREAD_VIRTUAL_COM_PRIO (NORMALPRIO) +#define THREAD_SUPERCAP_INIT_PRIO (NORMALPRIO-1) +#define THREAD_REFEREE_SKD_PRIO (NORMALPRIO-2) +#define THREAD_VISION_SKD_PRIO (NORMALPRIO-3) + +#define THREAD_INSPECTOR_PRIO (LOWPRIO + 10) +#define THREAD_COMMUNICATOR_PRIO (LOWPRIO + 9) +#define THREAD_STUCK_DETECT_PRIO (LOWPRIO + 4) +#define THREAD_SHOOT_BULLET_COUNTER_PRIO (LOWPRIO + 3) +#define THREAD_SHOOT_LG_VISION_PRIO (LOWPRIO + 2) +#define THREAD_SHELL_PRIO (LOWPRIO + 1) +#define THREAD_BUZZER_SKD_PRIO (LOWPRIO) + +#endif //META_INFANTRY_THREAD_PRIORITIES_H diff --git a/dev/interface/shell/shell.cpp b/dev/interface/shell/shell.cpp index bd5bbe67..bb15edac 100644 --- a/dev/interface/shell/shell.cpp +++ b/dev/interface/shell/shell.cpp @@ -30,7 +30,7 @@ THD_WORKING_AREA(wa, SHELL_RX_WORK_AREA_SIZE); // the working area for the shel static ShellConfig shellConfig; // Serial config for Serial Driver #if ENABLE_USB_SHELL == FALSE -static constexpr SerialConfig SHELL_SERIAL_CONFIG = {921600, +static constexpr SerialConfig SHELL_SERIAL_CONFIG = {230400, 0, USART_CR2_STOP1_BITS, 0};