Skip to content

Commit

Permalink
提交飞镖相关代码
Browse files Browse the repository at this point in the history
  • Loading branch information
@Jiu-xiao
张皓琳 authored and Jiu-xiao committed May 19, 2024
1 parent dcd74db commit 743ed60
Show file tree
Hide file tree
Showing 4 changed files with 177 additions and 239 deletions.
125 changes: 88 additions & 37 deletions src/module/dart_launcher/mod_dart_launcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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] =
Expand All @@ -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<DartLauncher*, Event>(event_callback, this,
this->param_.EVENT_MAP);
Component::CMD::RegisterEvent<DartLauncher*, DartEvent>(
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);
}
};
Expand All @@ -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();
}
}

Expand All @@ -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<float>(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;
}
44 changes: 28 additions & 16 deletions src/module/dart_launcher/mod_dart_launcher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,51 +12,63 @@ class DartLauncher {
typedef struct {
Device::AutoCaliLimitedMech<Device::RMMotor, Device::RMMotor::Param,
1>::Param rod_actr;
Device::AutoCaliLimitedMech<Device::RMMotor, Device::RMMotor::Param,
1>::Param reload_actr;
const std::vector<Component::CMD::EventMapItem> EVENT_MAP;

std::array<Component::SpeedActuator::Param, 4> fric_actr;
std::array<Device::RMMotor::Param, 4> fric_motor;
} 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;

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<Device::RMMotor, Device::RMMotor::Param, 1>
rod_actr_;
Device::AutoCaliLimitedMech<Device::RMMotor, Device::RMMotor::Param, 1>
reload_actr_;

rod_actr_; /* 内含电机、执行器、防堵数据 */
std::array<Component::SpeedActuator*, 4> fric_actr_;
std::array<Device::RMMotor*, 4> fric_motor_;

Expand Down
Loading

0 comments on commit 743ed60

Please sign in to comment.