diff --git a/src/device/custom_controller/dev_custom_controller.cpp b/src/device/custom_controller/dev_custom_controller.cpp index 3660c9f5..4fa12888 100644 --- a/src/device/custom_controller/dev_custom_controller.cpp +++ b/src/device/custom_controller/dev_custom_controller.cpp @@ -8,8 +8,8 @@ using namespace Device; static std::array rxbuf; -CustomController::CustomController() - : event_(Message::Event::FindEvent("cmd_event")) { +CustomController::CustomController(Param ¶m) + : param_(param), event_(Message::Event::FindEvent("cmd_event")) { auto rx_cplt_callback = [](void *arg) { CustomController *cust_ctrl = static_cast(arg); cust_ctrl->packet_recv_.Post(); @@ -17,12 +17,11 @@ CustomController::CustomController() bsp_uart_register_callback(BSP_UART_EXT, BSP_UART_RX_CPLT_CB, rx_cplt_callback, this); - Component::CMD::RegisterController(this->controller_angel_); auto controller_recv_thread = [](CustomController *cust_ctrl) { while (1) { - if (cust_ctrl->packet_recv_.Wait(20)) { + bsp_uart_receive(BSP_UART_EXT, &rxbuf[0], sizeof(rxbuf), false); + if (cust_ctrl->packet_recv_.Wait(500)) { cust_ctrl->Prase(); - cust_ctrl->controller_angel_.Publish(cust_ctrl->controller_data_); } else { cust_ctrl->Offline(); } @@ -34,15 +33,37 @@ CustomController::CustomController() } bool CustomController::StartRecv() { - return bsp_uart_receive(BSP_UART_EXT, rxbuf, REF_LEN_RX_BUFF, false) == + return bsp_uart_receive(BSP_UART_EXT, &rxbuf[0], REF_LEN_RX_BUFF, false) == BSP_OK; } void CustomController::Prase() { - this->controller_angel_.Publish(this->controller_data_); + auto len = bsp_uart_get_count(BSP_UART_EXT); + uint8_t *index = &rxbuf[0]; + + for (uint32_t i = 0; i < len; i++) { + if (index[i] == 0xa5 && index[i + 5] == 0x02 && index[i + 6] == 0x03 && + len - i > 37) { + online_ = true; + + memcpy(&data_, index + i, sizeof(data_)); + for (int i = 0; i < 6; i++) { + if (data_.angle[0] != 0) { + data_.angle[i] += param_.offset[i]; + } else { + online_ = false; + } + } + i += 37; + last_online_time_ = bsp_time_get_ms(); + } else { + continue; + } + } + + if (bsp_time_get_ms() - last_online_time_ > 500) { + online_ = false; + } } -void CustomController::Offline() { - this->controller_data_.online = false; - memset(&(this->controller_data_), 0, sizeof(this->controller_data_)); - this->controller_angel_.Publish(this->controller_data_); -}; + +void CustomController::Offline() { online_ = false; } diff --git a/src/device/custom_controller/dev_custom_controller.hpp b/src/device/custom_controller/dev_custom_controller.hpp index e12eb453..64cf4c27 100644 --- a/src/device/custom_controller/dev_custom_controller.hpp +++ b/src/device/custom_controller/dev_custom_controller.hpp @@ -1,28 +1,50 @@ +#include "comp_cmd.hpp" #include "comp_ui.hpp" #include "device.hpp" namespace Device { class CustomController { public: - CustomController(); + typedef struct { + float offset[6]; + } Param; + + CustomController(Param& param); + + typedef struct __attribute__((packed)) { + struct __attribute__((packed)) { + uint8_t sof; // 起始字节，固定值为0xA5 + uint16_t data_length; // 数据帧中data的长度 + uint8_t seq; // 包序号 + uint8_t crc8; // 帧头CRC8校验 + } frame_header; // 帧头 + uint16_t cmd_id; // 命令码 + union __attribute__((packed)) { + float angle[6]; // 自定义控制器的数据帧 + uint8_t raw_data[30]; + }; + } Data; // 自定义控制器数据包 typedef enum { NUM = 144 } ControllerEvent; + Param param_; + + bool online_ = false; + + uint32_t last_online_time_ = 0; + bool StartRecv(); void Prase(); void Offline(); - private: - System::Semaphore packet_recv_ = System::Semaphore(false); + Data data_; - Message::Topic controller_angel_ = - Message::Topic("collectangle"); + System::Semaphore packet_recv_ = System::Semaphore(false); Message::Event event_; System::Thread recv_thread_; System::Thread trans_thread_; - Component::CMD::Data controller_data_{}; }; } // namespace Device diff --git a/src/module/custom_controller/mod_custom_controller.cpp b/src/module/custom_controller/mod_custom_controller.cpp index 60243af6..344bdd42 100644 --- a/src/module/custom_controller/mod_custom_controller.cpp +++ b/src/module/custom_controller/mod_custom_controller.cpp @@ -3,3 +3,25 @@ #include "bsp_uart.h" using namespace Module; + +Controller txdata; +void CustomController::DataConcatenation(uint8_t *data, uint16_t data_lenth) { + static uint8_t seq = 0; + /// 帧头数据 + txdata.frame_header.sof = 0xA5; // 数据帧起始字节，固定值为 0xA5 + txdata.frame_header.data_length = 30; // 数据帧中数据段的长度 + txdata.frame_header.seq = seq; // 包序号 + txdata.frame_header.crc8 = Component::CRC8::Calculate( + reinterpret_cast(&txdata.frame_header), + FRAME_HEADER_LENGTH - sizeof(uint8_t), + CRC8_INIT); // 添加帧头 CRC8 校验位 + /// 命令码ID + txdata.cmd_id = 0x0302; + /// 数据段 + memcpy(txdata.data, data, data_lenth); + /// 帧尾CRC16，整包校验 + txdata.frame_tail = Component::CRC16::Calculate( + reinterpret_cast(&txdata), + DATA_FRAME_LENGTH - sizeof(uint16_t), CRC16_INIT); +} +using namespace Module; diff --git a/src/module/custom_controller/mod_custom_controller.hpp b/src/module/custom_controller/mod_custom_controller.hpp index 905251e8..9120241f 100644 --- a/src/module/custom_controller/mod_custom_controller.hpp +++ b/src/module/custom_controller/mod_custom_controller.hpp @@ -1,8 +1,31 @@ #include "bsp_uart.h" +#include "comp_crc16.hpp" +#include "comp_crc8.hpp" #include "dev_mt6701.hpp" #include "module.hpp" -static char uart_tx_buff[1024] = {0}; +#define FRAME_HEADER_LENGTH 5 // 帧头数据长度 +#define CMD_ID_LENGTH 2 // 命令码ID数据长度 +#define DATA_LENGTH 30 // 数据段长度 +#define FRAME_TAIL_LENGTH 2 // 帧尾数据长度 + +#define DATA_FRAME_LENGTH \ + (FRAME_HEADER_LENGTH + CMD_ID_LENGTH + DATA_LENGTH + \ + FRAME_TAIL_LENGTH) // 整个数据帧的长度 + +#define CONTROLLER_CMD_ID 0x0302 // 自定义控制器命令码 + +typedef struct __attribute__((packed)) { + struct __attribute__((packed)) { + uint8_t sof; // 起始字节，固定值为0xA5 + uint16_t data_length; // 数据帧中data的长度 + uint8_t seq; // 包序号 + uint8_t crc8; // 帧头CRC8校验 + } frame_header; // 帧头 + __packed uint16_t cmd_id; // 命令码 + __packed uint8_t data[30]; // 自定义控制器的数据帧 + __packed uint16_t frame_tail; // 帧尾CRC16校验 +} Controller; // 自定义控制器数据包 namespace Module { class CustomController { @@ -10,6 +33,7 @@ class CustomController { typedef struct { Device::MT6701::Param mt6701[6]; } Param; + CustomController(Param& param) : mt6701_1(param.mt6701[0]), mt6701_2(param.mt6701[1]), @@ -18,24 +42,31 @@ class CustomController { mt6701_5(param.mt6701[4]), mt6701_6(param.mt6701[5]) { auto task_fun = [](CustomController* ctrl) { - (void)snprintf( - uart_tx_buff, sizeof(uart_tx_buff), "%f,%f,%f,%f,%f,%f\n", - ctrl->mt6701_1.angle_.Value(), ctrl->mt6701_2.angle_.Value(), - ctrl->mt6701_3.angle_.Value(), ctrl->mt6701_4.angle_.Value(), - ctrl->mt6701_5.angle_.Value(), ctrl->mt6701_6.angle_.Value()); - - bsp_uart_transmit(BSP_UART_MCU, reinterpret_cast(uart_tx_buff), - strlen(uart_tx_buff), false); + ctrl->txbuff[0] = ctrl->mt6701_1.angle_.Value(); + ctrl->txbuff[1] = ctrl->mt6701_2.angle_.Value(); + ctrl->txbuff[2] = ctrl->mt6701_3.angle_.Value(); + ctrl->txbuff[3] = ctrl->mt6701_4.angle_.Value(); + ctrl->txbuff[4] = ctrl->mt6701_5.angle_.Value(); + ctrl->txbuff[5] = ctrl->mt6701_6.angle_.Value(); + ctrl->DataConcatenation(reinterpret_cast(ctrl->txbuff), + sizeof(txbuff)); + bsp_uart_transmit(BSP_UART_MCU, reinterpret_cast(&ctrl->txdata), + DATA_FRAME_LENGTH, false); }; - System::Timer::Create(task_fun, this, 10); + System::Timer::Create(task_fun, this, 40); } + void DataConcatenation(uint8_t* data, uint16_t data_lenth); + Device::MT6701 mt6701_1; Device::MT6701 mt6701_2; Device::MT6701 mt6701_3; Device::MT6701 mt6701_4; Device::MT6701 mt6701_5; Device::MT6701 mt6701_6; + + float txbuff[6] = {0}; + Controller txdata; }; } // namespace Module diff --git a/src/module/dart_launcher/mod_dart_launcher.cpp b/src/module/dart_launcher/mod_dart_launcher.cpp index 38498469..ba42e39d 100644 --- a/src/module/dart_launcher/mod_dart_launcher.cpp +++ b/src/module/dart_launcher/mod_dart_launcher.cpp @@ -3,15 +3,15 @@ #include "bsp_time.h" #define MAX_FRIC_SPEED (7500.0f) +#define DART_LEN 0.225 using namespace Module; DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) - : param_(param), - rod_actr_(param.rod_actr, control_freq, 500.0f), - reload_actr_(param.reload_actr, control_freq, 500.0f) { - this->setpoint_.rod = 0.1; - this->setpoint_.reload = 0.0f; + : ctrl_lock_(true), + param_(param), + rod_actr_(param.rod_actr, control_freq, 500.0f) { + this->setpoint_.rod_position = 0.1f; for (int i = 0; i < 4; i++) { fric_actr_[i] = @@ -21,43 +21,49 @@ DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) (std::string("dart_fric_") + std::to_string(i)).c_str()); } - auto event_callback = [](Event event, DartLauncher* dart) { + auto event_callback = [](DartEvent event, DartLauncher* dart) { + dart->ctrl_lock_.Wait(UINT32_MAX); switch (event) { - case RELOAD: - if (bsp_time_get_ms() - dart->last_reload_time_ < 500) { - return; - } else { - dart->last_reload_time_ = bsp_time_get_ms(); - dart->setpoint_.reload += 0.013; - clampf(&dart->setpoint_.reload, 0, 1.0); - } - dart->setpoint_.fric_speed = 0; + case RELAX: + dart->SetFricMode(FRIC_OFF); break; - case RESET: - dart->setpoint_.reload = 0; - dart->setpoint_.fric_speed = 0; + case SET_FRIC_OUTOST: + dart->SetFricMode(FRIC_OUTOST); break; - case ON: - dart->setpoint_.rod = 1.0f; - dart->setpoint_.fric_speed = 1.0f; + case SET_FRIC_BASE: + dart->SetFricMode(FRIC_BASE); break; - case OFF: - dart->setpoint_.fric_speed = 0; - dart->setpoint_.rod = 0.0f; + case SET_FRIC_OFF: + dart->SetFricMode(FRIC_OFF); + break; + case SET_STAY: + dart->SetRodMode(STAY); + break; + case RESET_POSITION: + dart->SetRodMode(BACK); + break; + case FIRE: + dart->SetRodMode(ADVANCE); + default: break; } + dart->ctrl_lock_.Post(); }; - Component::CMD::RegisterEvent(event_callback, this, - this->param_.EVENT_MAP); + Component::CMD::RegisterEvent( + event_callback, this, this->param_.EVENT_MAP); auto thread_fn = [](DartLauncher* dart) { uint32_t last_online_time = bsp_time_get_ms(); while (true) { + dart->ctrl_lock_.Wait(UINT32_MAX); + dart->UpdateFeedback(); dart->Control(); + dart->ctrl_lock_.Post(); + dart->thread_.SleepUntil(2, last_online_time); } }; @@ -68,9 +74,8 @@ DartLauncher::DartLauncher(DartLauncher::Param& param, float control_freq) void DartLauncher::UpdateFeedback() { this->rod_actr_.UpdateFeedback(); - this->reload_actr_.UpdateFeedback(); - for (auto motor : fric_motor_) { - motor->Update(); + for (size_t i = 0; i < 4; i++) { + this->fric_motor_[i]->Update(); } } @@ -81,19 +86,65 @@ void DartLauncher::Control() { this->last_wakeup_ = this->now_; - this->rod_actr_.Control(this->setpoint_.rod * this->param_.rod_actr.max_range, - dt_); - - this->reload_actr_.Control( - this->setpoint_.reload * this->param_.reload_actr.max_range, dt_); + switch (this->fric_mode_) { + case FRIC_OUTOST: + this->setpoint_.fric_speed = 0.933f; + break; + case FRIC_BASE: + this->setpoint_.fric_speed = 1.0f; + break; + case FRIC_OFF: + this->setpoint_.fric_speed = 0.0f; + break; + } + switch (this->rod_mode_) { + case ADVANCE: + if (this->fric_mode_ == FRIC_OUTOST || + this->fric_mode_ == + FRIC_BASE) { /*只有在摩擦轮开启状态下飞镖被向前推进*/ + this->setpoint_.rod_position = + (static_cast(rod_position_)) * DART_LEN + 0.1; + } + break; + case STAY: + break; + case BACK: + this->setpoint_.rod_position = 0.1f; + if (rod_position_ == 4) { + rod_position_ = 0; + } + } + /* 上电自动校准 和控制电机输出 */ + this->rod_actr_.Control( + this->setpoint_.rod_position * this->param_.rod_actr.max_range, dt_); + /* fric */ for (int i = 0; i < 4; i++) { motor_out_[i] = this->fric_actr_[i]->Calculate( - this->setpoint_.fric_speed, - this->fric_motor_[i]->GetSpeed() / MAX_FRIC_SPEED, dt_); + this->setpoint_.fric_speed * MAX_FRIC_SPEED, + this->fric_motor_[i]->GetSpeed(), dt_); } - for (int i = 0; i < 4; i++) { this->fric_motor_[i]->Control(motor_out_[i]); } } +/*判断是否切换模式*/ +void DartLauncher::SetRodMode(RodMode mode) { + if (mode == this->rod_mode_) { /* 未更改，return */ + return; + } else { + this->rod_mode_ = mode; + /*如果切换到前进模式，位置向前推进*/ + if (mode == ADVANCE && + (this->fric_mode_ == FRIC_OUTOST || this->fric_mode_ == FRIC_BASE)) { + rod_position_ = ((rod_position_ + 1) % 5); + } + } +} + +void DartLauncher::SetFricMode(FricMode mode) { + if (mode == this->fric_mode_) { /* 未更改，return */ + return; + } + this->fric_mode_ = mode; +} diff --git a/src/module/dart_launcher/mod_dart_launcher.hpp b/src/module/dart_launcher/mod_dart_launcher.hpp index dd150fd4..ad6063a1 100644 --- a/src/module/dart_launcher/mod_dart_launcher.hpp +++ b/src/module/dart_launcher/mod_dart_launcher.hpp @@ -12,8 +12,6 @@ class DartLauncher { typedef struct { Device::AutoCaliLimitedMech::Param rod_actr; - Device::AutoCaliLimitedMech::Param reload_actr; const std::vector EVENT_MAP; std::array fric_actr; @@ -21,27 +19,45 @@ class DartLauncher { } Param; typedef struct { - float rod; - float reload; + float rod_position; float fric_speed; } Setpoint; - typedef enum { - RELOAD, - RESET, - ON, - OFF, - } Event; + FRIC_OUTOST, /*摩擦轮前哨战模式*/ + FRIC_BASE, /*摩擦轮基地模式*/ + FRIC_OFF, + } FricMode; + typedef enum { + ADVANCE, /* 推进1个单位 */ + STAY, + BACK, /* 回到初始位置0,1 */ + } RodMode; + typedef enum { + RELAX, /* 摩擦轮不输出，回到初始位置0.1 */ + SET_FRIC_OFF, + SET_STAY, + RESET_POSITION, /* 回到初始位置0.1 */ + FIRE, /* 发射 */ + SET_FRIC_OUTOST, /*攻击前哨站*/ + SET_FRIC_BASE, /*攻击基地*/ + } DartEvent; /* 事件 */ DartLauncher(Param& param, float control_freq); void UpdateFeedback(); void Control(); + void SetRodMode(RodMode mode); + void SetFricMode(FricMode mode); private: + System::Semaphore ctrl_lock_; + Param& param_; + FricMode fric_mode_; + RodMode rod_mode_; + Setpoint setpoint_; float dt_ = 0.0f; @@ -49,14 +65,10 @@ class DartLauncher { uint64_t last_wakeup_ = 0; uint64_t now_ = 0; - - uint32_t last_reload_time_ = 0; + int rod_position_ = 0; /* 丝杆位置0,1,2,3循环 */ Device::AutoCaliLimitedMech - rod_actr_; - Device::AutoCaliLimitedMech - reload_actr_; - + rod_actr_; /* 内含电机、执行器、防堵数据 */ std::array fric_actr_; std::array fric_motor_; diff --git a/src/module/launcher_drone/mod_launcher_drone.cpp b/src/module/launcher_drone/mod_launcher_drone.cpp index 62577585..15d20114 100644 --- a/src/module/launcher_drone/mod_launcher_drone.cpp +++ b/src/module/launcher_drone/mod_launcher_drone.cpp @@ -24,22 +24,22 @@ UVALauncher::UVALauncher(Param& param, float control_freq) case CHANGE_FIRE_MODE_SAFE: launcher->SetTrigMode(SAFE); - launcher->fire_ctrl_.fire = false; - launcher->fire_ctrl_.firc_on = false; + launcher->fire_ctrl_.fire = false; /*拨弹盘静止*/ + launcher->fire_ctrl_.fric_on = false; /*摩擦轮静止*/ break; case CHANGE_TRIG_MODE_SINGLE: - launcher->fire_ctrl_.fire = true; - launcher->fire_ctrl_.firc_on = true; + launcher->fire_ctrl_.fire = true; /*拨弹盘转动*/ + launcher->fire_ctrl_.fric_on = true; /*摩擦轮转动*/ launcher->SetTrigMode(SINGLE); break; case CHANGE_TRIG_MODE_BURST: - launcher->fire_ctrl_.fire = false; - launcher->fire_ctrl_.firc_on = false; + launcher->fire_ctrl_.fire = true; /*拨弹盘转动*/ + launcher->fire_ctrl_.fric_on = true; /*摩擦轮转动*/ launcher->SetTrigMode(BURST); break; @@ -97,7 +97,7 @@ void UVALauncher::Control() { this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio; this->now_ = bsp_time_get(); - this->dt_ = this->now_ - this->last_wakeup_; + this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_); /*注意单位是US*/ this->last_wakeup_ = this->now_; /* 根据开火模式计算发射行为 */ @@ -120,7 +120,7 @@ void UVALauncher::Control() { /* 计算是否是第一次按下开火键 */ this->fire_ctrl_.first_pressed_fire = this->fire_ctrl_.fire && !this->fire_ctrl_.last_fire; - this->fire_ctrl_.last_fire = this->fire_ctrl_.fire; + this->fire_ctrl_.last_fire = 0; /* 设置要发射多少弹丸 */ if (this->fire_ctrl_.first_pressed_fire && !this->fire_ctrl_.to_launch) { @@ -133,7 +133,6 @@ void UVALauncher::Control() { break; case SAFE: this->fire_ctrl_.launch_delay = UINT32_MAX; - break; default: break; @@ -172,13 +171,15 @@ void UVALauncher::PraseRef() { } void UVALauncher::FricControl() { if (/*this->raw_ref_.robot_status.power_launcher_output == 1 &&*/ - this->fire_ctrl_.firc_on == true) { + this->fire_ctrl_.fric_on == true) { + /*摩擦轮开启模式*/ bsp_pwm_start(BSP_PWM_SERVO_A); bsp_pwm_start(BSP_PWM_SERVO_B); - bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.09f); - bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.09f); + bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.08f); /*此处最高不得超过0.10f*/ + bsp_pwm_set_comp(BSP_PWM_SERVO_B, 0.08f); } else { + /*摩擦轮预热模式*/ bsp_pwm_start(BSP_PWM_SERVO_A); bsp_pwm_set_comp(BSP_PWM_SERVO_A, 0.02f); bsp_pwm_start(BSP_PWM_SERVO_B); diff --git a/src/module/launcher_drone/mod_launcher_drone.hpp b/src/module/launcher_drone/mod_launcher_drone.hpp index 81037ac4..7fb4576f 100644 --- a/src/module/launcher_drone/mod_launcher_drone.hpp +++ b/src/module/launcher_drone/mod_launcher_drone.hpp @@ -15,7 +15,6 @@ class UVALauncher { SINGLE, /* 单发开火模式 */ BURST, /* N爆发开火模式 */ SAFE, - FIRE } TrigMode; typedef struct { @@ -35,12 +34,10 @@ class UVALauncher { typedef enum { CHANGE_FIRE_MODE_RELAX, CHANGE_FIRE_MODE_SAFE, - CHANGE_FIRE_MODE_LOADED, + CHANGE_FIRE_MODE_LOADED, /*摩擦轮*/ CHANGE_TRIG_MODE_SINGLE, CHANGE_TRIG_MODE_BURST, - CHANGE_TRIG_MODE, - OPEN_COVER, - CLOSE_COVER, + CHANGE_TRIG_MODE, /*拨弹盘*/ LAUNCHER_START_FIRE, } LauncherEvent; @@ -50,8 +47,8 @@ class UVALauncher { }; struct FireControl { - bool fire = false; - bool firc_on = false; + bool fire = false; /*拨弹盘开启状态*/ + bool fric_on = false; /*摩擦轮开启状态*/ uint32_t last_launch = 0; /* 上次发射器时间 单位：ms */ bool last_fire = false; /* 上次开火状态 */ float last_trig_angle = 1.0f; @@ -82,7 +79,7 @@ class UVALauncher { float now_ = 0; - float dt_ = 0; + float dt_ = 0; /*单位us*/ float trig_angle_ = 0; diff --git a/src/robot/dart/robot.cpp b/src/robot/dart/robot.cpp index 688e693c..2bee946a 100644 --- a/src/robot/dart/robot.cpp +++ b/src/robot/dart/robot.cpp @@ -53,7 +53,7 @@ Dart::Param param = { Device::RMMotor::Param{ .id_feedback = 0x205, .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M3508, + .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_1, }, }, @@ -64,144 +64,126 @@ Dart::Param param = { .cali_speed = -2000.0f, - .max_range = 322.0f, + .max_range = 750.0f, .margin_error = 3.0f, .reduction_ratio = 3591.0f / 187.0f }, - .reload_actr = { - .stall_detect = { - .speed_thld = 0.0f, - .current_thld = 0.0f, - .stop_current_thld = 0.0f, - .temp_thld = -1.0f, - .timeout = 0.0f - }, - - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0002f, - .p = 4.0f, - .i = 0.6f, - .d = 0.03f, - .i_limit = 0.5f, - .out_limit = 0.5f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 6000.0f, - .p = 2.0f, - .i = 1.0f, - .d = 0.8f, - .i_limit = 1000.0f, - .out_limit = 2000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x207, - .id_control = GM6020_CTRL_ID_BASE, - .model = Device::RMMotor::MOTOR_GM6020, - .can = BSP_CAN_1, - }, - }, - - .motor_name = { - "reload" - }, - .cali_speed = -0.0f, +/* +目前左边： +top 回到位置0（0.1处）fric不动 +mid relax（0.1）fric 不动 +bot 往前推 fric动 - .max_range = 6.29748f, +左边：（推板） +top 上电位置0.1（包含后撤） +mid 不写（保证触发bot) +bot 往前推1 - .margin_error = 0.0f, +右边： +top 摩擦轮relax +mid 摩擦轮on +bot 摩擦轮on - .reduction_ratio = 3591.0f / 187.0f - }, +*/ .EVENT_MAP = { Component::CMD::EventMapItem{ Component::CMD::CMD_EVENT_LOST_CTRL, - Module::DartLauncher::OFF + Module::DartLauncher::RELAX }, - Component::CMD::EventMapItem{ + Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_TOP, - Module::DartLauncher::OFF + Module::DartLauncher::RESET_POSITION }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_L_POS_MID, - Module::DartLauncher::ON + Module::DartLauncher::SET_STAY }, Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_L_POS_BOT, + Module::DartLauncher::FIRE + }, + Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_TOP, - Module::DartLauncher::RESET + Module::DartLauncher::SET_FRIC_OFF + }, + Component::CMD::EventMapItem{ + Device::DR16::DR16_SW_R_POS_MID, + Module::DartLauncher::SET_FRIC_OUTOST }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::DartLauncher::RELOAD + Module::DartLauncher::SET_FRIC_BASE }, }, .fric_actr = { Component::SpeedActuator::Param{ .speed = { - .k = 0.8f, + .k = 0.0005f, .p = 1.0f, - .i = 0.3f, - .d = 0.0f, + .i = 0.4f, + .d = 0.01f, .i_limit = 0.5f, .out_limit = 0.5f, .d_cutoff_freq = -1.0f, .cycle = false, }, + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, Component::SpeedActuator::Param{ .speed = { - .k = 0.8f, + .k = 0.0005f, .p = 1.0f, - .i = 0.3f, - .d = 0.0f, + .i = 0.4f, + .d = 0.01f, .i_limit = 0.5f, .out_limit = 0.5f, .d_cutoff_freq = -1.0f, .cycle = false, }, + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, Component::SpeedActuator::Param{ .speed = { - .k = 0.8f, + .k = 0.0005f, .p = 1.0f, - .i = 0.3f, - .d = 0.0f, + .i = 0.4f, + .d = 0.01f, .i_limit = 0.5f, .out_limit = 0.5f, .d_cutoff_freq = -1.0f, .cycle = false, }, + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + }, Component::SpeedActuator::Param{ .speed = { - .k = 0.8f, + .k = 0.0005f, .p = 1.0f, - .i = 0.3f, - .d = 0.0f, + .i = 0.4f, + .d = 0.01f, .i_limit = 0.5f, .out_limit = 0.5f, .d_cutoff_freq = -1.0f, .cycle = false, }, + .in_cutoff_freq = -1.0f, + + .out_cutoff_freq = -1.0f, + } }, @@ -211,7 +193,7 @@ Dart::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, - .reverse = false, + .reverse = true, }, Device::RMMotor::Param{ .id_feedback = 0x202, @@ -225,7 +207,7 @@ Dart::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, - .reverse = true, + .reverse = false, }, Device::RMMotor::Param{ .id_feedback = 0x204, @@ -235,108 +217,6 @@ Dart::Param param = { .reverse = false, } } - }, - .gimbal={ - .yaw_actr = { - .speed = { - /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.001f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.2f, - .out_limit = 1.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 100.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.0f, - .out_limit = 1000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = -1.0f, - - .out_cutoff_freq = -1.0f, - }, - - .pitch_actr = { - .stall_detect = { - .speed_thld = 1300.0f, - .current_thld = 2.0f, - .stop_current_thld = 4.0f, - .temp_thld = 48.0f, - .timeout = 0.1f - }, - - .pos_actuator = { - Component::PosActuator::Param{ - .speed = { - .k = 0.0002f, - .p = 1.0f, - .i = 0.0f, - .d = 0.0f, - .i_limit = 0.5f, - .out_limit = 0.5f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .position = { - .k = 500.0f, - .p = 1.0f, - .i = 0.0f, - .d = 0.8f, - .i_limit = 1000.0f, - .out_limit = 2000.0f, - .d_cutoff_freq = -1.0f, - .cycle = false, - }, - - .in_cutoff_freq = 10.0f, - - .out_cutoff_freq = 30.0f, - }, - }, - - .motor_param = { - Device::RMMotor::Param{ - .id_feedback = 0x206, - .id_control = M3508_M2006_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_M2006, - .can = BSP_CAN_2, - }, - }, - - .motor_name = { - "pitch" - }, - - .cali_speed = -2000.0f, - - .max_range = 30.0f, - - .margin_error = 1.0f, - - .reduction_ratio = 36 - }, - - .yaw_motor = { - .id_feedback = 0x20A, - .id_control = GM6020_CTRL_ID_EXTAND, - .model = Device::RMMotor::MOTOR_GM6020, - .can = BSP_CAN_2, - .reverse = false, - }, - - .yaw_zero = 0.7432, }, .bmi088 = { .rot_mat = { diff --git a/src/robot/dart/robot.hpp b/src/robot/dart/robot.hpp index c9308a6e..ff1b3be9 100644 --- a/src/robot/dart/robot.hpp +++ b/src/robot/dart/robot.hpp @@ -6,7 +6,6 @@ #include "dev_dr16.hpp" #include "dev_led_rgb.hpp" #include "dev_referee.hpp" -#include "mod_dart_gimbal.hpp" #include "mod_dart_launcher.hpp" void robot_init(); @@ -15,7 +14,6 @@ class Dart { public: typedef struct Param { Module::DartLauncher::Param dart{}; - Module::Dartgimbal::Param gimbal{}; Device::BMI088::Rotation bmi088{}; } Param; @@ -28,11 +26,8 @@ class Dart { Device::AHRS ahrs_; Module::DartLauncher dartlauncher_; - Module::Dartgimbal gimbal_; Dart(Param& param, float control_freq) - : bmi088_(param.bmi088), - dartlauncher_(param.dart, control_freq), - gimbal_(param.gimbal, control_freq) {} + : bmi088_(param.bmi088), dartlauncher_(param.dart, control_freq) {} }; } // namespace Robot diff --git a/src/robot/drone_gimbal/robot.cpp b/src/robot/drone_gimbal/robot.cpp index 5b40d3b2..89d585ea 100644 --- a/src/robot/drone_gimbal/robot.cpp +++ b/src/robot/drone_gimbal/robot.cpp @@ -14,9 +14,9 @@ Robot::UVA::Param param={ .yaw_actr = { .speed = { /* GIMBAL_CTRL_YAW_OMEGA_IDX */ - .k = 0.2f, + .k = 0.22f, .p = 1.0f, - .i = 0.0f, + .i = 0.5f, .d = 0.0f, .i_limit = 0.2f, .out_limit = 0.1f, @@ -26,7 +26,7 @@ Robot::UVA::Param param={ .position = { /* GIMBAL_CTRL_YAW_ANGLE_IDX */ - .k = 18.0f, + .k = 20.0f, .p = 2.5f, .i = 0.0f, .d = 0.0f, @@ -87,16 +87,16 @@ Robot::UVA::Param param={ }, .mech_zero = { - .yaw = 4.8f, + .yaw = 1.65f, .pit = 1.7f, .rol = 0.0f, }, .limit = { - .pitch_max = M_2PI - 0.06, + .pitch_max = M_2PI - 0.4, .pitch_min = M_2PI - 1.08, - .yaw_max = M_2PI - 3.8, - .yaw_min = M_2PI - 5.5, + .yaw_max = M_2PI - 0.7, + .yaw_min = M_2PI - 2.7, }, .EVENT_MAP = { @@ -133,7 +133,7 @@ Robot::UVA::Param param={ .i = 0.0f, .d = 0.03f, .i_limit = 0.5f, - .out_limit = 0.5f, + .out_limit = 0.6f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -192,14 +192,6 @@ Robot::UVA::Param param={ Device::DR16::KEY_L_RELEASE, Module::UVALauncher::CHANGE_FIRE_MODE_SAFE }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_R, - Module::UVALauncher::OPEN_COVER - }, - Component::CMD::EventMapItem{ - Device::DR16::KEY_F, - Module::UVALauncher::CLOSE_COVER - } }, }, .bmi088_rot = { diff --git a/src/robot/infantry/robot.cpp b/src/robot/infantry/robot.cpp index c94d77e5..5626486b 100644 --- a/src/robot/infantry/robot.cpp +++ b/src/robot/infantry/robot.cpp @@ -127,6 +127,7 @@ Robot::Infantry::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_1, + }, Device::RMMotor::Param{ .id_feedback = 0x202, @@ -205,7 +206,7 @@ Robot::Infantry::Param param = { .i = 0.0f, .d = 0.0f, .i_limit = 0.0f, - .out_limit = 10.0f, + .out_limit = 0.0f, .d_cutoff_freq = -1.0f, .cycle = true, }, @@ -222,7 +223,7 @@ Robot::Infantry::Param param = { .i = 0.f, .d = 0.f, .i_limit = 0.8f, - .out_limit = 1.0f, + .out_limit = 0.0f, .d_cutoff_freq = -1.0f, .cycle = false, }, @@ -256,18 +257,18 @@ Robot::Infantry::Param param = { .id_control = GM6020_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_GM6020, .can = BSP_CAN_2, - .reverse = true , + .reverse = false , }, .mech_zero = { - .yaw = 1.3f, - .pit =M_2PI - 5.37046671f, + .yaw = 1.3f + M_PI, + .pit = 3.43f, .rol = 0.0f, }, .limit = { - .pitch_max =M_2PI - 5.1f, - .pitch_min =M_2PI - 5.8f, + .pitch_max = 3.7f, + .pitch_min = 3.1f, }, .EVENT_MAP = { @@ -281,11 +282,11 @@ Robot::Infantry::Param param = { }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_MID, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::START_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::DR16_SW_R_POS_BOT, - Module::Gimbal::SET_MODE_ABSOLUTE + Module::Gimbal::START_AUTO_AIM }, Component::CMD::EventMapItem{ Device::DR16::KEY_R_PRESS, @@ -312,8 +313,8 @@ Robot::Infantry::Param param = { .trig_actr = { Component::PosActuator::Param{ .speed = { - .k = 1.5f, - .p = 1.0f, + .k = 3.5f, + .p = 1.1f, .i = 0.0f, .d = 0.03f, .i_limit = 0.5f, @@ -323,7 +324,7 @@ Robot::Infantry::Param param = { }, .position = { - .k = 1.2f, + .k = 1.5f, .p = 1.0f, .i = 0.0f, .d = 0.012f, @@ -342,7 +343,7 @@ Robot::Infantry::Param param = { .fric_actr = { Component::SpeedActuator::Param{ .speed = { - .k = 0.00025f, + .k = 0.00052f, .p = 1.0f, .i = 0.4f, .d = 0.01f, @@ -358,7 +359,7 @@ Robot::Infantry::Param param = { }, Component::SpeedActuator::Param{ .speed = { - .k = 0.00025f, + .k = 0.00052f, .p = 1.0f, .i = 0.4f, .d = 0.01f, @@ -380,6 +381,7 @@ Robot::Infantry::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M2006, .can = BSP_CAN_2, + .reverse = true, } }, @@ -389,12 +391,14 @@ Robot::Infantry::Param param = { .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, + .reverse = true, }, Device::RMMotor::Param{ .id_feedback = 0x204, .id_control = M3508_M2006_CTRL_ID_BASE, .model = Device::RMMotor::MOTOR_M3508, .can = BSP_CAN_2, + .reverse = true, }, }, @@ -440,8 +444,8 @@ Robot::Infantry::Param param = { .bmi088_rot = { .rot_mat = { - { +1, +0, +0}, - { +0, +1, +0}, + { -1, +0, +0}, + { +0, -1, +0}, { +0, +0, +1}, }, },