From 539da93f939a075a2316ecd1f1b0e1632a8d8784 Mon Sep 17 00:00:00 2001 From: Wu Feiyang Date: Wed, 26 Jul 2023 21:41:39 +0800 Subject: [PATCH] dart can store energy and return to the normal postion. --- CMakeLists.txt | 12 +-- .../unit_tests/ut_referee_if/hardware_conf.h | 2 +- .../ut_referee_if/ut_referee_if.cpp | 41 +++++---- .../vehicles/dart/can_motor_config.h | 2 +- dev/application/vehicles/dart/user_dart.cpp | 86 ++++++++++++++++--- dev/application/vehicles/dart/user_dart.h | 10 +++ 6 files changed, 116 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 411cae5a..f4f360d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -335,12 +335,12 @@ target_include_directories(ut_buzzer PRIVATE dev/application/unit_tests/ut_buzze # Referee # temporarily disabled by Tony Zhang # TODO re-enable this one, since referee is outdated -#add_executable(ut_referee_interface -# dev/common/CRC8.cpp -# dev/common/CRC16.cpp -# dev/interface/referee/referee_interface.cpp -# dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp) -#target_include_directories(ut_referee_interface PRIVATE dev/application/unit_tests/ut_referee_if) +add_executable(ut_referee_interface + dev/common/CRC8.cpp + dev/common/CRC16.cpp + dev/interface/referee/referee_interface.cpp + dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp) +target_include_directories(ut_referee_interface PRIVATE dev/application/unit_tests/ut_referee_if) add_executable(ut_sd_card dev/interface/sd_card/sd_card_interface.cpp diff --git a/dev/application/unit_tests/ut_referee_if/hardware_conf.h b/dev/application/unit_tests/ut_referee_if/hardware_conf.h index 98867627..7350330c 100644 --- a/dev/application/unit_tests/ut_referee_if/hardware_conf.h +++ b/dev/application/unit_tests/ut_referee_if/hardware_conf.h @@ -23,7 +23,7 @@ #endif #if !defined(ENABLE_USB_SHELL) || defined(__DOXYGEN__) -#define ENABLE_USB_SHELL FALSE +#define ENABLE_USB_SHELL TRUE #endif #endif //META_INFANTRY_HARDWARE_CONF_H diff --git a/dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp b/dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp index 487aa864..b14712fc 100644 --- a/dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp +++ b/dev/application/unit_tests/ut_referee_if/ut_referee_if.cpp @@ -21,21 +21,23 @@ class RefereeEchoThread : public BaseStaticThread <2048> { setName("referee_echo"); while(!shouldTerminate()) { -// LOG("chassis_volt = %u" , (unsigned int) Referee::power_heat_data.chassis_volt); -// LOG("chassis_current = %u" , (unsigned int) Referee::power_heat_data.chassis_current); -// LOG("chassis_power = %f" , Referee::power_heat_data.chassis_power); -// LOG("chassis_power_buffer = %u" , (unsigned int) Referee::power_heat_data.chassis_power_buffer); -// LOG("shooter_heat0 = %u" , (unsigned int) Referee::power_heat_data.shooter_id1_17mm_cooling_heat); -// LOG(""); -// -// LOG("bullet_type = %u" , (unsigned int) Referee::shoot_data.bullet_type); -// LOG("bullet_freq = %u" , (unsigned int) Referee::shoot_data.bullet_freq); -// LOG("bullet_speed = %u" , (unsigned int) Referee::shoot_data.bullet_speed); -// LOG(""); + LOG("chassis_volt = %u" , (unsigned int) Referee::power_heat.chassis_volt); + LOG("chassis_current = %u" , (unsigned int) Referee::power_heat.chassis_current); + LOG("chassis_power = %f" , Referee::power_heat.chassis_power); + LOG("chassis_power_buffer = %u" , (unsigned int) Referee::power_heat.chassis_power_buffer); + LOG("shooter_heat0 = %u" , (unsigned int) Referee::power_heat.shooter_id1_17mm_cooling_heat); + LOG(""); + // -// LOG("armor_id = %u" , (unsigned int) Referee::robot_hurt.armor_id); -// LOG("hurt_type = %u" , (unsigned int) Referee::robot_hurt.hurt_type); -// LOG(""); + LOG("armor_id = %u" , (unsigned int) Referee::robot_hurt.armor_id); + LOG("hurt_type = %u" , (unsigned int) Referee::robot_hurt.hurt_type); + LOG("launch opening status = %u", (unsigned int) Referee::dart_client.dart_launch_opening_status); + LOG("attack target = %u", (unsigned int) Referee::dart_client.dart_attack_target); + LOG("target change time = %u", (unsigned int) Referee::dart_client.target_change_time); + LOG("launch cmd time = %u", (unsigned int) Referee::dart_client.operate_launch_cmd_time); + LOG("remain_time = %u ",(unsigned int) Referee::ext_dart_remaining_time.dart_remaining_time); + + LOG(""); sleep(TIME_MS2I(200)); } } @@ -71,7 +73,8 @@ DEF_SHELL_CMD_START(cmd_add_rect) rect.radius = 200; - Referee::set_graphic(rect); +// Referee::set_graphic(rect); + Referee::add_tx_graphic(rect); enabled_send = true; chprintf(chp, "added" SHELL_NEWLINE_STR); return true; // command executed successfully @@ -107,7 +110,8 @@ DEF_SHELL_CMD_START(cmd_modify_rect) rect.radius = 200; - Referee::set_graphic(rect); +// Referee::set_graphic(rect); + Referee::add_tx_graphic(rect); chprintf(chp, "modified" SHELL_NEWLINE_STR); return true; // command executed successfully DEF_SHELL_CMD_END @@ -142,7 +146,8 @@ DEF_SHELL_CMD_START(cmd_del_rect) rect.radius = 200; - Referee::set_graphic(rect); +// Referee::set_graphic(rect); + Referee::add_tx_graphic(rect); enabled_send = false; chprintf(chp, "deleted" SHELL_NEWLINE_STR); return true; // command executed successfully @@ -165,7 +170,7 @@ int main() { // Start ChibiOS shell at high priority, so even if a thread stucks, we still have access to shell. Shell::start(HIGHPRIO); - Referee::init(NORMALPRIO); + Referee::init(); refereeEchoThread.start(NORMALPRIO+1); // See chconf.h for what this #define means. diff --git a/dev/application/vehicles/dart/can_motor_config.h b/dev/application/vehicles/dart/can_motor_config.h index 5d22f194..67ce19a8 100644 --- a/dev/application/vehicles/dart/can_motor_config.h +++ b/dev/application/vehicles/dart/can_motor_config.h @@ -1,5 +1,5 @@ // -// Created by 钱晨 on 11/14/21. +// Created by Wu Feiyang on 7/6/23. // #ifndef META_INFANTRY_CANBUS_MOTOR_CFG_H diff --git a/dev/application/vehicles/dart/user_dart.cpp b/dev/application/vehicles/dart/user_dart.cpp index a1fda123..7ce244a7 100644 --- a/dev/application/vehicles/dart/user_dart.cpp +++ b/dev/application/vehicles/dart/user_dart.cpp @@ -3,10 +3,29 @@ // #include "user_dart.h" UserDart::UserThread UserDart::userThread; +bool UserDart::UserThread::start_flag; +bool UserDart::timer_started; + void UserDart::start(tprio_t user_thd_prio) { userThread.start(user_thd_prio); } +void UserDart::return_puller() { + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,0); + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,0); + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false; + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false; + chThdSleepMilliseconds(10); + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true; + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true; + timer_started = false; +} + + void UserDart::UserThread::main(){ setName("RemoteControl"); static float angle; @@ -14,10 +33,19 @@ void UserDart::UserThread::main(){ angle = feedback.accumulate_angle(); Rudder rudder1(&PWMD4, nullptr,0,Rudder::MG995); rudder1.start(); + start_flag = true; + timer_started = false; + static virtual_timer_t release_vt; + chVTObjectInit(&release_vt); + int start_time; + int current_time; + + + while(!shouldTerminate()){ - chSysLock(); if(Remote::rc.s1 == Remote::S_UP){ // Forced Relax Mode + chSysLock(); for(uint i=0;i 0.2){ + start_flag = true; + } + if(start_flag){ + if(!timer_started){ + start_time = TIME_I2MS(chVTGetSystemTimeX()); + timer_started = true; + } + current_time = TIME_I2MS(chVTGetSystemTimeX()); + if(current_time - start_time < 12000){ + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,3900); + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,-3900); + + chThdSleepSeconds(1); + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = false; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false; + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false; + chThdSleepMilliseconds(5); + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true; + CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true; + CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true; + }else if(current_time - start_time >= 12000){ + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_LEFT,0); + CANMotorController::set_target_angle(CANMotorCFG::STORE_ENERGY_RIGHT,0); + timer_started = false; + } + } + + } + + }else if(Remote::rc.s1 == Remote::S_MIDDLE && Remote::rc.s2 == Remote::S_MIDDLE){ chSysLock(); - }else if(Remote::rc.s1 == Remote::S_DOWN){ // PC control mode, goes back to initial encoder angle CANMotorCFG::enable_v2i[CANMotorCFG::YAW] = true; CANMotorCFG::enable_a2v[CANMotorCFG::YAW] = true; CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = false; @@ -49,16 +119,10 @@ void UserDart::UserThread::main(){ CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = false; CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = false; CANMotorController::set_target_angle(CANMotorCFG::YAW,angle); - uint8_t remain_time = Referee::ext_dart_remaining_time.dart_remaining_time; - if(remain_time == 0){ - CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_LEFT] = true; - CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_LEFT] = true; - CANMotorCFG::enable_v2i[CANMotorCFG::STORE_ENERGY_RIGHT] = true; - CANMotorCFG::enable_a2v[CANMotorCFG::STORE_ENERGY_RIGHT] = true; - } + chSysUnlock(); } - chSysUnlock(); + // feedback = CANMotorIF::motor_feedback[CANMotorCFG::YAW]; // LOG("initial angle: %f, current angle: %f\r\n",angle,feedback.accumulate_angle()); chThdSleepMilliseconds(100); diff --git a/dev/application/vehicles/dart/user_dart.h b/dev/application/vehicles/dart/user_dart.h index e1b3c7ef..a16affa8 100644 --- a/dev/application/vehicles/dart/user_dart.h +++ b/dev/application/vehicles/dart/user_dart.h @@ -18,11 +18,21 @@ class UserDart{ static void start(tprio_t user_thd_prio); private: class UserThread:public BaseStaticThread<512>{ + public: + static bool start_flag; private: void main() final; CANMotorFeedback feedback; }; + static bool timer_started; + static UserThread userThread; + + static void return_puller(); + + static void release_dart(); + + };