From bde53c389a328fc265050af012d52f09db26938d Mon Sep 17 00:00:00 2001 From: Ziyi Wang Date: Sat, 29 Jul 2023 18:11:47 +0800 Subject: [PATCH 01/15] [feat]: added servo for hero trigger Able to rotate from -90 (deg) to +70 (deg) --- boards/CMakeLists.txt | 2 +- examples/motor/CMakeLists.txt | 3 +++ examples/motor/HERO_SERVO.cc | 51 +++++++++++++++++++++++++++++++++++ examples/motor/pwm.cc | 4 +-- shared/libraries/motor.cc | 25 +++++++++++++++++ shared/libraries/motor.h | 22 ++++++++++++--- 6 files changed, 100 insertions(+), 7 deletions(-) create mode 100644 examples/motor/HERO_SERVO.cc diff --git a/boards/CMakeLists.txt b/boards/CMakeLists.txt index 102b2324..124c8a98 100644 --- a/boards/CMakeLists.txt +++ b/boards/CMakeLists.txt @@ -19,7 +19,7 @@ # ---------------------------------------------------------------------- # # generic board interface -add_library(board_interface INTERFACE) +add_library(board_interface INTERFACE ../examples/motor/HERO_SERVO.cc) target_compile_definitions(board_interface INTERFACE USE_HAL_DRIVER) target_compile_options(board_interface INTERFACE $<$:-x assembler-with-cpp> diff --git a/examples/motor/CMakeLists.txt b/examples/motor/CMakeLists.txt index 04a8425f..ffe86797 100644 --- a/examples/motor/CMakeLists.txt +++ b/examples/motor/CMakeLists.txt @@ -79,3 +79,6 @@ irm_add_arm_executable(${PROJECT_NAME}_m3510 irm_add_arm_executable(${PROJECT_NAME}_m3510_PID TARGET DJI_Board_TypeC SOURCES m3510_PID.cc) +irm_add_arm_executable(${PROJECT_NAME}_HERO_SERVO + TARGET DJI_Board_TypeC + SOURCES HERO_SERVO.cc) \ No newline at end of file diff --git a/examples/motor/HERO_SERVO.cc b/examples/motor/HERO_SERVO.cc new file mode 100644 index 00000000..85efcf6d --- /dev/null +++ b/examples/motor/HERO_SERVO.cc @@ -0,0 +1,51 @@ +#include "bsp_gpio.h" +#include "bsp_print.h" +#include "cmsis_os.h" +#include "main.h" +#include "motor.h" + + + +uint8_t PWM_CHANNEL = 1; +uint32_t TIM_CLOCK_FREQ = 2000000; +// Magic Number +uint32_t MOTOR_OUT_FREQ = 500; +// Magic number +uint32_t PULSE_WIDTH = 1500; +// PULSE_WIDTH when servo is idle + +bsp::GPIO* key = nullptr; + +#define KEY_GPIO_GROUP GPIOA +#define KEY_GPIO_PIN GPIO_PIN_0 + +control::PDIHV* motor1; + +void RM_RTOS_Init(){ + print_use_uart(&huart1); + key = new bsp::GPIO(KEY_GPIO_GROUP, KEY_GPIO_PIN); + motor1 = new control::PDIHV(&htim1, PWM_CHANNEL,TIM_CLOCK_FREQ,MOTOR_OUT_FREQ, PULSE_WIDTH); + motor1->SetOutput(1500); + osDelay(300); +} + +void RM_RTOS_Default_Task(const void* args) { + UNUSED(args); + float angle = 0.0; + int16_t power = 2000; + while(true){ +// motor1->SetOutPutAngle(angle); + motor1->SetOutput(power); + if(!key->Read()){ + angle += 10.0; + power += 10; + } +// power += 10; +// osDelay(1000); +// angle += 1.0; + print("angle: %f\r\n", angle); + print("power: %d\r\n", power); + + + } + } diff --git a/examples/motor/pwm.cc b/examples/motor/pwm.cc index 1b27952b..fe99c483 100644 --- a/examples/motor/pwm.cc +++ b/examples/motor/pwm.cc @@ -31,8 +31,8 @@ #define LEFT_MOTOR_PWM_CHANNEL 1 #define RIGHT_MOTOR_PWM_CHANNEL 4 #define TIM_CLOCK_FREQ 1000000 -#define MOTOR_OUT_FREQ 500 -#define SNAIL_IDLE_THROTTLE 1080 +#define MOTOR_OUT_FREQ 1500 +#define SNAIL_IDLE_THROTTLE 500 control::MotorPWMBase* motor1; control::MotorPWMBase* motor2; diff --git a/shared/libraries/motor.cc b/shared/libraries/motor.cc index 82880b32..5eead4f1 100644 --- a/shared/libraries/motor.cc +++ b/shared/libraries/motor.cc @@ -308,6 +308,31 @@ void MotorPWMBase::SetOutput(int16_t val) { pwm_.SetPulseWidth(val + idle_throttle_); } +//================================================================================================== +// PDI_HV Servo +//================================================================================================== +PDIHV::PDIHV(TIM_HandleTypeDef* htim, uint8_t channel, uint32_t clock_freq, + uint32_t output_freq, uint32_t idle_throttle) + : MotorPWMBase(htim, channel, clock_freq, output_freq, idle_throttle) { + +} + +void PDIHV::SetOutPutAngle(float degree) { + float slope = (1913.0-900.0) / 160.0; + int16_t val = int16_t (clip(degree, -90, 70) * slope + 1500); + constexpr int16_t MIN_OUTPUT = 900; + constexpr int16_t MAX_OUTPUT = 1913; +// 900 to 1913 for pulse width input u second +// -90 to 90 for angle input + + this->SetOutput(clip(val, MIN_OUTPUT, MAX_OUTPUT)); + this->SetOutput(val); +} +void PDIHV::SetOutput(int16_t val) { + constexpr int16_t MIN_OUTPUT = 900; + constexpr int16_t MAX_OUTPUT = 1913; + MotorPWMBase::SetOutput(clip(val, MIN_OUTPUT, MAX_OUTPUT)); +} //================================================================================================== // Motor2305 //================================================================================================== diff --git a/shared/libraries/motor.h b/shared/libraries/motor.h index edcdbc7b..9e9055fd 100644 --- a/shared/libraries/motor.h +++ b/shared/libraries/motor.h @@ -298,10 +298,6 @@ class Motor6623 : public MotorCANBase { static const int16_t CURRENT_CORRECTION = -1; // current direction is reversed }; - - - - //================================================================================================== // MotorPWMBase //================================================================================================== @@ -337,6 +333,22 @@ class MotorPWMBase : public MotorBase { uint32_t idle_throttle_; }; +//================================================================================================== +// PDIHV +//================================================================================================== + +/** + * @brief Hero Trigger servo + */ +class PDIHV : public MotorPWMBase { + public: + PDIHV(TIM_HandleTypeDef* htim, uint8_t channel, uint32_t clock_freq, uint32_t output_freq, + uint32_t idle_throttle); + /* override base implementation with max angle */ + void SetOutPutAngle(float deg); + void SetOutput(int16_t val) override final; +}; + //================================================================================================== // Motor2305 //================================================================================================== @@ -350,6 +362,8 @@ class Motor2305 : public MotorPWMBase { void SetOutput(int16_t val) override final; }; + + //================================================================================================== // ServoMotor //================================================================================================== From ba6ca084e4aebfa2b87b79f64fe4d63a1600f8b5 Mon Sep 17 00:00:00 2001 From: Ziyi Wang Date: Sun, 30 Jul 2023 02:00:55 +0800 Subject: [PATCH 02/15] [fix]: adjusted accuracy for 0 deg --- examples/motor/HERO_SERVO.cc | 29 ++++++++++++++++++----------- shared/libraries/motor.cc | 16 ++++++++-------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/examples/motor/HERO_SERVO.cc b/examples/motor/HERO_SERVO.cc index 85efcf6d..66a567a6 100644 --- a/examples/motor/HERO_SERVO.cc +++ b/examples/motor/HERO_SERVO.cc @@ -4,13 +4,16 @@ #include "main.h" #include "motor.h" +/** + * @brief when installing, please use this example to set angle to 0.0 degree for calibration. + * The current program is functioning, however, might not be 100% accurate + */ - +// All of these following parameters are tuned for this servo. uint8_t PWM_CHANNEL = 1; -uint32_t TIM_CLOCK_FREQ = 2000000; -// Magic Number -uint32_t MOTOR_OUT_FREQ = 500; -// Magic number +uint32_t TIM_CLOCK_FREQ = 1980000; /* TODO: could use more calibration if data is available*/ +// rule of thumb, TIM_CLOCK_FREQ could use more calibration if more data is available for PDI-HV5932 +uint32_t MOTOR_OUT_FREQ = 500; /* TODO: could use more calibration if data is available*/ uint32_t PULSE_WIDTH = 1500; // PULSE_WIDTH when servo is idle @@ -25,26 +28,30 @@ void RM_RTOS_Init(){ print_use_uart(&huart1); key = new bsp::GPIO(KEY_GPIO_GROUP, KEY_GPIO_PIN); motor1 = new control::PDIHV(&htim1, PWM_CHANNEL,TIM_CLOCK_FREQ,MOTOR_OUT_FREQ, PULSE_WIDTH); - motor1->SetOutput(1500); +// motor1->SetOutput(1500); osDelay(300); } void RM_RTOS_Default_Task(const void* args) { UNUSED(args); float angle = 0.0; - int16_t power = 2000; + // angle could be set to 80 ~ -80 deg + int16_t power = 1947; + // power is from range 972 to 1947, data on purchasing page is not available, pulse width for central point is 1500 while(true){ -// motor1->SetOutPutAngle(angle); - motor1->SetOutput(power); + motor1->SetOutPutAngle(angle); + +// motor1->SetOutput(power); if(!key->Read()){ angle += 10.0; - power += 10; + power -= 10; } // power += 10; -// osDelay(1000); + osDelay(2); // angle += 1.0; print("angle: %f\r\n", angle); print("power: %d\r\n", power); + print("\r\n"); } diff --git a/shared/libraries/motor.cc b/shared/libraries/motor.cc index 5eead4f1..cef0d165 100644 --- a/shared/libraries/motor.cc +++ b/shared/libraries/motor.cc @@ -318,19 +318,19 @@ PDIHV::PDIHV(TIM_HandleTypeDef* htim, uint8_t channel, uint32_t clock_freq, } void PDIHV::SetOutPutAngle(float degree) { - float slope = (1913.0-900.0) / 160.0; - int16_t val = int16_t (clip(degree, -90, 70) * slope + 1500); - constexpr int16_t MIN_OUTPUT = 900; - constexpr int16_t MAX_OUTPUT = 1913; -// 900 to 1913 for pulse width input u second -// -90 to 90 for angle input + float slope = (2036.0-972.0) / 160.0; + int16_t val = int16_t (clip(degree, -80, 80) * slope + 1470); + constexpr int16_t MIN_OUTPUT = 972; + constexpr int16_t MAX_OUTPUT = 1947; +// 972 to 1947 for pulse width input u second +// -80 to 80 for angle input this->SetOutput(clip(val, MIN_OUTPUT, MAX_OUTPUT)); this->SetOutput(val); } void PDIHV::SetOutput(int16_t val) { - constexpr int16_t MIN_OUTPUT = 900; - constexpr int16_t MAX_OUTPUT = 1913; + constexpr int16_t MIN_OUTPUT = 972; + constexpr int16_t MAX_OUTPUT = 1947; MotorPWMBase::SetOutput(clip(val, MIN_OUTPUT, MAX_OUTPUT)); } //================================================================================================== From 794a0f8d0064eaf7b40ec59ac211eec812067c9f Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Wed, 2 Aug 2023 01:40:46 +0800 Subject: [PATCH 03/15] finish the chassis and gimbal on the hero(need test) --- vehicles/CMakeLists.txt | 1 + vehicles/Hero/CMakeLists.txt | 26 ++ vehicles/Hero/main.cc | 503 +++++++++++++++++++++++++++++++++++ 3 files changed, 530 insertions(+) create mode 100644 vehicles/Hero/CMakeLists.txt create mode 100644 vehicles/Hero/main.cc diff --git a/vehicles/CMakeLists.txt b/vehicles/CMakeLists.txt index 2bb85c5b..6c2314f0 100644 --- a/vehicles/CMakeLists.txt +++ b/vehicles/CMakeLists.txt @@ -21,3 +21,4 @@ add_subdirectory(Fortress) add_subdirectory(Sentry) add_subdirectory(Steering) +add_subdirectory(Hero) diff --git a/vehicles/Hero/CMakeLists.txt b/vehicles/Hero/CMakeLists.txt new file mode 100644 index 00000000..91881187 --- /dev/null +++ b/vehicles/Hero/CMakeLists.txt @@ -0,0 +1,26 @@ +# ---------------------------------------------------------------------- # +# # +# Copyright (C) 2022 # +# Illini RoboMaster @ University of Illinois at Urbana-Champaign. # +# # +# This program is free software: you can redistribute it and/or modify # +# it under the terms of the GNU General Public License as published by # +# the Free Software Foundation, either version 3 of the License, or # +# (at your option) any later version. # +# # +# This program is distributed in the hope that it will be useful, # +# but WITHOUT ANY WARRANTY; without even the implied warranty of # +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # +# GNU General Public License for more details. # +# # +# You should have received a copy of the GNU General Public License # +# along with this program. If not, see . # +# # +# ---------------------------------------------------------------------- # + +project(Hero ASM C CXX) + +irm_add_arm_executable(Hero + TARGET DJI_Board_TypeC + SOURCES main.cc) + diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc new file mode 100644 index 00000000..6a6f2669 --- /dev/null +++ b/vehicles/Hero/main.cc @@ -0,0 +1,503 @@ +/**************************************************************************** + * * + * Copyright (C) 2023 RoboMaster. * + * Illini RoboMaster @ University of Illinois at Urbana-Champaign * + * * + * This program is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program. If not, see . * + * * + ****************************************************************************/ + +#include "chassis.h" + +#include "bsp_gpio.h" +#include "bsp_os.h" +#include "bsp_print.h" +#include "cmsis_os.h" +#include "controller.h" +#include "dbus.h" +#include "main.h" +#include "motor.h" +#include "protocol.h" +#include "rgb.h" +#include "oled.h" +#include "bsp_buzzer.h" +#include "bsp_laser.h" + +// Global Variables +static remote::DBUS* dbus = nullptr; +static bsp::CAN* can1 = nullptr; +static bsp::CAN* can2 = nullptr; +static display::RGB* RGB = nullptr; + +// Special Modes +static BoolEdgeDetector FakeDeath(false); +static volatile bool Dead = false; +static volatile bool GimbalDead = false; +static volatile bool Elevation = false; + +// Delays +static const int KILLALL_DELAY = 100; +static const int DEFAULT_TASK_DELAY = 100; +static const int CHASSIS_TASK_DELAY = 2; +static const int GIMBAL_TASK_DELAY = 2; +static const int SELFTEST_TASK_DELAY = 100; + +// Params used in both chassis and gimbal task +static volatile float yaw_sum = 0; +static control::Motor4310* yaw_motor = nullptr; + +//================================================================================================== +// Referee +//================================================================================================== + +#define REFEREE_RX_SIGNAL (1 << 1) + +const osThreadAttr_t refereeTaskAttribute = {.name = "refereeTask", + .attr_bits = osThreadDetached, + .cb_mem = nullptr, + .cb_size = 0, + .stack_mem = nullptr, + .stack_size = 1024 * 4, + .priority = (osPriority_t)osPriorityAboveNormal, + .tz_module = 0, + .reserved = 0}; + +osThreadId_t refereeTaskHandle; + +// Referee UART Class initialization +class RefereeUART : public bsp::UART { + public: + using bsp::UART::UART; + + protected: + void RxCompleteCallback() final { osThreadFlagsSet(refereeTaskHandle, REFEREE_RX_SIGNAL); } +}; + +// Params Initialization +static communication::Referee* referee = nullptr; +static RefereeUART* referee_uart = nullptr; + +void refereeTask(void* arg) { + UNUSED(arg); + uint32_t length; + uint8_t* data; + + while (true) { + uint32_t flags = osThreadFlagsWait(REFEREE_RX_SIGNAL, osFlagsWaitAll, osWaitForever); + if (flags & REFEREE_RX_SIGNAL) { + length = referee_uart->Read(&data); + referee->Receive(communication::package_t{data, (int)length}); + } + } +} + +//================================================================================================== +// Chassis(TODO) +//================================================================================================== + +const osThreadAttr_t chassisTaskAttribute = {.name = "chassisTask", + .attr_bits = osThreadDetached, + .cb_mem = nullptr, + .cb_size = 0, + .stack_mem = nullptr, + .stack_size = 256 * 4, + .priority = (osPriority_t)osPriorityNormal, + .tz_module = 0, + .reserved = 0}; +osThreadId_t chassisTaskHandle; + +// Params Initialization +static control::MotorCANBase* fl_motor = nullptr; +static control::MotorCANBase* fr_motor = nullptr; +static control::MotorCANBase* bl_motor = nullptr; +static control::MotorCANBase* br_motor = nullptr; +static control::Chassis* chassis = nullptr; + +void chassisTask(void* arg) { + UNUSED(arg); + // motors initialization + control::MotorCANBase* motors_can1_chassis[] = {fl_motor, fr_motor, bl_motor, br_motor}; + float vx_keyboard = 0, vy_keyboard = 0; + float vx_remote = 0, vy_remote = 0; + float vx = 0, vy = 0, wz = 0; + + // waiting for the start signal + while (true) { + if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; + osDelay(100); + } + + while (true) { + // read the data from keyboard + if (dbus->keyboard.bit.A) vx_keyboard -= 61.5; + if (dbus->keyboard.bit.D) vx_keyboard += 61.5; + if (dbus->keyboard.bit.W) vy_keyboard += 61.5; + if (dbus->keyboard.bit.S) vy_keyboard -= 61.5; + // process the data from keyboard + if (-35 <= vx_keyboard && vx_keyboard <= 35) vx_keyboard = 0; + if (-35 <= vy_keyboard && vy_keyboard <= 35) vy_keyboard = 0; + // soft start and stop in the keyboard + if (vx_keyboard > 0) + vx_keyboard -= 60; + else if (vx_keyboard < 0) + vx_keyboard += 60; + if (vy_keyboard > 0) + vy_keyboard -= 60; + else if (vy_keyboard < 0) + vy_keyboard += 60; + // clip the keyboard data + vx_keyboard = clip(vx_keyboard, -1200, 1200); + vy_keyboard = clip(vy_keyboard, -1200, 1200); + // read the data from the remote controller + vx_remote = dbus->ch0; + vy_remote = dbus->ch1; + // sum whole x and y direction data + vx = vx_keyboard + vx_remote; + vy = vy_keyboard + vy_remote; + // rotation velocity calculation + wz = yaw_sum * 0.5; + + // Update the speed by power limit from refree + if (!Dead && !Elevation) { + chassis->SetSpeed(vx, vy, wz); + } else { + chassis->SetSpeed(0, 0, 0); + } + chassis->Update(true, (float)referee->game_robot_status.chassis_power_limit, + referee->power_heat_data.chassis_power, + (float)referee->power_heat_data.chassis_power_buffer); + + control::MotorCANBase::TransmitOutput(motors_can1_chassis, 4); + + osDelay(CHASSIS_TASK_DELAY); + } +} + +//================================================================================================== +// Gimbal(TODO:need test) +//================================================================================================== + +const osThreadAttr_t gimbalTaskAttribute = {.name = "gimbalTask", + .attr_bits = osThreadDetached, + .cb_mem = nullptr, + .cb_size = 0, + .stack_mem = nullptr, + .stack_size = 512 * 4, + .priority = (osPriority_t)osPriorityHigh, + .tz_module = 0, + .reserved = 0}; +osThreadId_t gimbalTaskHandle; + +// Params Initialization +static control::Motor4310* pitch_motor = nullptr; +static bsp::Laser* laser = nullptr; + +void gimbalTask(void* arg) { + UNUSED(arg); + // motor pointer initialization + control::Motor4310* motors_can2_gimbal[] = {pitch_motor}; + control::Motor4310* motors_can1_gimbal[] = {yaw_motor}; + + print("Wait for beginning signal...\r\n"); + RGB->Display(display::color_red); + laser->On(); + // waiting for start signal + while (true) { + if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; + osDelay(100); + } + +// pitch_motor->SetZeroPos(); //（for test） + // the start code for motor 4310s + pitch_motor->MotorEnable(); + yaw_motor->MotorEnable(); + osDelay(GIMBAL_TASK_DELAY); + + // initialize start position + RGB->Display(display::color_yellow); + laser->Off(); + // need test Pos_Vel mode params + pitch_motor->SetOutput(0, 3); + // need test MIT mode params + yaw_motor->SetOutput(0, 0, 5, 0.5, 0); + control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); + control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); + osDelay(200); + + print("Gimbal Begin!\r\n"); + RGB->Display(display::color_green); + laser->On(); + + float pitch_keyboard = 0; // E is lifting, Q is lowering + float pitch_mouse = 0, yaw_mouse = 0; + float pitch_remote = 0, yaw_remote = 0; + float pitch_sum = 0; + float pitch_target = 0, yaw_target = 0; + float pitch_pos = 0, yaw_pos = 0; + + while (true) { + // Dead + while (Dead || GimbalDead) osDelay(100); + + // Data Collection + // pitch data from keyboard + if (dbus->keyboard.bit.E) pitch_keyboard += (3 * PI); // offset need test + if (dbus->keyboard.bit.Q) pitch_keyboard -= (3 * PI); // offset need test + // pitch data from mouse (direction and offset need test) + pitch_mouse = dbus->mouse.y / 32767.0 * 0.05 * PI; + // pitch data from remote controller (offset and data need test) + pitch_remote = -dbus->ch3 / 660.0 / 210 * PI; + // sum whole pitch data + pitch_sum = pitch_keyboard + pitch_mouse + pitch_remote; + + // yaw data from mouse(direction and offset need test) + yaw_mouse = -dbus->mouse.x / 32767.0 * 0.1 * PI; + yaw_remote = -dbus->ch2 / 660.0 / 210.0 * PI; + // sum whole yaw data + yaw_sum = yaw_mouse + yaw_remote; + + // Data Processing and Update(yaw clip range need test) + // pitch processing + pitch_target = clip(pitch_target + pitch_sum, 0, 100 * PI); + pitch_pos = clip(pitch_pos + pitch_target, 0, 100 * PI); + // pitch update + pitch_motor->SetOutput(pitch_pos, 30); + control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); + // if not elevation, update the yaw position in the chassis + if (Elevation) { + // yaw processing (gear ratio 1 : 4) + yaw_target = clip(yaw_target + yaw_sum, -PI / 2, PI / 2); + yaw_pos = clip(yaw_pos + yaw_target, -PI / 2, PI / 2); + // yaw update + yaw_motor->SetOutput(yaw_pos, 0, 5, 0.5, 0); + control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); + } + + osDelay(GIMBAL_TASK_DELAY); + } +} + +//================================================================================================== +// SelfTest(TODO: need to modify the position) +//================================================================================================== + +const osThreadAttr_t selfTestingTask = {.name = "selfTestTask", + .attr_bits = osThreadDetached, + .cb_mem = nullptr, + .cb_size = 0, + .stack_mem = nullptr, + .stack_size = 256 * 4, + .priority = (osPriority_t)osPriorityBelowNormal, + .tz_module = 0, + .reserved = 0}; +osThreadId_t selfTestTaskHandle; + +// Params Initialization +static bsp::Buzzer* buzzer = nullptr; +static display::OLED* OLED = nullptr; +using Note = bsp::BuzzerNote; +static bsp::BuzzerNoteDelayed Mario[] = { + {Note::Mi3M, 80}, {Note::Silent, 80}, {Note::Mi3M, 80}, {Note::Silent, 240}, {Note::Mi3M, 80}, {Note::Silent, 240}, {Note::Do1M, 80}, {Note::Silent, 80}, {Note::Mi3M, 80}, {Note::Silent, 240}, {Note::So5M, 80}, {Note::Silent, 560}, {Note::So5L, 80}, {Note::Silent, 0}, {Note::Finish, 0}}; + +// Chassis motor flags +static bool fl_motor_flag = false; +static bool fr_motor_flag = false; +static bool bl_motor_flag = false; +static bool br_motor_flag = false; +// Gimbal motor flags +static bool pitch_motor_flag = false; +static bool yaw_motor_flag = false; +// Dbus +static bool dbus_flag = false; + +void self_Check_Task(void* arg) { + UNUSED(arg); + osDelay(SELFTEST_TASK_DELAY); + // Enter screen + OLED->ShowIlliniRMLOGO(); + buzzer->SingSong(Mario, [](uint32_t milli) { osDelay(milli);}); + OLED->OperateGram(display::PEN_CLEAR); + + // Showing Name for self-check + // Gimbal motor + OLED->ShowString(0, 0, (uint8_t*)"GP"); + OLED->ShowString(0, 5, (uint8_t*)"GY"); + // Lidar + OLED->ShowString(2, 5, (uint8_t*)"Ldr"); + // Dbus + OLED->ShowString(3, 6, (uint8_t*)"Dbs"); + // Chassis motor + OLED->ShowString(1, 12, (uint8_t*)"FL"); + OLED->ShowString(2, 12, (uint8_t*)"FR"); + OLED->ShowString(3, 12, (uint8_t*)"BL"); + OLED->ShowString(4, 12, (uint8_t*)"BR"); + + while (true) { + // chassis motors + fl_motor->connection_flag_ = false; + fr_motor->connection_flag_ = false; + bl_motor->connection_flag_ = false; + br_motor->connection_flag_ = false; + // gimbal motors + pitch_motor->connection_flag_ = false; + yaw_motor->connection_flag_ = false; + // Dbus + dbus->connection_flag_ = false; + osDelay(SELFTEST_TASK_DELAY); + // chassis motors + fl_motor_flag = fl_motor->connection_flag_; + fr_motor_flag = fr_motor->connection_flag_; + bl_motor_flag = bl_motor->connection_flag_; + br_motor_flag = br_motor->connection_flag_; + // gimbal motors + pitch_motor_flag = pitch_motor->connection_flag_; + yaw_motor_flag = yaw_motor->connection_flag_; + // Dbus + dbus_flag = dbus->connection_flag_; + + //Showing the result for self check + // Chassis motors + OLED->ShowBlock(1, 15, fl_motor_flag); + OLED->ShowBlock(2, 15, fr_motor_flag); + OLED->ShowBlock(3, 15, bl_motor_flag); + OLED->ShowBlock(4, 15, br_motor_flag); + // Gimbal motors + OLED->ShowBlock(0, 2, pitch_motor_flag); + OLED->ShowBlock(0, 7, yaw_motor_flag); + // Dbus + OLED->ShowBlock(3, 9, dbus_flag); + + osDelay(100); + } +} + +//================================================================================================== +// RM Init +//================================================================================================== + +void RM_RTOS_Init() { + // peripherals initialization + print_use_uart(&huart1); + bsp::SetHighresClockTimer(&htim5); + // Dbus + dbus = new remote::DBUS(&huart3); + // Can + can1 = new bsp::CAN(&hcan1, true); + can2 = new bsp::CAN(&hcan2, false); + // RGB + RGB = new display::RGB(&htim5, 3, 2, 1, 1000000); + + // Chassis class initialization + // motor initialization + fl_motor = new control::Motor3508(can1, 0x201); + fr_motor = new control::Motor3508(can1, 0x202); + br_motor = new control::Motor3508(can1, 0x203); + bl_motor = new control::Motor3508(can1, 0x204); + // motor distribution + control::MotorCANBase* motors[control::FourWheel::motor_num]; + motors[control::FourWheel::front_left] = fl_motor; + motors[control::FourWheel::front_right] = fr_motor; + motors[control::FourWheel::back_left] = bl_motor; + motors[control::FourWheel::back_right] = br_motor; + // chassis struct initialization + control::chassis_t chassis_data; + chassis_data.motors = motors; + chassis_data.model = control::CHASSIS_MECANUM_WHEEL; + chassis = new control::Chassis(chassis_data); + + // Gimbal initialization + pitch_motor = new control::Motor4310(can2, 0x02, 0x01, control::POS_VEL); + yaw_motor = new control::Motor4310(can1, 0x04, 0x03, control::MIT); + + // Referee initialization + referee_uart = new RefereeUART(&huart6); + referee_uart->SetupRx(300); + referee_uart->SetupTx(300); + referee = new communication::Referee; +} + +//================================================================================================== +// RTOS Threads Init +//================================================================================================== + +void RM_RTOS_Threads_Init(void) { + refereeTaskHandle = osThreadNew(refereeTask, nullptr, &refereeTaskAttribute); + chassisTaskHandle = osThreadNew(chassisTask, nullptr, &chassisTaskAttribute); + gimbalTaskHandle = osThreadNew(gimbalTask, nullptr, &gimbalTaskAttribute); + selfTestTaskHandle = osThreadNew(self_Check_Task, nullptr, &selfTestingTask); +} + +//================================================================================================== +// Kill All +//================================================================================================== + +void KillAll() { + RM_EXPECT_TRUE(false, "Operation Killed!\r\n"); + + control::MotorCANBase* motors_can1_chassis[] = {fl_motor, fr_motor, bl_motor, br_motor}; + RGB->Display(display::color_blue); + + while (true) { + // Restart after emergency stop + FakeDeath.input(dbus->swl == remote::DOWN); + if (FakeDeath.posEdge()) { + Dead = false; + RGB->Display(display::color_green); + laser->On(); + pitch_motor->MotorEnable(); + yaw_motor->MotorEnable(); + break; + } + // Dead, wait for restart signal from User + // Gimbal motors + pitch_motor->MotorDisable(); + yaw_motor->MotorDisable(); + // Chassis motors + fl_motor->SetOutput(0); + bl_motor->SetOutput(0); + fr_motor->SetOutput(0); + br_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(motors_can1_chassis, 4); + + osDelay(KILLALL_DELAY); + } +} + +//================================================================================================== +// RTOS Default Task +//================================================================================================== +// Debug signal +static bool debug = false; + +void RM_RTOS_Default_Task(const void* args) { + UNUSED(args); + + while (true) { + // Emergency stop + FakeDeath.input(dbus->swl == remote::DOWN); + if (FakeDeath.posEdge()) { + Dead = true; + KillAll(); + } + + if (debug) { + set_cursor(0, 0); + clear_screen(); + print("power limit: %.3f chassis power: %.3f power buffer: %.3f\r\n", (float)referee->game_robot_status.chassis_power_limit, + referee->power_heat_data.chassis_power, + (float)referee->power_heat_data.chassis_power_buffer); + } + osDelay(DEFAULT_TASK_DELAY); + } +} From 21c3d754853b5325f998d12151025e5fe030a32f Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Wed, 2 Aug 2023 23:46:30 +0800 Subject: [PATCH 04/15] start add the shooter --- shared/libraries/chassis.cc | 2 +- vehicles/Hero/main.cc | 88 +++++++++++++++++++++++++------------ 2 files changed, 61 insertions(+), 29 deletions(-) diff --git a/shared/libraries/chassis.cc b/shared/libraries/chassis.cc index dd0590a4..c723e046 100644 --- a/shared/libraries/chassis.cc +++ b/shared/libraries/chassis.cc @@ -38,7 +38,7 @@ Chassis::Chassis(const chassis_t chassis) : pids_() { motors_[FourWheel::back_left] = chassis.motors[FourWheel::back_left]; motors_[FourWheel::back_right] = chassis.motors[FourWheel::back_right]; - float* pid_param = new float[3]{40, 3, 0}; + float* pid_param = new float[3]{40, 6, 1}; float motor_max_iout = 2000; float motor_max_out = 20000; pids_[FourWheel::front_left].Reinit(pid_param, motor_max_iout, motor_max_out); diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 6a6f2669..c65b4e84 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -44,7 +44,7 @@ static display::RGB* RGB = nullptr; static BoolEdgeDetector FakeDeath(false); static volatile bool Dead = false; static volatile bool GimbalDead = false; -static volatile bool Elevation = false; +static volatile bool Elevation = true; // Delays static const int KILLALL_DELAY = 100; @@ -166,7 +166,7 @@ void chassisTask(void* arg) { vx = vx_keyboard + vx_remote; vy = vy_keyboard + vy_remote; // rotation velocity calculation - wz = yaw_sum * 0.5; + wz = yaw_sum * 10000; // 10000 is magic number // Update the speed by power limit from refree if (!Dead && !Elevation) { @@ -174,18 +174,19 @@ void chassisTask(void* arg) { } else { chassis->SetSpeed(0, 0, 0); } + print("vel: vx: %f, vy: %f, wz: %f\r\n", vx, vy, wz); + print("power limit: %d\r\n", referee->game_robot_status.chassis_power_limit); chassis->Update(true, (float)referee->game_robot_status.chassis_power_limit, referee->power_heat_data.chassis_power, (float)referee->power_heat_data.chassis_power_buffer); control::MotorCANBase::TransmitOutput(motors_can1_chassis, 4); - osDelay(CHASSIS_TASK_DELAY); } } //================================================================================================== -// Gimbal(TODO:need test) +// Gimbal(TODO:need test 4310 connection) //================================================================================================== const osThreadAttr_t gimbalTaskAttribute = {.name = "gimbalTask", @@ -201,17 +202,17 @@ osThreadId_t gimbalTaskHandle; // Params Initialization static control::Motor4310* pitch_motor = nullptr; -static bsp::Laser* laser = nullptr; +//static bsp::Laser* laser = nullptr; void gimbalTask(void* arg) { UNUSED(arg); // motor pointer initialization - control::Motor4310* motors_can2_gimbal[] = {pitch_motor}; +// control::Motor4310* motors_can2_gimbal[] = {pitch_motor}; control::Motor4310* motors_can1_gimbal[] = {yaw_motor}; print("Wait for beginning signal...\r\n"); RGB->Display(display::color_red); - laser->On(); +// laser->On(); // has some problem. // waiting for start signal while (true) { if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; @@ -219,33 +220,35 @@ void gimbalTask(void* arg) { } // pitch_motor->SetZeroPos(); //（for test） +// yaw_motor->SetZeroPos(); // for test + osDelay(1000); // wait for 4310s enable + // the start code for motor 4310s - pitch_motor->MotorEnable(); +// pitch_motor->MotorEnable(); yaw_motor->MotorEnable(); osDelay(GIMBAL_TASK_DELAY); // initialize start position RGB->Display(display::color_yellow); - laser->Off(); +// laser->Off(); // need test Pos_Vel mode params - pitch_motor->SetOutput(0, 3); +// pitch_motor->SetOutput(0, 3); // need test MIT mode params - yaw_motor->SetOutput(0, 0, 5, 0.5, 0); + yaw_motor->SetOutput(0, 0, 8, 1.5, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); - control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); +// control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); osDelay(200); print("Gimbal Begin!\r\n"); RGB->Display(display::color_green); - laser->On(); +// laser->On(); float pitch_keyboard = 0; // E is lifting, Q is lowering float pitch_mouse = 0, yaw_mouse = 0; float pitch_remote = 0, yaw_remote = 0; float pitch_sum = 0; - float pitch_target = 0, yaw_target = 0; + float pitch_target = 0; float pitch_pos = 0, yaw_pos = 0; - while (true) { // Dead while (Dead || GimbalDead) osDelay(100); @@ -262,8 +265,8 @@ void gimbalTask(void* arg) { pitch_sum = pitch_keyboard + pitch_mouse + pitch_remote; // yaw data from mouse(direction and offset need test) - yaw_mouse = -dbus->mouse.x / 32767.0 * 0.1 * PI; - yaw_remote = -dbus->ch2 / 660.0 / 210.0 * PI; + yaw_mouse = dbus->mouse.x / 32767.0 * 0.1 * PI; + yaw_remote = dbus->ch2 / 660.0 / 210.0 / 5 * PI; // sum whole yaw data yaw_sum = yaw_mouse + yaw_remote; @@ -272,15 +275,14 @@ void gimbalTask(void* arg) { pitch_target = clip(pitch_target + pitch_sum, 0, 100 * PI); pitch_pos = clip(pitch_pos + pitch_target, 0, 100 * PI); // pitch update - pitch_motor->SetOutput(pitch_pos, 30); - control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); +// pitch_motor->SetOutput(pitch_pos, 30); +// control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); // if not elevation, update the yaw position in the chassis if (Elevation) { // yaw processing (gear ratio 1 : 4) - yaw_target = clip(yaw_target + yaw_sum, -PI / 2, PI / 2); - yaw_pos = clip(yaw_pos + yaw_target, -PI / 2, PI / 2); + yaw_pos = clip(yaw_pos + yaw_sum, -PI / 3, PI / 3); // yaw update - yaw_motor->SetOutput(yaw_pos, 0, 5, 0.5, 0); + yaw_motor->SetOutput(yaw_pos, 5, 20, 1, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); } @@ -288,6 +290,30 @@ void gimbalTask(void* arg) { } } +//================================================================================================== +// Shooter(TODO) +//================================================================================================== +const osThreadAttr_t shooterTaskAttribute = {.name = "shooterTask", + .attr_bits = osThreadDetached, + .cb_mem = nullptr, + .cb_size = 0, + .stack_mem = nullptr, + .stack_size = 256 * 4, + .priority = (osPriority_t)osPriorityNormal, + .tz_module = 0, + .reserved = 0}; + +osThreadId_t shooterTaskHandle; + +static control::MotorCANBase* force_motor = nullptr; +static control::MotorCANBase* load_motor = nullptr; +static control::MotorCANBase* reload_motor = nullptr; + +void shooterTask(void* arg) { + UNUSED(arg); + +} + //================================================================================================== // SelfTest(TODO: need to modify the position) //================================================================================================== @@ -400,10 +426,10 @@ void RM_RTOS_Init() { // Chassis class initialization // motor initialization - fl_motor = new control::Motor3508(can1, 0x201); - fr_motor = new control::Motor3508(can1, 0x202); - br_motor = new control::Motor3508(can1, 0x203); - bl_motor = new control::Motor3508(can1, 0x204); + fl_motor = new control::Motor3508(can1, 0x202); + fr_motor = new control::Motor3508(can1, 0x201); + bl_motor = new control::Motor3508(can1, 0x203); + br_motor = new control::Motor3508(can1, 0x204); // motor distribution control::MotorCANBase* motors[control::FourWheel::motor_num]; motors[control::FourWheel::front_left] = fl_motor; @@ -420,6 +446,11 @@ void RM_RTOS_Init() { pitch_motor = new control::Motor4310(can2, 0x02, 0x01, control::POS_VEL); yaw_motor = new control::Motor4310(can1, 0x04, 0x03, control::MIT); + //Shooter initialization + load_motor = new control::Motor3508(can2, 0x201); + reload_motor = new control::Motor3508(can2, 0x202); + force_motor = new control::Motor3508(can2, 0x203); + // Referee initialization referee_uart = new RefereeUART(&huart6); referee_uart->SetupRx(300); @@ -435,7 +466,8 @@ void RM_RTOS_Threads_Init(void) { refereeTaskHandle = osThreadNew(refereeTask, nullptr, &refereeTaskAttribute); chassisTaskHandle = osThreadNew(chassisTask, nullptr, &chassisTaskAttribute); gimbalTaskHandle = osThreadNew(gimbalTask, nullptr, &gimbalTaskAttribute); - selfTestTaskHandle = osThreadNew(self_Check_Task, nullptr, &selfTestingTask); +// shooterTaskHandle = osThreadNew(shooterTask, nullptr, &shooterTaskAttribute); +// selfTestTaskHandle = osThreadNew(self_Check_Task, nullptr, &selfTestingTask); } //================================================================================================== @@ -454,7 +486,7 @@ void KillAll() { if (FakeDeath.posEdge()) { Dead = false; RGB->Display(display::color_green); - laser->On(); +// laser->On(); pitch_motor->MotorEnable(); yaw_motor->MotorEnable(); break; From 1b5c4d14bdb7b6bcf28ef3162cb2d89f5409b196 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Thu, 3 Aug 2023 04:12:25 +0800 Subject: [PATCH 05/15] test example --- examples/hero/CMakeLists.txt | 4 + examples/hero/hero_shooter_2023.cc | 190 +++++++++++++++++++++++++++++ shared/libraries/motor.cc | 1 + vehicles/Hero/main.cc | 132 ++++++++++++++++++++ 4 files changed, 327 insertions(+) create mode 100644 examples/hero/hero_shooter_2023.cc diff --git a/examples/hero/CMakeLists.txt b/examples/hero/CMakeLists.txt index d041656d..160a8c9a 100644 --- a/examples/hero/CMakeLists.txt +++ b/examples/hero/CMakeLists.txt @@ -23,3 +23,7 @@ project(example_hero ASM C CXX) irm_add_arm_executable(${PROJECT_NAME} TARGET DJI_Board_TypeA SOURCES main.cc) + +irm_add_arm_executable(${PROJECT_NAME}_shooter_2023 + TARGET DJI_Board_TypeC + SOURCES hero_shooter_2023.cc) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc new file mode 100644 index 00000000..e7a6046e --- /dev/null +++ b/examples/hero/hero_shooter_2023.cc @@ -0,0 +1,190 @@ +/**************************************************************************** + * * + * Copyright (C) 2023 RoboMaster. * + * Illini RoboMaster @ University of Illinois at Urbana-Champaign * + * * + * This program is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program. If not, see . * + * * + ****************************************************************************/ + +#include "chassis.h" + +#include "bsp_gpio.h" +#include "bsp_os.h" +#include "bsp_print.h" +#include "cmsis_os.h" +#include "controller.h" +#include "dbus.h" +#include "main.h" +#include "motor.h" +#include "protocol.h" +#include "rgb.h" +#include "oled.h" +#include "bsp_buzzer.h" +#include "bsp_laser.h" + +// Global Variables +static remote::DBUS* dbus = nullptr; +static bsp::CAN* can1 = nullptr; +static display::RGB* RGB = nullptr; + +// Special Modes +static BoolEdgeDetector LoadDetect(false); +static BoolEdgeDetector ForceDetect(false); + +static control::MotorCANBase* force_motor = nullptr; +static control::MotorCANBase* load_motor = nullptr; +static control::MotorCANBase* reload_motor = nullptr; +static control::PDIHV* trigger = nullptr; // CCW for positive angle, CW for negative angle +static control::ServoMotor* load_servo = nullptr; +static control::ServoMotor* reload_servo = nullptr; +static control::ServoMotor* force_servo = nullptr; + +void RM_RTOS_Init() { + // peripherals initialization + print_use_uart(&huart1); + bsp::SetHighresClockTimer(&htim5); + // Dbus + dbus = new remote::DBUS(&huart3); + // Can + can1 = new bsp::CAN(&hcan1, true); + // RGB + RGB = new display::RGB(&htim5, 3, 2, 1, 1000000); + + //Shooter initialization + load_motor = new control::Motor3508(can1, 0x201); + reload_motor = new control::Motor3508(can1, 0x202); + force_motor = new control::Motor3508(can1, 0x203); + // magic number from the data test for this servo + trigger = new control::PDIHV(&htim1, 1, 1980000, 500, 1500); + + // Servo control for each shooter motor + control::servo_t servo_data; + servo_data.motor = load_motor; + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; + servo_data.transmission_ratio = M3508P19_RATIO; + servo_data.omega_pid_param = new float [3] {1, 1, 1}; // pid need test + servo_data.max_iout = 1000; + servo_data.max_out = 13000; + load_servo = new control::ServoMotor(servo_data); + + servo_data.motor = reload_motor; + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; + servo_data.omega_pid_param = new float [3] {1, 1, 1}; // pid need test + reload_servo = new control::ServoMotor(servo_data); + + servo_data.motor = force_motor; + servo_data.max_speed = 6 * PI; // params need test + servo_data.max_acceleration = 50 * PI; + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test + force_servo = new control::ServoMotor(servo_data); +} + +void RM_RTOS_Default_Task(const void* args) { + UNUSED(args); + control::MotorCANBase* can2_reloader[] = { reload_motor}; +// control::MotorCANBase* can2_loader[] = { load_motor}; +// control::MotorCANBase* can2_force[] = { force_motor}; + + // Variable initialization (params need test) + // reload variables +// bool reload_pull = false; +// bool reload_push = false; + float reload_pos = 10*PI; + // load variable +// bool loading = false; +// float load_angle = 10*PI; + int i = 0; + +// waiting for the start signal + while (true) { + if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; + osDelay(100); + } + + while (true) { + // the shoot and load process is automatic + // 1. using servo pull the trigger (shooting process) + // 2. reload motor pull the bullet board to the desire position (load process below) + // 3. using servo push the trigger the hold the bullet board + // 4. load motor load one bullet on the board + // 5. reload motor release to the original position to prepare for the next load + // 6. optional: determine whether we need to change the force motor position + // 7. repeat 1-6 + i = 0; + // Load Detector + LoadDetect.input(dbus->swr == remote::UP); + if (LoadDetect.posEdge()) { + // step 1 + trigger->SetOutPutAngle(20); // need test the trigger angle + osDelay(1000); // need test the delay time(wait for the) + // step 2 + while (true) { + // break condition (reach the desire position) + if (++i > 10 / 2 && abs(reload_servo->GetOmega()) <= 0.0001) break; + // set target pull position once +// if (!reload_pull) { +// reload_pull = true; + reload_servo->SetTarget(reload_pos, true); +// } + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + osDelay(2); + } + // after reload pulling + i = 0; +// reload_pull = false; + osDelay(300); // need test the delay time(wait for the) + // step 3 + trigger->SetOutPutAngle(-80); // need test the trigger angle + osDelay(1000); // need test the delay time(wait for the) + // step 4 +// while (true) { +// // break condition (loading) +// if (++i > 10 / 2 && abs(load_servo->GetOmega()) <= 0.0001) break; +// // loading once +// if (!loading) { +// loading = true; +// load_servo->SetTarget(load_servo->GetTarget() + load_angle, true); +// } +// load_servo->CalcOutput(); +// control::MotorCANBase::TransmitOutput(can2_loader, 1); +// osDelay(2); +// } +// // after loading +// loading = false; +// osDelay(300); // need test the delay time(wait for the + // step 5 +// while (true) { +// // break condition (reach the desire position) +// if (reload_servo->GetOmega() <= 0.0001) break; +// // set target push position once +// if (!reload_push) { +// reload_push = true; +// reload_servo->SetTarget(0, true); +// } +// reload_servo->CalcOutput(); +// control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); +// osDelay(2); +// } +// // after reload pushing +// reload_push = false; +// osDelay(300); // need test the delay time(wait for the) +// // step 6(TODO) + } + osDelay(10); + } +} diff --git a/shared/libraries/motor.cc b/shared/libraries/motor.cc index d4b4befc..540bb37b 100644 --- a/shared/libraries/motor.cc +++ b/shared/libraries/motor.cc @@ -328,6 +328,7 @@ void PDIHV::SetOutPutAngle(float degree) { this->SetOutput(clip(val, MIN_OUTPUT, MAX_OUTPUT)); this->SetOutput(val); } + void PDIHV::SetOutput(int16_t val) { constexpr int16_t MIN_OUTPUT = 972; constexpr int16_t MAX_OUTPUT = 1947; diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index c65b4e84..23a7534c 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -42,6 +42,8 @@ static display::RGB* RGB = nullptr; // Special Modes static BoolEdgeDetector FakeDeath(false); +static BoolEdgeDetector LoadDetect(false); +static BoolEdgeDetector ForceDetect(false); static volatile bool Dead = false; static volatile bool GimbalDead = false; static volatile bool Elevation = true; @@ -56,6 +58,9 @@ static const int SELFTEST_TASK_DELAY = 100; // Params used in both chassis and gimbal task static volatile float yaw_sum = 0; static control::Motor4310* yaw_motor = nullptr; +// Params used in both gimbal and shooter task +static volatile bool force_flag = false; +static volatile bool force_transforming = false; //================================================================================================== // Referee @@ -308,10 +313,105 @@ osThreadId_t shooterTaskHandle; static control::MotorCANBase* force_motor = nullptr; static control::MotorCANBase* load_motor = nullptr; static control::MotorCANBase* reload_motor = nullptr; +static control::PDIHV* trigger = nullptr; // CCW for positive angle, CW for negative angle (trigger angle is 100 degree) +static control::ServoMotor* load_servo = nullptr; +static control::ServoMotor* reload_servo = nullptr; +static control::ServoMotor* force_servo = nullptr; void shooterTask(void* arg) { UNUSED(arg); + // motors initialization + control::MotorCANBase* motors_can2_shooter[] = {force_motor, load_motor, reload_motor}; + // Variable initialization (params need test) + // reload variables + bool reload_pull = false; + bool reload_push = false; + float reload_pos = 10 * PI; + // load variable + bool loading = false; + float load_angle = PI; + + // waiting for the start signal + while (true) { + if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; + osDelay(100); + } + while (true) { + // Dead + while (Dead || GimbalDead) osDelay(100); + + // the shoot and load process is automatic + // 1. using servo pull the trigger (shooting process) + // 2. reload motor pull the bullet board to the desire position (load process below) + // 3. using servo push the trigger the hold the bullet board + // 4. load motor load one bullet on the board + // 5. reload motor release to the original position to prepare for the next load + // 6. optional: determine whether we need to change the force motor position + // 7. repeat 1-6 + + // Load Detector + LoadDetect.input(dbus->mouse.l); + if (LoadDetect.posEdge()) { + // step 1 + trigger->SetOutPutAngle(20); + osDelay(1000); // need test the delay time(wait for the) + // step 2 + while (true) { + // break condition (reach the desire position) + if (reload_servo->GetOmega() <= 0.0001) break; + // set target pull position once + if (!reload_pull) { + reload_pull = true; + reload_servo->SetTarget(reload_pos, true); + } + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); + osDelay(2); + } + // after reload pulling + reload_pull = false; + osDelay(1000); // need test the delay time(wait for the) + // step 3 + trigger->SetOutPutAngle(-80); + osDelay(300); // need test the delay time(wait for the) + // step 4 + while (true) { + // break condition (loading) + if (load_servo->GetOmega() <= 0.0001) break; + // loading once + if (!loading) { + loading = true; + load_servo->SetTarget(load_angle, true); + } + load_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); + osDelay(2); + + } + // after loading + loading = false; + osDelay(300); // need test the delay time(wait for the) + // step 5 + while (true) { + // break condition (reach the desire position) + if (reload_servo->GetOmega() <= 0.0001) break; + // set target push position once + if (!reload_push) { + reload_push = true; + reload_servo->SetTarget(0, true); + } + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); + osDelay(2); + } + // after reload pushing + reload_push = false; + osDelay(300); // need test the delay time(wait for the) + // step 6(TODO) + } + osDelay(10); + } } //================================================================================================== @@ -450,6 +550,31 @@ void RM_RTOS_Init() { load_motor = new control::Motor3508(can2, 0x201); reload_motor = new control::Motor3508(can2, 0x202); force_motor = new control::Motor3508(can2, 0x203); + // magic number from the data test for this servo + trigger = new control::PDIHV(&htim1, 1, 1980000, 500, 1500); + + // Servo control for each shooter motor + control::servo_t servo_data; + servo_data.motor = load_motor; + servo_data.max_speed = 6 * PI; // params need test + servo_data.max_acceleration = 8 * PI; + servo_data.transmission_ratio = M3508P19_RATIO; + servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + servo_data.max_iout = 1000; + servo_data.max_out = 13000; + load_servo = new control::ServoMotor(servo_data); + + servo_data.motor = reload_motor; + servo_data.max_speed = 6 * PI; // params need test + servo_data.max_acceleration = 8 * PI; + servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + reload_servo = new control::ServoMotor(servo_data); + + servo_data.motor = force_motor; + servo_data.max_speed = 6 * PI; // params need test + servo_data.max_acceleration = 8 * PI; + servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + force_servo = new control::ServoMotor(servo_data); // Referee initialization referee_uart = new RefereeUART(&huart6); @@ -478,6 +603,7 @@ void KillAll() { RM_EXPECT_TRUE(false, "Operation Killed!\r\n"); control::MotorCANBase* motors_can1_chassis[] = {fl_motor, fr_motor, bl_motor, br_motor}; + control::MotorCANBase* motors_can2_shooter[] = {load_motor, reload_motor, force_motor}; RGB->Display(display::color_blue); while (true) { @@ -501,6 +627,11 @@ void KillAll() { fr_motor->SetOutput(0); br_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(motors_can1_chassis, 4); + // shooter motors + load_motor->SetOutput(0); + reload_motor->SetOutput(0); + force_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); osDelay(KILLALL_DELAY); } @@ -518,6 +649,7 @@ void RM_RTOS_Default_Task(const void* args) { while (true) { // Emergency stop FakeDeath.input(dbus->swl == remote::DOWN); + ForceDetect.input(dbus->keyboard.bit.F); if (FakeDeath.posEdge()) { Dead = true; KillAll(); From 1e2fe9d323ac2de4d88ae6678eb0066eaad70718 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Thu, 3 Aug 2023 07:44:01 +0800 Subject: [PATCH 06/15] pass the load test and pitch test, need keyboard and mouse test --- examples/hero/hero_shooter_2023.cc | 111 ++++++++++++++---------- vehicles/Hero/main.cc | 130 +++++++++++++++++------------ 2 files changed, 144 insertions(+), 97 deletions(-) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index e7a6046e..d361a908 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -37,6 +37,7 @@ // Global Variables static remote::DBUS* dbus = nullptr; static bsp::CAN* can1 = nullptr; +static bsp::CAN* can2 = nullptr; static display::RGB* RGB = nullptr; // Special Modes @@ -59,11 +60,12 @@ void RM_RTOS_Init() { dbus = new remote::DBUS(&huart3); // Can can1 = new bsp::CAN(&hcan1, true); + can2 = new bsp::CAN(&hcan2, false); // RGB RGB = new display::RGB(&htim5, 3, 2, 1, 1000000); //Shooter initialization - load_motor = new control::Motor3508(can1, 0x201); + load_motor = new control::Motor3508(can2, 0x201); reload_motor = new control::Motor3508(can1, 0x202); force_motor = new control::Motor3508(can1, 0x203); // magic number from the data test for this servo @@ -75,7 +77,7 @@ void RM_RTOS_Init() { servo_data.max_speed = 4 * PI; // params need test servo_data.max_acceleration = 10 * PI; servo_data.transmission_ratio = M3508P19_RATIO; - servo_data.omega_pid_param = new float [3] {1, 1, 1}; // pid need test + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test servo_data.max_iout = 1000; servo_data.max_out = 13000; load_servo = new control::ServoMotor(servo_data); @@ -83,7 +85,7 @@ void RM_RTOS_Init() { servo_data.motor = reload_motor; servo_data.max_speed = 4 * PI; // params need test servo_data.max_acceleration = 10 * PI; - servo_data.omega_pid_param = new float [3] {1, 1, 1}; // pid need test + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; @@ -95,18 +97,18 @@ void RM_RTOS_Init() { void RM_RTOS_Default_Task(const void* args) { UNUSED(args); - control::MotorCANBase* can2_reloader[] = { reload_motor}; -// control::MotorCANBase* can2_loader[] = { load_motor}; +// control::MotorCANBase* can2_reloader[] = { reload_motor}; + control::MotorCANBase* can2_loader[] = { load_motor}; // control::MotorCANBase* can2_force[] = { force_motor}; // Variable initialization (params need test) // reload variables // bool reload_pull = false; // bool reload_push = false; - float reload_pos = 10*PI; +// float reload_pos = 10 * PI; // load variable -// bool loading = false; -// float load_angle = 10*PI; + bool loading = false; + float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio int i = 0; // waiting for the start signal @@ -116,6 +118,9 @@ void RM_RTOS_Default_Task(const void* args) { } while (true) { + // Dead +// while (Dead || GimbalDead) osDelay(100); + // the shoot and load process is automatic // 1. using servo pull the trigger (shooting process) // 2. reload motor pull the bullet board to the desire position (load process below) @@ -129,61 +134,77 @@ void RM_RTOS_Default_Task(const void* args) { LoadDetect.input(dbus->swr == remote::UP); if (LoadDetect.posEdge()) { // step 1 - trigger->SetOutPutAngle(20); // need test the trigger angle - osDelay(1000); // need test the delay time(wait for the) - // step 2 - while (true) { - // break condition (reach the desire position) - if (++i > 10 / 2 && abs(reload_servo->GetOmega()) <= 0.0001) break; - // set target pull position once +// trigger->SetOutPutAngle(20); // need test the trigger angle +// osDelay(1000); // need test the delay time(wait for the) +// // step 2 +// while (true) { +// // break condition (reach the desire position) +// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; +// // set the speed and acceleration for the reload motor +// // set target pull position once // if (!reload_pull) { // reload_pull = true; - reload_servo->SetTarget(reload_pos, true); +// reload_servo->SetTarget(reload_pos); // } - reload_servo->CalcOutput(); - control::MotorCANBase::TransmitOutput(can2_reloader, 1); +// print("reload theta: %f\n", reload_servo->GetTheta()); +// reload_servo->CalcOutput(); +// control::MotorCANBase::TransmitOutput(can2_reloader, 1); +// osDelay(2); +// } +// // after reload pulling +// i = 0; +// reload_motor->SetOutput(0); +// print("omega %f", reload_servo->GetOmega()); +// control::MotorCANBase::TransmitOutput(can2_reloader, 1); +// reload_pull = false; +// osDelay(100); // need test the delay time(wait for the) +// // step 3 +// trigger->SetOutPutAngle(-80); // need test the trigger angle + osDelay(100); // need test the delay time(wait for the) + // step 4 + while (true) { + // break condition (loading) + if (++i > 20 / 2 && abs(load_servo->GetOmega()) <= 0.001) break; + // loading once + if (!loading) { + loading = true; + load_servo->SetTarget(load_servo->GetTarget() - load_angle); + print("load target: %f\n", load_servo->GetTarget()); + } + print("load Theta: %f\n", load_servo->GetTheta()); + load_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_loader, 1); osDelay(2); } - // after reload pulling + // after loading i = 0; -// reload_pull = false; - osDelay(300); // need test the delay time(wait for the) - // step 3 - trigger->SetOutPutAngle(-80); // need test the trigger angle - osDelay(1000); // need test the delay time(wait for the) - // step 4 -// while (true) { -// // break condition (loading) -// if (++i > 10 / 2 && abs(load_servo->GetOmega()) <= 0.0001) break; -// // loading once -// if (!loading) { -// loading = true; -// load_servo->SetTarget(load_servo->GetTarget() + load_angle, true); -// } -// load_servo->CalcOutput(); -// control::MotorCANBase::TransmitOutput(can2_loader, 1); -// osDelay(2); -// } -// // after loading -// loading = false; -// osDelay(300); // need test the delay time(wait for the - // step 5 + load_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_loader, 1); + loading = false; + osDelay(100); // need test the delay time(wait for the +// // step 5 // while (true) { // // break condition (reach the desire position) -// if (reload_servo->GetOmega() <= 0.0001) break; +// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; // // set target push position once // if (!reload_push) { // reload_push = true; // reload_servo->SetTarget(0, true); +// print("reload target: %f\n", reload_servo->GetTarget()); +// print("reload theta: %f\n", reload_servo->GetTheta()); // } // reload_servo->CalcOutput(); -// control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); +// control::MotorCANBase::TransmitOutput(can2_reloader, 1); // osDelay(2); // } // // after reload pushing +// i = 0; +// reload_motor->SetOutput(0); +// control::MotorCANBase::TransmitOutput(can2_reloader, 1); // reload_push = false; -// osDelay(300); // need test the delay time(wait for the) -// // step 6(TODO) +// osDelay(100); // need test the delay time(wait for the) + // step 6(TODO) + } osDelay(10); } diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 23a7534c..47255373 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -46,7 +46,7 @@ static BoolEdgeDetector LoadDetect(false); static BoolEdgeDetector ForceDetect(false); static volatile bool Dead = false; static volatile bool GimbalDead = false; -static volatile bool Elevation = true; +static volatile bool Elevation = false; // Delays static const int KILLALL_DELAY = 100; @@ -108,7 +108,7 @@ void refereeTask(void* arg) { } //================================================================================================== -// Chassis(TODO) +// Chassis //================================================================================================== const osThreadAttr_t chassisTaskAttribute = {.name = "chassisTask", @@ -121,6 +121,8 @@ const osThreadAttr_t chassisTaskAttribute = {.name = "chassisTask", .tz_module = 0, .reserved = 0}; osThreadId_t chassisTaskHandle; +// TODO: +// 1. test keyboard and mouse data // Params Initialization static control::MotorCANBase* fl_motor = nullptr; @@ -191,7 +193,7 @@ void chassisTask(void* arg) { } //================================================================================================== -// Gimbal(TODO:need test 4310 connection) +// Gimbal //================================================================================================== const osThreadAttr_t gimbalTaskAttribute = {.name = "gimbalTask", @@ -204,6 +206,9 @@ const osThreadAttr_t gimbalTaskAttribute = {.name = "gimbalTask", .tz_module = 0, .reserved = 0}; osThreadId_t gimbalTaskHandle; +// TODO: +// 1. test keyboard and mouse data +// 2. test pitch start cali // Params Initialization static control::Motor4310* pitch_motor = nullptr; @@ -212,7 +217,7 @@ static control::Motor4310* pitch_motor = nullptr; void gimbalTask(void* arg) { UNUSED(arg); // motor pointer initialization -// control::Motor4310* motors_can2_gimbal[] = {pitch_motor}; + control::Motor4310* motors_can2_gimbal[] = {pitch_motor}; control::Motor4310* motors_can1_gimbal[] = {yaw_motor}; print("Wait for beginning signal...\r\n"); @@ -229,7 +234,7 @@ void gimbalTask(void* arg) { osDelay(1000); // wait for 4310s enable // the start code for motor 4310s -// pitch_motor->MotorEnable(); + pitch_motor->MotorEnable(); yaw_motor->MotorEnable(); osDelay(GIMBAL_TASK_DELAY); @@ -237,11 +242,11 @@ void gimbalTask(void* arg) { RGB->Display(display::color_yellow); // laser->Off(); // need test Pos_Vel mode params -// pitch_motor->SetOutput(0, 3); + pitch_motor->SetOutput(0, 3); // need test MIT mode params yaw_motor->SetOutput(0, 0, 8, 1.5, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); -// control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); + control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); osDelay(200); print("Gimbal Begin!\r\n"); @@ -252,7 +257,6 @@ void gimbalTask(void* arg) { float pitch_mouse = 0, yaw_mouse = 0; float pitch_remote = 0, yaw_remote = 0; float pitch_sum = 0; - float pitch_target = 0; float pitch_pos = 0, yaw_pos = 0; while (true) { // Dead @@ -260,12 +264,12 @@ void gimbalTask(void* arg) { // Data Collection // pitch data from keyboard - if (dbus->keyboard.bit.E) pitch_keyboard += (3 * PI); // offset need test - if (dbus->keyboard.bit.Q) pitch_keyboard -= (3 * PI); // offset need test + if (dbus->keyboard.bit.E) pitch_keyboard -= (10 * PI); // offset need test + if (dbus->keyboard.bit.Q) pitch_keyboard += (10 * PI); // offset need test // pitch data from mouse (direction and offset need test) pitch_mouse = dbus->mouse.y / 32767.0 * 0.05 * PI; // pitch data from remote controller (offset and data need test) - pitch_remote = -dbus->ch3 / 660.0 / 210 * PI; + pitch_remote = -dbus->ch3 / 660.0 * 30 * PI; // sum whole pitch data pitch_sum = pitch_keyboard + pitch_mouse + pitch_remote; @@ -277,11 +281,10 @@ void gimbalTask(void* arg) { // Data Processing and Update(yaw clip range need test) // pitch processing - pitch_target = clip(pitch_target + pitch_sum, 0, 100 * PI); - pitch_pos = clip(pitch_pos + pitch_target, 0, 100 * PI); + pitch_pos = clip(pitch_pos + pitch_sum, -70 * PI, 70 * PI); // pitch update -// pitch_motor->SetOutput(pitch_pos, 30); -// control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); + pitch_motor->SetOutput(pitch_pos, pitch_sum); + control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); // if not elevation, update the yaw position in the chassis if (Elevation) { // yaw processing (gear ratio 1 : 4) @@ -310,6 +313,10 @@ const osThreadAttr_t shooterTaskAttribute = {.name = "shooterTask", osThreadId_t shooterTaskHandle; +// TODO: +// All test params + +// Params Initialization static control::MotorCANBase* force_motor = nullptr; static control::MotorCANBase* load_motor = nullptr; static control::MotorCANBase* reload_motor = nullptr; @@ -321,15 +328,18 @@ static control::ServoMotor* force_servo = nullptr; void shooterTask(void* arg) { UNUSED(arg); // motors initialization - control::MotorCANBase* motors_can2_shooter[] = {force_motor, load_motor, reload_motor}; - // Variable initialization (params need test) + control::MotorCANBase* can2_reloader[] = { reload_motor}; + control::MotorCANBase* can2_loader[] = { load_motor}; + // control::MotorCANBase* can2_force[] = { force_motor}; // Variable initialization (params need test) // reload variables bool reload_pull = false; bool reload_push = false; float reload_pos = 10 * PI; // load variable bool loading = false; - float load_angle = PI; + // 99.506 is the ratio of the load servo and devide the 3508 ratio to load one bullet + float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; + int i= 0; // waiting for the start signal while (true) { @@ -349,66 +359,82 @@ void shooterTask(void* arg) { // 5. reload motor release to the original position to prepare for the next load // 6. optional: determine whether we need to change the force motor position // 7. repeat 1-6 - + i = 0; // Load Detector - LoadDetect.input(dbus->mouse.l); + LoadDetect.input(dbus->swr == remote::UP); if (LoadDetect.posEdge()) { // step 1 trigger->SetOutPutAngle(20); - osDelay(1000); // need test the delay time(wait for the) + osDelay(1000); // wait for the bullet out // step 2 while (true) { // break condition (reach the desire position) - if (reload_servo->GetOmega() <= 0.0001) break; + if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + // set the speed and acceleration for the reload motor // set target pull position once if (!reload_pull) { reload_pull = true; - reload_servo->SetTarget(reload_pos, true); + reload_servo->SetTarget(reload_pos); } + print("reload theta: %f\n", reload_servo->GetTheta()); reload_servo->CalcOutput(); - control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); osDelay(2); } // after reload pulling + i = 0; + reload_motor->SetOutput(0); + print("omega %f", reload_servo->GetOmega()); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; - osDelay(1000); // need test the delay time(wait for the) + osDelay(100); // need test the delay time(wait for the) // step 3 trigger->SetOutPutAngle(-80); - osDelay(300); // need test the delay time(wait for the) + osDelay(100); // need test the delay time(wait for the) // step 4 while (true) { // break condition (loading) - if (load_servo->GetOmega() <= 0.0001) break; + if (++i > 20 / 2 && abs(load_servo->GetOmega()) <= 0.001) break; // loading once if (!loading) { loading = true; - load_servo->SetTarget(load_angle, true); + load_servo->SetTarget(load_servo->GetTarget() + load_angle); + print("load target: %f\n", load_servo->GetTarget()); } + print("load Theta: %f\n", load_servo->GetTheta()); load_servo->CalcOutput(); - control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); + control::MotorCANBase::TransmitOutput(can2_loader, 1); osDelay(2); - } // after loading + i = 0; + load_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_loader, 1); loading = false; - osDelay(300); // need test the delay time(wait for the) + osDelay(100); // need test the delay time // step 5 - while (true) { - // break condition (reach the desire position) - if (reload_servo->GetOmega() <= 0.0001) break; - // set target push position once - if (!reload_push) { - reload_push = true; - reload_servo->SetTarget(0, true); - } - reload_servo->CalcOutput(); - control::MotorCANBase::TransmitOutput(motors_can2_shooter, 3); - osDelay(2); + while (true) { + // break condition (reach the desire position) + if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + // set target push position once + if (!reload_push) { + reload_push = true; + reload_servo->SetTarget(0, true); + print("reload target: %f\n", reload_servo->GetTarget()); + print("reload theta: %f\n", reload_servo->GetTheta()); } + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + osDelay(2); + } // after reload pushing + i = 0; + reload_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_push = false; - osDelay(300); // need test the delay time(wait for the) + osDelay(100); // step 6(TODO) + } osDelay(10); } @@ -556,24 +582,24 @@ void RM_RTOS_Init() { // Servo control for each shooter motor control::servo_t servo_data; servo_data.motor = load_motor; - servo_data.max_speed = 6 * PI; // params need test - servo_data.max_acceleration = 8 * PI; + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; servo_data.transmission_ratio = M3508P19_RATIO; - servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; servo_data.max_iout = 1000; servo_data.max_out = 13000; load_servo = new control::ServoMotor(servo_data); servo_data.motor = reload_motor; - servo_data.max_speed = 6 * PI; // params need test - servo_data.max_acceleration = 8 * PI; - servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; - servo_data.max_speed = 6 * PI; // params need test - servo_data.max_acceleration = 8 * PI; - servo_data.omega_pid_param = new float [3] {30, 5, 2}; // pid need test + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; + servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; force_servo = new control::ServoMotor(servo_data); // Referee initialization From 94dc9dd41fab59e495ebb854080893ab6715fc40 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Thu, 3 Aug 2023 17:22:59 +0800 Subject: [PATCH 07/15] need test --- examples/hero/hero_shooter_2023.cc | 54 +++++++++++++++++-- vehicles/Hero/main.cc | 85 ++++++++++++++++++++++-------- 2 files changed, 115 insertions(+), 24 deletions(-) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index d361a908..94eaca38 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -42,7 +42,6 @@ static display::RGB* RGB = nullptr; // Special Modes static BoolEdgeDetector LoadDetect(false); -static BoolEdgeDetector ForceDetect(false); static control::MotorCANBase* force_motor = nullptr; static control::MotorCANBase* load_motor = nullptr; @@ -52,6 +51,9 @@ static control::ServoMotor* load_servo = nullptr; static control::ServoMotor* reload_servo = nullptr; static control::ServoMotor* force_servo = nullptr; +static BoolEdgeDetector ForceWeakDetect(false); +static BoolEdgeDetector ForceStrongDetect(false); + void RM_RTOS_Init() { // peripherals initialization print_use_uart(&huart1); @@ -65,7 +67,7 @@ void RM_RTOS_Init() { RGB = new display::RGB(&htim5, 3, 2, 1, 1000000); //Shooter initialization - load_motor = new control::Motor3508(can2, 0x201); + load_motor = new control::Motor3508(can1, 0x201); reload_motor = new control::Motor3508(can1, 0x202); force_motor = new control::Motor3508(can1, 0x203); // magic number from the data test for this servo @@ -99,7 +101,7 @@ void RM_RTOS_Default_Task(const void* args) { UNUSED(args); // control::MotorCANBase* can2_reloader[] = { reload_motor}; control::MotorCANBase* can2_loader[] = { load_motor}; -// control::MotorCANBase* can2_force[] = { force_motor}; + control::MotorCANBase* can2_force[] = { force_motor}; // Variable initialization (params need test) // reload variables @@ -110,6 +112,12 @@ void RM_RTOS_Default_Task(const void* args) { bool loading = false; float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio int i = 0; + // force variable + bool force_week = false; + bool force_strong = false; + bool force_transforming = false; + float force_pos = 0; + float force_angle = 10 * PI; // waiting for the start signal while (true) { @@ -121,6 +129,46 @@ void RM_RTOS_Default_Task(const void* args) { // Dead // while (Dead || GimbalDead) osDelay(100); + // whether change the force motor position + ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->swr == remote::UP); + ForceStrongDetect.input(dbus->keyboard.bit.C || dbus->swr == remote::UP); + // just execute force transform once + if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) { + force_transforming = true; + force_week = true; + force_strong = false; + } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) { + force_transforming = true; + force_week = false; + force_strong = true; + } + // force transforming + if (force_transforming) { + while (true) { + // break condition (reach the desire position) + if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001) break; + // set the speed and acceleration for the reload motor + // set target pull position once + if (i == 1) { + // direction need test + if (force_week && !force_strong) { + force_pos += force_angle; + } else if (force_strong && !force_week) { + force_pos -= force_angle; + } + force_servo->SetTarget(force_pos); + } + force_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_force, 1); + osDelay(2); + } + // after changing the force motor position + i = 0; + force_transforming = false; + force_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_force, 1); + osDelay(100); // need test the delay time(wait for the) + } // the shoot and load process is automatic // 1. using servo pull the trigger (shooting process) // 2. reload motor pull the bullet board to the desire position (load process below) diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 47255373..23e64648 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -43,7 +43,6 @@ static display::RGB* RGB = nullptr; // Special Modes static BoolEdgeDetector FakeDeath(false); static BoolEdgeDetector LoadDetect(false); -static BoolEdgeDetector ForceDetect(false); static volatile bool Dead = false; static volatile bool GimbalDead = false; static volatile bool Elevation = false; @@ -58,9 +57,6 @@ static const int SELFTEST_TASK_DELAY = 100; // Params used in both chassis and gimbal task static volatile float yaw_sum = 0; static control::Motor4310* yaw_motor = nullptr; -// Params used in both gimbal and shooter task -static volatile bool force_flag = false; -static volatile bool force_transforming = false; //================================================================================================== // Referee @@ -325,12 +321,17 @@ static control::ServoMotor* load_servo = nullptr; static control::ServoMotor* reload_servo = nullptr; static control::ServoMotor* force_servo = nullptr; +static BoolEdgeDetector ForceWeakDetect(false); +static BoolEdgeDetector ForceStrongDetect(false); + void shooterTask(void* arg) { UNUSED(arg); // motors initialization control::MotorCANBase* can2_reloader[] = { reload_motor}; control::MotorCANBase* can2_loader[] = { load_motor}; - // control::MotorCANBase* can2_force[] = { force_motor}; // Variable initialization (params need test) + control::MotorCANBase* can2_force[] = { force_motor}; + + // Variable initialization (params need test) // reload variables bool reload_pull = false; bool reload_push = false; @@ -339,6 +340,13 @@ void shooterTask(void* arg) { bool loading = false; // 99.506 is the ratio of the load servo and devide the 3508 ratio to load one bullet float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; + // force variable + bool force_week = false; + bool force_strong = false; + bool force_transforming = false; + float force_pos = 0; + float force_angle = 10 * PI; + int i= 0; // waiting for the start signal @@ -351,17 +359,58 @@ void shooterTask(void* arg) { // Dead while (Dead || GimbalDead) osDelay(100); + i = 0; + // whether change the force motor position + ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue); + ForceStrongDetect.input(dbus->keyboard.bit.C + || (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue)); + // just execute force transform once + if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) { + force_transforming = true; + force_week = true; + force_strong = false; + } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) { + force_transforming = true; + force_week = false; + force_strong = true; + } + // force transforming + if (force_transforming) { + while (true) { + // break condition (reach the desire position) + if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001) break; + // set the speed and acceleration for the reload motor + // set target pull position once + if (i == 1) { + // direction need test + if (force_week && !force_strong) { + force_pos += force_angle; + } else if (force_strong && !force_week) { + force_pos -= force_angle; + } + force_servo->SetTarget(force_pos); + } + force_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_force, 1); + osDelay(2); + } + // after changing the force motor position + i = 0; + force_transforming = false; + force_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_force, 1); + osDelay(100); // need test the delay time(wait for the) + } + // the shoot and load process is automatic // 1. using servo pull the trigger (shooting process) // 2. reload motor pull the bullet board to the desire position (load process below) // 3. using servo push the trigger the hold the bullet board // 4. load motor load one bullet on the board // 5. reload motor release to the original position to prepare for the next load - // 6. optional: determine whether we need to change the force motor position - // 7. repeat 1-6 - i = 0; + // Load Detector - LoadDetect.input(dbus->swr == remote::UP); + LoadDetect.input(dbus->swr == remote::UP || dbus->mouse.l); if (LoadDetect.posEdge()) { // step 1 trigger->SetOutPutAngle(20); @@ -369,7 +418,7 @@ void shooterTask(void* arg) { // step 2 while (true) { // break condition (reach the desire position) - if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001) break; // set the speed and acceleration for the reload motor // set target pull position once if (!reload_pull) { @@ -384,7 +433,6 @@ void shooterTask(void* arg) { // after reload pulling i = 0; reload_motor->SetOutput(0); - print("omega %f", reload_servo->GetOmega()); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; osDelay(100); // need test the delay time(wait for the) @@ -394,14 +442,12 @@ void shooterTask(void* arg) { // step 4 while (true) { // break condition (loading) - if (++i > 20 / 2 && abs(load_servo->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001) break; // loading once if (!loading) { loading = true; - load_servo->SetTarget(load_servo->GetTarget() + load_angle); - print("load target: %f\n", load_servo->GetTarget()); + load_servo->SetTarget(load_servo->GetTarget() - load_angle); } - print("load Theta: %f\n", load_servo->GetTheta()); load_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_loader, 1); osDelay(2); @@ -415,13 +461,11 @@ void shooterTask(void* arg) { // step 5 while (true) { // break condition (reach the desire position) - if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001) break; // set target push position once if (!reload_push) { reload_push = true; reload_servo->SetTarget(0, true); - print("reload target: %f\n", reload_servo->GetTarget()); - print("reload theta: %f\n", reload_servo->GetTheta()); } reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); @@ -433,9 +477,9 @@ void shooterTask(void* arg) { control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_push = false; osDelay(100); - // step 6(TODO) - } + + dbus->previous_wheel_value = dbus->wheel.wheel; osDelay(10); } } @@ -675,7 +719,6 @@ void RM_RTOS_Default_Task(const void* args) { while (true) { // Emergency stop FakeDeath.input(dbus->swl == remote::DOWN); - ForceDetect.input(dbus->keyboard.bit.F); if (FakeDeath.posEdge()) { Dead = true; KillAll(); From e995bafcf142e01bbc3ef7eed580f096d2e4f008 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Thu, 3 Aug 2023 22:38:20 +0800 Subject: [PATCH 08/15] most param need test --- examples/hero/hero_shooter_2023.cc | 119 +++++++++++++++-------------- examples/relay/TypeC.cc | 2 +- vehicles/Hero/main.cc | 60 ++++++++++++--- 3 files changed, 109 insertions(+), 72 deletions(-) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index 94eaca38..8b4bf241 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -91,23 +91,23 @@ void RM_RTOS_Init() { reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; - servo_data.max_speed = 6 * PI; // params need test - servo_data.max_acceleration = 50 * PI; + servo_data.max_speed = 4 * PI; // params need test + servo_data.max_acceleration = 10 * PI; servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test force_servo = new control::ServoMotor(servo_data); } void RM_RTOS_Default_Task(const void* args) { UNUSED(args); -// control::MotorCANBase* can2_reloader[] = { reload_motor}; + control::MotorCANBase* can2_reloader[] = { reload_motor}; control::MotorCANBase* can2_loader[] = { load_motor}; control::MotorCANBase* can2_force[] = { force_motor}; // Variable initialization (params need test) // reload variables -// bool reload_pull = false; -// bool reload_push = false; -// float reload_pos = 10 * PI; + bool reload_pull = false; + bool reload_push = false; + float reload_pos = 10 * PI; // load variable bool loading = false; float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio @@ -126,18 +126,19 @@ void RM_RTOS_Default_Task(const void* args) { } while (true) { - // Dead -// while (Dead || GimbalDead) osDelay(100); - // whether change the force motor position - ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->swr == remote::UP); - ForceStrongDetect.input(dbus->keyboard.bit.C || dbus->swr == remote::UP); + ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue); + ForceStrongDetect.input(dbus->keyboard.bit.C + || (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue)); + // just execute force transform once if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) { + print("week"); force_transforming = true; force_week = true; force_strong = false; } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) { + print("strong"); force_transforming = true; force_week = false; force_strong = true; @@ -182,32 +183,32 @@ void RM_RTOS_Default_Task(const void* args) { LoadDetect.input(dbus->swr == remote::UP); if (LoadDetect.posEdge()) { // step 1 -// trigger->SetOutPutAngle(20); // need test the trigger angle -// osDelay(1000); // need test the delay time(wait for the) -// // step 2 -// while (true) { -// // break condition (reach the desire position) -// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; -// // set the speed and acceleration for the reload motor -// // set target pull position once -// if (!reload_pull) { -// reload_pull = true; -// reload_servo->SetTarget(reload_pos); -// } -// print("reload theta: %f\n", reload_servo->GetTheta()); -// reload_servo->CalcOutput(); -// control::MotorCANBase::TransmitOutput(can2_reloader, 1); -// osDelay(2); -// } -// // after reload pulling -// i = 0; -// reload_motor->SetOutput(0); -// print("omega %f", reload_servo->GetOmega()); -// control::MotorCANBase::TransmitOutput(can2_reloader, 1); -// reload_pull = false; -// osDelay(100); // need test the delay time(wait for the) -// // step 3 -// trigger->SetOutPutAngle(-80); // need test the trigger angle + trigger->SetOutPutAngle(20); // need test the trigger angle + osDelay(1000); // need test the delay time(wait for the) + // step 2 + while (true) { + // break condition (reach the desire position) + if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + // set the speed and acceleration for the reload motor + // set target pull position once + if (!reload_pull) { + reload_pull = true; + reload_servo->SetTarget(reload_pos); + } + print("reload theta: %f\n", reload_servo->GetTheta()); + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + osDelay(2); + } + // after reload pulling + i = 0; + reload_motor->SetOutput(0); + print("omega %f", reload_servo->GetOmega()); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + reload_pull = false; + osDelay(100); // need test the delay time(wait for the) + // step 3 + trigger->SetOutPutAngle(-80); // need test the trigger angle osDelay(100); // need test the delay time(wait for the) // step 4 while (true) { @@ -230,30 +231,30 @@ void RM_RTOS_Default_Task(const void* args) { control::MotorCANBase::TransmitOutput(can2_loader, 1); loading = false; osDelay(100); // need test the delay time(wait for the -// // step 5 -// while (true) { -// // break condition (reach the desire position) -// if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; -// // set target push position once -// if (!reload_push) { -// reload_push = true; -// reload_servo->SetTarget(0, true); -// print("reload target: %f\n", reload_servo->GetTarget()); -// print("reload theta: %f\n", reload_servo->GetTheta()); -// } -// reload_servo->CalcOutput(); -// control::MotorCANBase::TransmitOutput(can2_reloader, 1); -// osDelay(2); -// } -// // after reload pushing -// i = 0; -// reload_motor->SetOutput(0); -// control::MotorCANBase::TransmitOutput(can2_reloader, 1); -// reload_push = false; -// osDelay(100); // need test the delay time(wait for the) - // step 6(TODO) + // step 5 + while (true) { + // break condition (reach the desire position) + if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + // set target push position once + if (!reload_push) { + reload_push = true; + reload_servo->SetTarget(0, true); + print("reload target: %f\n", reload_servo->GetTarget()); + print("reload theta: %f\n", reload_servo->GetTheta()); + } + reload_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + osDelay(2); + } + // after reload pushing + i = 0; + reload_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_reloader, 1); + reload_push = false; + osDelay(100); // need test the delay time(wait for the) } + dbus->previous_wheel_value = dbus->wheel.wheel; osDelay(10); } } diff --git a/examples/relay/TypeC.cc b/examples/relay/TypeC.cc index 82df6718..3e6af935 100644 --- a/examples/relay/TypeC.cc +++ b/examples/relay/TypeC.cc @@ -47,7 +47,7 @@ static bsp::Relay* relay; void RM_RTOS_Init(void) { print_use_uart(&huart1); - relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14) */ + relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14)->PIN 7 in the board*/ } void RM_RTOS_Default_Task(const void* arguments) { diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 23e64648..6efe7fc6 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -23,6 +23,7 @@ #include "bsp_gpio.h" #include "bsp_os.h" #include "bsp_print.h" +#include "bsp_relay.h" #include "cmsis_os.h" #include "controller.h" #include "dbus.h" @@ -39,13 +40,17 @@ static remote::DBUS* dbus = nullptr; static bsp::CAN* can1 = nullptr; static bsp::CAN* can2 = nullptr; static display::RGB* RGB = nullptr; +static bsp::Relay* relay = nullptr; // Special Modes static BoolEdgeDetector FakeDeath(false); static BoolEdgeDetector LoadDetect(false); +static BoolEdgeDetector ElevationDetect(false); +static BoolEdgeDetector PreloadDetect(false); static volatile bool Dead = false; static volatile bool GimbalDead = false; static volatile bool Elevation = false; +static volatile bool Preload = false; // Delays static const int KILLALL_DELAY = 100; @@ -119,6 +124,7 @@ const osThreadAttr_t chassisTaskAttribute = {.name = "chassisTask", osThreadId_t chassisTaskHandle; // TODO: // 1. test keyboard and mouse data +// 2. test relay // Params Initialization static control::MotorCANBase* fl_motor = nullptr; @@ -142,6 +148,19 @@ void chassisTask(void* arg) { } while (true) { + + // Elevation(shift) + ElevationDetect.input(dbus->keyboard.bit.SHIFT); + if (ElevationDetect.posEdge()) { + Elevation = !Elevation; + if (Elevation) { + relay->On(); + } else { + relay->Off(); + } + osDelay(100); + } + // read the data from keyboard if (dbus->keyboard.bit.A) vx_keyboard -= 61.5; if (dbus->keyboard.bit.D) vx_keyboard += 61.5; @@ -177,8 +196,7 @@ void chassisTask(void* arg) { } else { chassis->SetSpeed(0, 0, 0); } - print("vel: vx: %f, vy: %f, wz: %f\r\n", vx, vy, wz); - print("power limit: %d\r\n", referee->game_robot_status.chassis_power_limit); + chassis->Update(true, (float)referee->game_robot_status.chassis_power_limit, referee->power_heat_data.chassis_power, (float)referee->power_heat_data.chassis_power_buffer); @@ -360,6 +378,26 @@ void shooterTask(void* arg) { while (Dead || GimbalDead) osDelay(100); i = 0; + if (Preload) { + while (true) { + // break condition (loading) + if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001) break; + // loading once + if (i == 1) { + Preload = false; + load_servo->SetTarget(load_servo->GetTarget() - load_angle); + } + load_servo->CalcOutput(); + control::MotorCANBase::TransmitOutput(can2_loader, 1); + osDelay(2); + } + // after loading + i = 0; + load_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(can2_loader, 1); + osDelay(100); // need test the delay time + } + // whether change the force motor position ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue); ForceStrongDetect.input(dbus->keyboard.bit.C @@ -651,6 +689,9 @@ void RM_RTOS_Init() { referee_uart->SetupRx(300); referee_uart->SetupTx(300); referee = new communication::Referee; + + // Relay initialization + relay = new bsp::Relay(RELAY_1_GPIO_Port, RELAY_1_Pin); /* USE GPIO_1 (PB14)->PIN 7 in the board */ } //================================================================================================== @@ -661,7 +702,7 @@ void RM_RTOS_Threads_Init(void) { refereeTaskHandle = osThreadNew(refereeTask, nullptr, &refereeTaskAttribute); chassisTaskHandle = osThreadNew(chassisTask, nullptr, &chassisTaskAttribute); gimbalTaskHandle = osThreadNew(gimbalTask, nullptr, &gimbalTaskAttribute); -// shooterTaskHandle = osThreadNew(shooterTask, nullptr, &shooterTaskAttribute); + shooterTaskHandle = osThreadNew(shooterTask, nullptr, &shooterTaskAttribute); // selfTestTaskHandle = osThreadNew(self_Check_Task, nullptr, &selfTestingTask); } @@ -710,8 +751,6 @@ void KillAll() { //================================================================================================== // RTOS Default Task //================================================================================================== -// Debug signal -static bool debug = false; void RM_RTOS_Default_Task(const void* args) { UNUSED(args); @@ -723,14 +762,11 @@ void RM_RTOS_Default_Task(const void* args) { Dead = true; KillAll(); } - - if (debug) { - set_cursor(0, 0); - clear_screen(); - print("power limit: %.3f chassis power: %.3f power buffer: %.3f\r\n", (float)referee->game_robot_status.chassis_power_limit, - referee->power_heat_data.chassis_power, - (float)referee->power_heat_data.chassis_power_buffer); + PreloadDetect.input(dbus->swl == remote::UP); + if (PreloadDetect.posEdge() && !Preload) { + Preload = true; } + osDelay(DEFAULT_TASK_DELAY); } } From fe4ba7be20c75fd553b7a135b193e79cf8e6eacb Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Fri, 4 Aug 2023 05:34:08 +0800 Subject: [PATCH 09/15] just leave the reload motor parameter --- examples/dbus/CMakeLists.txt | 2 +- examples/dbus/main.cc | 4 +- examples/hero/hero_shooter_2023.cc | 41 +++++----- vehicles/Hero/main.cc | 117 +++++++++++++++++++---------- 4 files changed, 98 insertions(+), 66 deletions(-) diff --git a/examples/dbus/CMakeLists.txt b/examples/dbus/CMakeLists.txt index f373f6a5..542a22a5 100644 --- a/examples/dbus/CMakeLists.txt +++ b/examples/dbus/CMakeLists.txt @@ -21,6 +21,6 @@ project(example_dbus ASM C CXX) irm_add_arm_executable(${PROJECT_NAME} - TARGET DJI_Board_TypeA + TARGET DJI_Board_TypeC SOURCES main.cc) diff --git a/examples/dbus/main.cc b/examples/dbus/main.cc index 38850368..fbcb2bf8 100644 --- a/examples/dbus/main.cc +++ b/examples/dbus/main.cc @@ -27,8 +27,8 @@ static remote::DBUS* dbus; void RM_RTOS_Init(void) { - print_use_uart(&huart8); - dbus = new remote::DBUS(&huart1); + print_use_uart(&huart1); + dbus = new remote::DBUS(&huart3); } void RM_RTOS_Default_Task(const void* arguments) { diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index 8b4bf241..aa69a61a 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -67,9 +67,9 @@ void RM_RTOS_Init() { RGB = new display::RGB(&htim5, 3, 2, 1, 1000000); //Shooter initialization - load_motor = new control::Motor3508(can1, 0x201); - reload_motor = new control::Motor3508(can1, 0x202); - force_motor = new control::Motor3508(can1, 0x203); + load_motor = new control::Motor3508(can2, 0x201); + reload_motor = new control::Motor3508(can2, 0x202); + force_motor = new control::Motor3508(can2, 0x203); // magic number from the data test for this servo trigger = new control::PDIHV(&htim1, 1, 1980000, 500, 1500); @@ -86,8 +86,8 @@ void RM_RTOS_Init() { servo_data.motor = reload_motor; servo_data.max_speed = 4 * PI; // params need test - servo_data.max_acceleration = 10 * PI; - servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; // pid need test + servo_data.max_acceleration = 8 * PI; + servo_data.omega_pid_param = new float [3] {130, 15, 5}; // pid need test reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; @@ -107,7 +107,7 @@ void RM_RTOS_Default_Task(const void* args) { // reload variables bool reload_pull = false; bool reload_push = false; - float reload_pos = 10 * PI; + float reload_pos = 3.7 * PI * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the reload servo, devide the 3508 ratio // load variable bool loading = false; float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio @@ -117,7 +117,7 @@ void RM_RTOS_Default_Task(const void* args) { bool force_strong = false; bool force_transforming = false; float force_pos = 0; - float force_angle = 10 * PI; + float force_angle = 55 * PI ; // waiting for the start signal while (true) { @@ -127,7 +127,7 @@ void RM_RTOS_Default_Task(const void* args) { while (true) { // whether change the force motor position - ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue); + ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel < 1024); ForceStrongDetect.input(dbus->keyboard.bit.C || (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue)); @@ -183,44 +183,44 @@ void RM_RTOS_Default_Task(const void* args) { LoadDetect.input(dbus->swr == remote::UP); if (LoadDetect.posEdge()) { // step 1 - trigger->SetOutPutAngle(20); // need test the trigger angle - osDelay(1000); // need test the delay time(wait for the) + trigger->SetOutPutAngle(0); // need test the trigger angle + osDelay(100); // need test the delay time(wait for the) // step 2 while (true) { // break condition (reach the desire position) - if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 50 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; // set the speed and acceleration for the reload motor // set target pull position once if (!reload_pull) { reload_pull = true; reload_servo->SetTarget(reload_pos); + print("target: %f", reload_servo->GetTarget()); } print("reload theta: %f\n", reload_servo->GetTheta()); reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); osDelay(2); } + // step 3 + trigger->SetOutPutAngle(-80); // need test the trigger angle + osDelay(1700); // need test the delay time(wait for the) // after reload pulling i = 0; reload_motor->SetOutput(0); print("omega %f", reload_servo->GetOmega()); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; - osDelay(100); // need test the delay time(wait for the) - // step 3 - trigger->SetOutPutAngle(-80); // need test the trigger angle - osDelay(100); // need test the delay time(wait for the) + osDelay(50); // need test the delay time(wait for the) + // step 4 while (true) { // break condition (loading) - if (++i > 20 / 2 && abs(load_servo->GetOmega()) <= 0.001) break; + if (++i > 50 / 2 && abs(load_servo->GetOmega()) <= 0.001) break; // loading once if (!loading) { loading = true; load_servo->SetTarget(load_servo->GetTarget() - load_angle); - print("load target: %f\n", load_servo->GetTarget()); } - print("load Theta: %f\n", load_servo->GetTheta()); load_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_loader, 1); osDelay(2); @@ -234,13 +234,11 @@ void RM_RTOS_Default_Task(const void* args) { // step 5 while (true) { // break condition (reach the desire position) - if (++i > 20 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 50 / 2 && abs(reload_servo->GetOmega()) <= 0.001) break; // set target push position once if (!reload_push) { reload_push = true; reload_servo->SetTarget(0, true); - print("reload target: %f\n", reload_servo->GetTarget()); - print("reload theta: %f\n", reload_servo->GetTheta()); } reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); @@ -252,7 +250,6 @@ void RM_RTOS_Default_Task(const void* args) { control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_push = false; osDelay(100); // need test the delay time(wait for the) - } dbus->previous_wheel_value = dbus->wheel.wheel; osDelay(10); diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 6efe7fc6..d922908b 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -57,6 +57,9 @@ static const int KILLALL_DELAY = 100; static const int DEFAULT_TASK_DELAY = 100; static const int CHASSIS_TASK_DELAY = 2; static const int GIMBAL_TASK_DELAY = 2; +static const int SHOOTER_INNER_TASK_DELAY = 2; +static const int SHOOTER_OUTER_TASK_DELAY = 100; +static const int SHOOTER_TASK_DELAY = 10; static const int SELFTEST_TASK_DELAY = 100; // Params used in both chassis and gimbal task @@ -137,6 +140,7 @@ void chassisTask(void* arg) { UNUSED(arg); // motors initialization control::MotorCANBase* motors_can1_chassis[] = {fl_motor, fr_motor, bl_motor, br_motor}; + control::Motor4310* motor_can1_gimbal[] = {yaw_motor}; float vx_keyboard = 0, vy_keyboard = 0; float vx_remote = 0, vy_remote = 0; float vx = 0, vy = 0, wz = 0; @@ -188,11 +192,13 @@ void chassisTask(void* arg) { vx = vx_keyboard + vx_remote; vy = vy_keyboard + vy_remote; // rotation velocity calculation - wz = yaw_sum * 10000; // 10000 is magic number + wz = yaw_sum * 120000; // 120000 is magic number - // Update the speed by power limit from refree + // Update the speed by power limit from referee if (!Dead && !Elevation) { chassis->SetSpeed(vx, vy, wz); + yaw_motor->SetOutput(0, 0, 8, 1.5, 0); + control::Motor4310::TransmitOutput(motor_can1_gimbal, 1); } else { chassis->SetSpeed(0, 0, 0); } @@ -222,7 +228,6 @@ const osThreadAttr_t gimbalTaskAttribute = {.name = "gimbalTask", osThreadId_t gimbalTaskHandle; // TODO: // 1. test keyboard and mouse data -// 2. test pitch start cali // Params Initialization static control::Motor4310* pitch_motor = nullptr; @@ -256,7 +261,7 @@ void gimbalTask(void* arg) { RGB->Display(display::color_yellow); // laser->Off(); // need test Pos_Vel mode params - pitch_motor->SetOutput(0, 3); + pitch_motor->SetOutput(0, 10); // need test MIT mode params yaw_motor->SetOutput(0, 0, 8, 1.5, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); @@ -278,20 +283,28 @@ void gimbalTask(void* arg) { // Data Collection // pitch data from keyboard - if (dbus->keyboard.bit.E) pitch_keyboard -= (10 * PI); // offset need test - if (dbus->keyboard.bit.Q) pitch_keyboard += (10 * PI); // offset need test + if (dbus->keyboard.bit.E) pitch_keyboard -= PI; // offset need test + if (dbus->keyboard.bit.Q) pitch_keyboard += PI; // offset need test + if (-0.55 * PI <= pitch_keyboard && pitch_keyboard <= 0.55 * PI) pitch_keyboard = 0; + if (pitch_keyboard > 0) + pitch_keyboard -= 0.85 * PI; + if (pitch_keyboard < 0) + pitch_keyboard += 0.85 * PI; + pitch_keyboard = clip(pitch_keyboard, -30 * PI, 30 * PI); + // pitch data from mouse (direction and offset need test) - pitch_mouse = dbus->mouse.y / 32767.0 * 0.05 * PI; + pitch_mouse = dbus->mouse.y / 32767.0 * 1000 * PI; // pitch data from remote controller (offset and data need test) pitch_remote = -dbus->ch3 / 660.0 * 30 * PI; // sum whole pitch data pitch_sum = pitch_keyboard + pitch_mouse + pitch_remote; // yaw data from mouse(direction and offset need test) - yaw_mouse = dbus->mouse.x / 32767.0 * 0.1 * PI; + yaw_mouse = dbus->mouse.x / 32767.0 / 20 * PI; yaw_remote = dbus->ch2 / 660.0 / 210.0 / 5 * PI; // sum whole yaw data yaw_sum = yaw_mouse + yaw_remote; + print("yaw_sum: %f\n", yaw_sum); // Data Processing and Update(yaw clip range need test) // pitch processing @@ -353,35 +366,39 @@ void shooterTask(void* arg) { // reload variables bool reload_pull = false; bool reload_push = false; - float reload_pos = 10 * PI; + // the 99.506 is the ratio of the reload servo and devide the 3508 ratio to reload one bullet + float reload_pos_weak = 2.75 * PI * 99.506 / M3508P19_RATIO; + float reload_pos_strong = 2.75 * PI * 99.506 / M3508P19_RATIO; + // load variable bool loading = false; // 99.506 is the ratio of the load servo and devide the 3508 ratio to load one bullet float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; + // force variable - bool force_week = false; + bool force_weak = true; // always start at the weak position bool force_strong = false; bool force_transforming = false; float force_pos = 0; - float force_angle = 10 * PI; + float force_angle = 55 * PI; int i= 0; // waiting for the start signal while (true) { if (dbus->keyboard.bit.B || dbus->swr == remote::DOWN) break; - osDelay(100); + osDelay(SHOOTER_OUTER_TASK_DELAY); } while (true) { // Dead - while (Dead || GimbalDead) osDelay(100); + while (Dead || GimbalDead) osDelay(SHOOTER_OUTER_TASK_DELAY); i = 0; if (Preload) { while (true) { // break condition (loading) - if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001 && !GimbalDead) break; // loading once if (i == 1) { Preload = false; @@ -389,7 +406,7 @@ void shooterTask(void* arg) { } load_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_loader, 1); - osDelay(2); + osDelay(SHOOTER_INNER_TASK_DELAY); } // after loading i = 0; @@ -399,45 +416,45 @@ void shooterTask(void* arg) { } // whether change the force motor position - ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel > remote::WheelDigitalValue); + ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel < 1024); ForceStrongDetect.input(dbus->keyboard.bit.C || (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue)); // just execute force transform once - if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) { + if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_weak) { force_transforming = true; - force_week = true; + force_weak = true; force_strong = false; } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) { force_transforming = true; - force_week = false; + force_weak = false; force_strong = true; } // force transforming if (force_transforming) { while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001 && !GimbalDead) break; // set the speed and acceleration for the reload motor // set target pull position once if (i == 1) { // direction need test - if (force_week && !force_strong) { + if (force_weak && !force_strong) { force_pos += force_angle; - } else if (force_strong && !force_week) { + } else if (force_strong && !force_weak) { force_pos -= force_angle; } force_servo->SetTarget(force_pos); } force_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_force, 1); - osDelay(2); + osDelay(SHOOTER_INNER_TASK_DELAY); } // after changing the force motor position i = 0; force_transforming = false; force_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_force, 1); - osDelay(100); // need test the delay time(wait for the) + osDelay(SHOOTER_OUTER_TASK_DELAY); // need test the delay time(wait for the) } // the shoot and load process is automatic @@ -451,36 +468,40 @@ void shooterTask(void* arg) { LoadDetect.input(dbus->swr == remote::UP || dbus->mouse.l); if (LoadDetect.posEdge()) { // step 1 - trigger->SetOutPutAngle(20); - osDelay(1000); // wait for the bullet out + trigger->SetOutPutAngle(0); + osDelay(SHOOTER_OUTER_TASK_DELAY); // wait for the bullet out // step 2 while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 30 && abs(reload_servo->GetOmega()) <= 0.001 && !GimbalDead) break; // set the speed and acceleration for the reload motor // set target pull position once if (!reload_pull) { reload_pull = true; - reload_servo->SetTarget(reload_pos); + if (force_weak) { + reload_servo->SetTarget(reload_pos_weak); + } else if (force_strong) { + reload_servo->SetTarget(reload_pos_strong); + } } - print("reload theta: %f\n", reload_servo->GetTheta()); reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); - osDelay(2); + osDelay(SHOOTER_INNER_TASK_DELAY); } + // step 3 (before release the reload board) + trigger->SetOutPutAngle(-80); // need test the trigger angle + osDelay(1700); // need test the delay time(wait for the) // after reload pulling i = 0; reload_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; - osDelay(100); // need test the delay time(wait for the) - // step 3 - trigger->SetOutPutAngle(-80); - osDelay(100); // need test the delay time(wait for the) + osDelay(SHOOTER_OUTER_TASK_DELAY); // need test the delay time(wait for the) + // step 4 while (true) { // break condition (loading) - if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001) break; + if (++i > 30 && abs(load_servo->GetOmega()) <= 0.001 && !GimbalDead) break; // loading once if (!loading) { loading = true; @@ -488,18 +509,18 @@ void shooterTask(void* arg) { } load_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_loader, 1); - osDelay(2); + osDelay(SHOOTER_INNER_TASK_DELAY); } // after loading i = 0; load_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_loader, 1); loading = false; - osDelay(100); // need test the delay time + osDelay(SHOOTER_OUTER_TASK_DELAY); // need test the delay time // step 5 while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001) break; + if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001 && !GimbalDead) break; // set target push position once if (!reload_push) { reload_push = true; @@ -507,18 +528,18 @@ void shooterTask(void* arg) { } reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); - osDelay(2); + osDelay(SHOOTER_INNER_TASK_DELAY); } // after reload pushing i = 0; reload_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_push = false; - osDelay(100); + osDelay(SHOOTER_OUTER_TASK_DELAY); } dbus->previous_wheel_value = dbus->wheel.wheel; - osDelay(10); + osDelay(SHOOTER_TASK_DELAY); } } @@ -754,6 +775,7 @@ void KillAll() { void RM_RTOS_Default_Task(const void* args) { UNUSED(args); + control::MotorCANBase* motors_can2_shooter_part[] = {reload_motor, force_motor}; while (true) { // Emergency stop @@ -762,11 +784,24 @@ void RM_RTOS_Default_Task(const void* args) { Dead = true; KillAll(); } + // preloading PreloadDetect.input(dbus->swl == remote::UP); if (PreloadDetect.posEdge() && !Preload) { Preload = true; } + if (referee->game_robot_status.mains_power_shooter_output == 0 + || referee->game_robot_status.mains_power_gimbal_output == 0) { + GimbalDead = true; + pitch_motor->MotorDisable(); + reload_motor->SetOutput(0); + force_motor->SetOutput(0); + control::MotorCANBase::TransmitOutput(motors_can2_shooter_part, 2); + } else { + GimbalDead = false; + pitch_motor->MotorEnable(); + } + osDelay(DEFAULT_TASK_DELAY); } } From 36c3b1c89208f67403ccefc3881474c4bb8dc278 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Fri, 4 Aug 2023 06:16:48 +0800 Subject: [PATCH 10/15] just leave the reload motor parameter --- examples/hero/hero_shooter_2023.cc | 6 +++--- vehicles/Hero/main.cc | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index aa69a61a..239aefac 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -86,8 +86,8 @@ void RM_RTOS_Init() { servo_data.motor = reload_motor; servo_data.max_speed = 4 * PI; // params need test - servo_data.max_acceleration = 8 * PI; - servo_data.omega_pid_param = new float [3] {130, 15, 5}; // pid need test + servo_data.max_acceleration = 10 * PI; + servo_data.omega_pid_param = new float [3] {150, 15, 5}; // pid need test reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; @@ -107,7 +107,7 @@ void RM_RTOS_Default_Task(const void* args) { // reload variables bool reload_pull = false; bool reload_push = false; - float reload_pos = 3.7 * PI * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the reload servo, devide the 3508 ratio + float reload_pos = 3.24 * PI * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the reload servo, devide the 3508 ratio // load variable bool loading = false; float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index d922908b..f68eca39 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -310,14 +310,14 @@ void gimbalTask(void* arg) { // pitch processing pitch_pos = clip(pitch_pos + pitch_sum, -70 * PI, 70 * PI); // pitch update - pitch_motor->SetOutput(pitch_pos, pitch_sum); + pitch_motor->SetOutput(pitch_pos, 15); control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); // if not elevation, update the yaw position in the chassis if (Elevation) { // yaw processing (gear ratio 1 : 4) yaw_pos = clip(yaw_pos + yaw_sum, -PI / 3, PI / 3); // yaw update - yaw_motor->SetOutput(yaw_pos, 5, 20, 1, 0); + yaw_motor->SetOutput(yaw_pos, 10, 20, 1, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); } @@ -696,7 +696,7 @@ void RM_RTOS_Init() { servo_data.motor = reload_motor; servo_data.max_speed = 4 * PI; // params need test servo_data.max_acceleration = 10 * PI; - servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; + servo_data.omega_pid_param = new float [3] {130, 15, 5}; reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; From 4ec437b9fdb423ac2e8bc2f4eb227f5f651140f5 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Fri, 4 Aug 2023 16:41:01 +0800 Subject: [PATCH 11/15] the data for the weak one is ready --- vehicles/Hero/main.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index f68eca39..e610e8d5 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -310,7 +310,7 @@ void gimbalTask(void* arg) { // pitch processing pitch_pos = clip(pitch_pos + pitch_sum, -70 * PI, 70 * PI); // pitch update - pitch_motor->SetOutput(pitch_pos, 15); + pitch_motor->SetOutput(pitch_pos, pitch_sum); control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); // if not elevation, update the yaw position in the chassis if (Elevation) { @@ -367,7 +367,7 @@ void shooterTask(void* arg) { bool reload_pull = false; bool reload_push = false; // the 99.506 is the ratio of the reload servo and devide the 3508 ratio to reload one bullet - float reload_pos_weak = 2.75 * PI * 99.506 / M3508P19_RATIO; + float reload_pos_weak = 3.57 * PI * 99.506 / M3508P19_RATIO; float reload_pos_strong = 2.75 * PI * 99.506 / M3508P19_RATIO; // load variable @@ -473,7 +473,7 @@ void shooterTask(void* arg) { // step 2 while (true) { // break condition (reach the desire position) - if (++i > 30 && abs(reload_servo->GetOmega()) <= 0.001 && !GimbalDead) break; + if (++i > 30 && abs(reload_servo->GetOmega()) <= 0.00005 && !GimbalDead) break; // set the speed and acceleration for the reload motor // set target pull position once if (!reload_pull) { @@ -496,12 +496,12 @@ void shooterTask(void* arg) { reload_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; - osDelay(SHOOTER_OUTER_TASK_DELAY); // need test the delay time(wait for the) + osDelay(200); // need test the delay time(wait for the) // step 4 while (true) { // break condition (loading) - if (++i > 30 && abs(load_servo->GetOmega()) <= 0.001 && !GimbalDead) break; + if (++i > 30 && abs(load_servo->GetOmega()) <= 0.0001 && !GimbalDead) break; // loading once if (!loading) { loading = true; @@ -520,7 +520,7 @@ void shooterTask(void* arg) { // step 5 while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.001 && !GimbalDead) break; + if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.0001 && !GimbalDead) break; // set target push position once if (!reload_push) { reload_push = true; From c09d2fb1522402dab7ff4da38ddecd785937c5c0 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Sat, 5 Aug 2023 13:45:37 +0800 Subject: [PATCH 12/15] pass the test in the match --- vehicles/Hero/main.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index e610e8d5..fa383818 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -308,7 +308,7 @@ void gimbalTask(void* arg) { // Data Processing and Update(yaw clip range need test) // pitch processing - pitch_pos = clip(pitch_pos + pitch_sum, -70 * PI, 70 * PI); + pitch_pos = clip(pitch_pos + pitch_sum, -60 * PI, 0); // pitch update pitch_motor->SetOutput(pitch_pos, pitch_sum); control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); @@ -317,7 +317,7 @@ void gimbalTask(void* arg) { // yaw processing (gear ratio 1 : 4) yaw_pos = clip(yaw_pos + yaw_sum, -PI / 3, PI / 3); // yaw update - yaw_motor->SetOutput(yaw_pos, 10, 20, 1, 0); + yaw_motor->SetOutput(yaw_pos, 8, 20, 1, 0); control::Motor4310::TransmitOutput(motors_can1_gimbal, 1); } @@ -367,7 +367,7 @@ void shooterTask(void* arg) { bool reload_pull = false; bool reload_push = false; // the 99.506 is the ratio of the reload servo and devide the 3508 ratio to reload one bullet - float reload_pos_weak = 3.57 * PI * 99.506 / M3508P19_RATIO; + float reload_pos_weak = 3.53 * PI * 99.506 / M3508P19_RATIO; float reload_pos_strong = 2.75 * PI * 99.506 / M3508P19_RATIO; // load variable @@ -395,7 +395,7 @@ void shooterTask(void* arg) { while (Dead || GimbalDead) osDelay(SHOOTER_OUTER_TASK_DELAY); i = 0; - if (Preload) { + if (Preload && !GimbalDead) { while (true) { // break condition (loading) if (++i > 10 && abs(load_servo->GetOmega()) <= 0.001 && !GimbalDead) break; @@ -430,7 +430,7 @@ void shooterTask(void* arg) { force_strong = true; } // force transforming - if (force_transforming) { + if (force_transforming && !GimbalDead) { while (true) { // break condition (reach the desire position) if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001 && !GimbalDead) break; @@ -466,7 +466,7 @@ void shooterTask(void* arg) { // Load Detector LoadDetect.input(dbus->swr == remote::UP || dbus->mouse.l); - if (LoadDetect.posEdge()) { + if (LoadDetect.posEdge() && !GimbalDead) { // step 1 trigger->SetOutPutAngle(0); osDelay(SHOOTER_OUTER_TASK_DELAY); // wait for the bullet out @@ -790,12 +790,12 @@ void RM_RTOS_Default_Task(const void* args) { Preload = true; } - if (referee->game_robot_status.mains_power_shooter_output == 0 - || referee->game_robot_status.mains_power_gimbal_output == 0) { + if (referee->game_robot_status.mains_power_shooter_output == 0) { GimbalDead = true; pitch_motor->MotorDisable(); reload_motor->SetOutput(0); force_motor->SetOutput(0); + load_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(motors_can2_shooter_part, 2); } else { GimbalDead = false; From ebc45c857a07ca7d7fda3bfe5a7a63d894297e3f Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Sun, 6 Aug 2023 07:19:24 +0800 Subject: [PATCH 13/15] add two shoot mode --- vehicles/Hero/main.cc | 52 +++++++++++++++++++++++++++++-------------- 1 file changed, 35 insertions(+), 17 deletions(-) diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index fa383818..121c2a94 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -66,6 +66,8 @@ static const int SELFTEST_TASK_DELAY = 100; static volatile float yaw_sum = 0; static control::Motor4310* yaw_motor = nullptr; +static volatile bool safe_mode = true; + //================================================================================================== // Referee //================================================================================================== @@ -304,11 +306,14 @@ void gimbalTask(void* arg) { yaw_remote = dbus->ch2 / 660.0 / 210.0 / 5 * PI; // sum whole yaw data yaw_sum = yaw_mouse + yaw_remote; - print("yaw_sum: %f\n", yaw_sum); // Data Processing and Update(yaw clip range need test) // pitch processing - pitch_pos = clip(pitch_pos + pitch_sum, -60 * PI, 0); + if (dbus->swl == remote::DOWN) { + pitch_pos = clip(pitch_pos + pitch_sum, -60 * PI, 60 * PI); + } else { + pitch_pos = clip(pitch_pos + pitch_sum, -25 * PI, 0); + } // pitch update pitch_motor->SetOutput(pitch_pos, pitch_sum); control::Motor4310::TransmitOutput(motors_can2_gimbal, 1); @@ -355,6 +360,8 @@ static control::ServoMotor* force_servo = nullptr; static BoolEdgeDetector ForceWeakDetect(false); static BoolEdgeDetector ForceStrongDetect(false); +static float force_angle = 65 * PI; + void shooterTask(void* arg) { UNUSED(arg); // motors initialization @@ -367,8 +374,8 @@ void shooterTask(void* arg) { bool reload_pull = false; bool reload_push = false; // the 99.506 is the ratio of the reload servo and devide the 3508 ratio to reload one bullet - float reload_pos_weak = 3.53 * PI * 99.506 / M3508P19_RATIO; - float reload_pos_strong = 2.75 * PI * 99.506 / M3508P19_RATIO; + float reload_pos_weak = 3.78 * PI * 99.506 / M3508P19_RATIO; + float reload_pos_strong = 4.7 * PI * 99.506 / M3508P19_RATIO; // load variable bool loading = false; @@ -380,7 +387,6 @@ void shooterTask(void* arg) { bool force_strong = false; bool force_transforming = false; float force_pos = 0; - float force_angle = 55 * PI; int i= 0; @@ -390,6 +396,8 @@ void shooterTask(void* arg) { osDelay(SHOOTER_OUTER_TASK_DELAY); } + safe_mode = false; + while (true) { // Dead while (Dead || GimbalDead) osDelay(SHOOTER_OUTER_TASK_DELAY); @@ -415,6 +423,10 @@ void shooterTask(void* arg) { osDelay(100); // need test the delay time } + if (dbus->swl == remote::DOWN) { + force_weak = false; + } + // whether change the force motor position ForceWeakDetect.input(dbus->keyboard.bit.F || dbus->wheel.wheel < 1024); ForceStrongDetect.input(dbus->keyboard.bit.C @@ -433,7 +445,7 @@ void shooterTask(void* arg) { if (force_transforming && !GimbalDead) { while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(force_motor->GetOmega()) <= 0.001 && !GimbalDead) break; + if (++i > 30 && abs(force_motor->GetOmega()) <= 0.001 && !GimbalDead) break; // set the speed and acceleration for the reload motor // set target pull position once if (i == 1) { @@ -467,6 +479,7 @@ void shooterTask(void* arg) { // Load Detector LoadDetect.input(dbus->swr == remote::UP || dbus->mouse.l); if (LoadDetect.posEdge() && !GimbalDead) { + osDelay(100); // step 1 trigger->SetOutPutAngle(0); osDelay(SHOOTER_OUTER_TASK_DELAY); // wait for the bullet out @@ -478,6 +491,8 @@ void shooterTask(void* arg) { // set target pull position once if (!reload_pull) { reload_pull = true; + reload_servo->SetMaxSpeed(10 * PI); + reload_servo->SetMaxAcceleration(25 * PI); if (force_weak) { reload_servo->SetTarget(reload_pos_weak); } else if (force_strong) { @@ -490,13 +505,13 @@ void shooterTask(void* arg) { } // step 3 (before release the reload board) trigger->SetOutPutAngle(-80); // need test the trigger angle - osDelay(1700); // need test the delay time(wait for the) + osDelay(1500); // need test the delay time(wait for the) // after reload pulling i = 0; reload_motor->SetOutput(0); control::MotorCANBase::TransmitOutput(can2_reloader, 1); reload_pull = false; - osDelay(200); // need test the delay time(wait for the) + osDelay(100); // need test the delay time(wait for the) // step 4 while (true) { @@ -520,11 +535,13 @@ void shooterTask(void* arg) { // step 5 while (true) { // break condition (reach the desire position) - if (++i > 10 && abs(reload_servo->GetOmega()) <= 0.0001 && !GimbalDead) break; + if (++i > 20 && abs(reload_servo->GetOmega()) <= 0.0001 && !GimbalDead) break; // set target push position once if (!reload_push) { reload_push = true; reload_servo->SetTarget(0, true); + reload_servo->SetMaxSpeed(50 * PI); + reload_servo->SetMaxAcceleration(100 * PI); } reload_servo->CalcOutput(); control::MotorCANBase::TransmitOutput(can2_reloader, 1); @@ -685,8 +702,8 @@ void RM_RTOS_Init() { // Servo control for each shooter motor control::servo_t servo_data; servo_data.motor = load_motor; - servo_data.max_speed = 4 * PI; // params need test - servo_data.max_acceleration = 10 * PI; + servo_data.max_speed = 3 * PI; // params need test + servo_data.max_acceleration = 8 * PI; servo_data.transmission_ratio = M3508P19_RATIO; servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; servo_data.max_iout = 1000; @@ -694,14 +711,14 @@ void RM_RTOS_Init() { load_servo = new control::ServoMotor(servo_data); servo_data.motor = reload_motor; - servo_data.max_speed = 4 * PI; // params need test - servo_data.max_acceleration = 10 * PI; + servo_data.max_speed = 15 * PI; // params need test + servo_data.max_acceleration = 25 * PI; servo_data.omega_pid_param = new float [3] {130, 15, 5}; reload_servo = new control::ServoMotor(servo_data); servo_data.motor = force_motor; - servo_data.max_speed = 4 * PI; // params need test - servo_data.max_acceleration = 10 * PI; + servo_data.max_speed = 100 * PI; + servo_data.max_acceleration = 100 * PI; servo_data.omega_pid_param = new float [3] {150, 1.2, 5}; force_servo = new control::ServoMotor(servo_data); @@ -740,7 +757,7 @@ void KillAll() { while (true) { // Restart after emergency stop - FakeDeath.input(dbus->swl == remote::DOWN); + FakeDeath.input(dbus->swr == remote::DOWN && safe_mode == false); if (FakeDeath.posEdge()) { Dead = false; RGB->Display(display::color_green); @@ -776,10 +793,11 @@ void KillAll() { void RM_RTOS_Default_Task(const void* args) { UNUSED(args); control::MotorCANBase* motors_can2_shooter_part[] = {reload_motor, force_motor}; + safe_mode = true; while (true) { // Emergency stop - FakeDeath.input(dbus->swl == remote::DOWN); + FakeDeath.input(dbus->swr == remote::DOWN && safe_mode == false); if (FakeDeath.posEdge()) { Dead = true; KillAll(); From 7fb02a330a3519fe8a685d76765a215505e32d2c Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Sat, 30 Sep 2023 16:17:47 -0500 Subject: [PATCH 14/15] the lock for the safe mode --- vehicles/Hero/main.cc | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/vehicles/Hero/main.cc b/vehicles/Hero/main.cc index 121c2a94..f892f1f8 100644 --- a/vehicles/Hero/main.cc +++ b/vehicles/Hero/main.cc @@ -281,7 +281,7 @@ void gimbalTask(void* arg) { float pitch_pos = 0, yaw_pos = 0; while (true) { // Dead - while (Dead || GimbalDead) osDelay(100); + while (Dead) osDelay(100); // Data Collection // pitch data from keyboard @@ -300,6 +300,9 @@ void gimbalTask(void* arg) { pitch_remote = -dbus->ch3 / 660.0 * 30 * PI; // sum whole pitch data pitch_sum = pitch_keyboard + pitch_mouse + pitch_remote; + if (GimbalDead) { + pitch_sum = 0; + } // yaw data from mouse(direction and offset need test) yaw_mouse = dbus->mouse.x / 32767.0 / 20 * PI; @@ -792,7 +795,7 @@ void KillAll() { void RM_RTOS_Default_Task(const void* args) { UNUSED(args); - control::MotorCANBase* motors_can2_shooter_part[] = {reload_motor, force_motor}; + control::MotorCANBase* motors_can2_shooter_part[] = {reload_motor, force_motor, load_motor}; safe_mode = true; while (true) { @@ -814,7 +817,7 @@ void RM_RTOS_Default_Task(const void* args) { reload_motor->SetOutput(0); force_motor->SetOutput(0); load_motor->SetOutput(0); - control::MotorCANBase::TransmitOutput(motors_can2_shooter_part, 2); + control::MotorCANBase::TransmitOutput(motors_can2_shooter_part, 3); } else { GimbalDead = false; pitch_motor->MotorEnable(); From ad9972b3b61f0dc4d2a811e508d675b95cce0ba3 Mon Sep 17 00:00:00 2001 From: Gabriel Gao Date: Sat, 30 Mar 2024 16:22:53 -0500 Subject: [PATCH 15/15] fix the problem in pr --- examples/hero/hero_shooter_2023.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/hero/hero_shooter_2023.cc b/examples/hero/hero_shooter_2023.cc index 239aefac..1c65189f 100644 --- a/examples/hero/hero_shooter_2023.cc +++ b/examples/hero/hero_shooter_2023.cc @@ -113,7 +113,7 @@ void RM_RTOS_Default_Task(const void* args) { float load_angle = 2 * PI / 6 * 99.506 / M3508P19_RATIO; // 99.506 is the ratio of the load servo, devide the 3508 ratio int i = 0; // force variable - bool force_week = false; + bool force_weak = false; bool force_strong = false; bool force_transforming = false; float force_pos = 0; @@ -132,15 +132,15 @@ void RM_RTOS_Default_Task(const void* args) { || (dbus->wheel.wheel == remote::WheelDigitalValue && dbus->previous_wheel_value == remote::WheelDigitalValue)); // just execute force transform once - if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_week) { - print("week"); + if (ForceWeakDetect.posEdge() && !ForceStrongDetect.posEdge() && !force_weak) { + print("weak"); force_transforming = true; - force_week = true; + force_weak = true; force_strong = false; } else if (ForceStrongDetect.posEdge() && !ForceWeakDetect.posEdge() && !force_strong) { print("strong"); force_transforming = true; - force_week = false; + force_weak = false; force_strong = true; } // force transforming @@ -152,9 +152,9 @@ void RM_RTOS_Default_Task(const void* args) { // set target pull position once if (i == 1) { // direction need test - if (force_week && !force_strong) { + if (force_weak && !force_strong) { force_pos += force_angle; - } else if (force_strong && !force_week) { + } else if (force_strong && !force_weak) { force_pos -= force_angle; } force_servo->SetTarget(force_pos);