Merge "Make SendDrivetrainPosition use the static fbs api"
diff --git a/frc971/autonomous/BUILD b/frc971/autonomous/BUILD
index d5b8cdc..0dd86ca 100644
--- a/frc971/autonomous/BUILD
+++ b/frc971/autonomous/BUILD
@@ -43,6 +43,20 @@
],
)
+cc_library(
+ name = "user_button_localized_autonomous_actor",
+ srcs = [
+ "user_button_localized_autonomous_actor.cc",
+ ],
+ hdrs = [
+ "user_button_localized_autonomous_actor.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":base_autonomous_actor",
+ ],
+)
+
aos_config(
name = "aos_config",
src = "autonomous_config.json",
diff --git a/frc971/autonomous/user_button_localized_autonomous_actor.cc b/frc971/autonomous/user_button_localized_autonomous_actor.cc
new file mode 100644
index 0000000..5358a28
--- /dev/null
+++ b/frc971/autonomous/user_button_localized_autonomous_actor.cc
@@ -0,0 +1,113 @@
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
+
+using ::aos::monotonic_clock;
+namespace chrono = ::std::chrono;
+namespace this_thread = ::std::this_thread;
+namespace drivetrain = frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace autonomous {
+
+UserButtonLocalizedAutonomousActor::UserButtonLocalizedAutonomousActor(
+ ::aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+ : BaseAutonomousActor(event_loop, dt_config),
+ robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ drivetrain_status_fetcher_.Fetch();
+ replan_timer_ = event_loop->AddTimer([this]() { DoReplan(); });
+
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Schedule(event_loop->monotonic_now());
+ button_poll_->Schedule(event_loop->monotonic_now(),
+ chrono::milliseconds(50));
+ });
+
+ button_poll_ = event_loop->AddTimer([this]() {
+ const aos::monotonic_clock::time_point now =
+ this->event_loop()->context().monotonic_event_time;
+ if (robot_state_fetcher_.Fetch()) {
+ if (robot_state_fetcher_->user_button()) {
+ user_indicated_safe_to_reset_ = true;
+ MaybeSendStartingPosition();
+ }
+ }
+ if (joystick_state_fetcher_.Fetch()) {
+ if (joystick_state_fetcher_->has_alliance() &&
+ (joystick_state_fetcher_->alliance() != alliance_)) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ is_planned_ = false;
+ // Only kick the planning out by 2 seconds. If we end up enabled in
+ // that second, then we will kick it out further based on the code
+ // below.
+ replan_timer_->Schedule(now + std::chrono::seconds(2));
+ }
+ if (joystick_state_fetcher_->enabled()) {
+ if (!is_planned_) {
+ // Only replan once we've been disabled for 5 seconds.
+ replan_timer_->Schedule(now + std::chrono::seconds(5));
+ }
+ }
+ }
+ });
+}
+
+void UserButtonLocalizedAutonomousActor::DoReplan() {
+ if (!drivetrain_status_fetcher_.Fetch()) {
+ replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
+ AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
+ return;
+ }
+
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+
+ sent_starting_position_ = false;
+
+ Replan();
+}
+
+void UserButtonLocalizedAutonomousActor::MaybeSendStartingPosition() {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+}
+
+void UserButtonLocalizedAutonomousActor::DoReset() {
+ InitializeEncoders();
+ ResetDrivetrain();
+
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
+
+ Reset();
+}
+
+bool UserButtonLocalizedAutonomousActor::RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) {
+ DoReset();
+
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
+
+ if (!user_indicated_safe_to_reset_) {
+ AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+ // Clear this so that we don't accidentally resend things as soon as we
+ // replan later.
+ user_indicated_safe_to_reset_ = false;
+ is_planned_ = false;
+ starting_position_.reset();
+
+ return Run(params);
+}
+
+} // namespace autonomous
+} // namespace frc971
diff --git a/frc971/autonomous/user_button_localized_autonomous_actor.h b/frc971/autonomous/user_button_localized_autonomous_actor.h
new file mode 100644
index 0000000..07d083a
--- /dev/null
+++ b/frc971/autonomous/user_button_localized_autonomous_actor.h
@@ -0,0 +1,60 @@
+#ifndef FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
+#define FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
+
+#include <memory>
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "aos/events/shm_event_loop.h"
+#include "frc971/autonomous/auto_generated.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2019/control_loops/drivetrain/target_selector_generated.h"
+
+namespace frc971 {
+namespace autonomous {
+
+class UserButtonLocalizedAutonomousActor : public BaseAutonomousActor {
+ public:
+ explicit UserButtonLocalizedAutonomousActor(
+ ::aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ protected:
+ virtual bool Run(
+ const ::frc971::autonomous::AutonomousActionParams *params) = 0;
+ virtual void SendStartingPosition(const Eigen::Vector3d &start) = 0;
+ virtual void Replan() = 0;
+ virtual void Reset() = 0;
+
+ void MaybeSendStartingPosition();
+
+ ::aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+ ::aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ bool is_planned_ = false;
+ bool sent_starting_position_ = false;
+
+ std::optional<Eigen::Vector3d> starting_position_;
+
+ private:
+ void DoReplan();
+ void DoReset();
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ bool user_indicated_safe_to_reset_ = false;
+};
+
+} // namespace autonomous
+} // namespace frc971
+
+#endif // FRC971_AUTONOMOUS_EXTENDED_AUTONOMOUS_ACTOR_H_
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 7e54bdf..e179193 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -743,3 +743,33 @@
# Diameter of 1.9", weight of: 100 grams
# TODO(austin): Get a number from Scott Westbrook for the mass
self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
+
+
+class KrakenFOC(object):
+ """Class representing the WCP Kraken X60 motor using
+ Field Oriented Controls (FOC) communication.
+
+ All numbers based on data from
+ https://wcproducts.com/products/kraken.
+ """
+
+ def __init__(self):
+ # Stall Torque in N m
+ self.stall_torque = 9.37
+ # Stall Current in Amps
+ self.stall_current = 483.0
+ # Free Speed in rad / sec
+ self.free_speed = 5800.0 / 60.0 * 2.0 * numpy.pi
+ # Free Current in Amps
+ self.free_current = 2.0
+ # Resistance of the motor, divided by 2 to account for the 2 motors
+ self.resistance = 12.0 / self.stall_current
+ # Motor velocity constant
+ self.Kv = (self.free_speed /
+ (12.0 - self.resistance * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Motor inertia in kg m^2
+ # Diameter of 1.9", weight of: 100 grams
+ # TODO(Filip): Update motor inertia for Kraken, currently using Falcon motor inertia
+ self.motor_inertia = 0.1 * ((0.95 * 0.0254)**2.0)
diff --git a/y2023/autonomous/BUILD b/y2023/autonomous/BUILD
index cfcc19c..3f2066f 100644
--- a/y2023/autonomous/BUILD
+++ b/y2023/autonomous/BUILD
@@ -48,7 +48,7 @@
"//aos/events:event_loop",
"//aos/logging",
"//aos/util:phased_loop",
- "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/autonomous:user_button_localized_autonomous_actor",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 8891223..e452d21 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -39,15 +39,12 @@
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
- : frc971::autonomous::BaseAutonomousActor(
+ : frc971::autonomous::UserButtonLocalizedAutonomousActor(
event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
"/drivetrain")),
- joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
auto_splines_(),
arm_goal_position_(control_loops::superstructure::arm::StartingIndex()),
superstructure_goal_sender_(
@@ -57,57 +54,9 @@
event_loop
->MakeFetcher<::y2023::control_loops::superstructure::Status>(
"/superstructure")),
- points_(control_loops::superstructure::arm::PointList()) {
- drivetrain_status_fetcher_.Fetch();
- replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
-
- event_loop->OnRun([this, event_loop]() {
- replan_timer_->Schedule(event_loop->monotonic_now());
- button_poll_->Schedule(event_loop->monotonic_now(),
- chrono::milliseconds(50));
- });
-
- // TODO(james): Really need to refactor this code since we keep using it.
- button_poll_ = event_loop->AddTimer([this]() {
- const aos::monotonic_clock::time_point now =
- this->event_loop()->context().monotonic_event_time;
- if (robot_state_fetcher_.Fetch()) {
- if (robot_state_fetcher_->user_button()) {
- user_indicated_safe_to_reset_ = true;
- MaybeSendStartingPosition();
- }
- }
- if (joystick_state_fetcher_.Fetch()) {
- if (joystick_state_fetcher_->has_alliance() &&
- (joystick_state_fetcher_->alliance() != alliance_)) {
- alliance_ = joystick_state_fetcher_->alliance();
- is_planned_ = false;
- // Only kick the planning out by 2 seconds. If we end up enabled in
- // that second, then we will kick it out further based on the code
- // below.
- replan_timer_->Schedule(now + std::chrono::seconds(2));
- }
- if (joystick_state_fetcher_->enabled()) {
- if (!is_planned_) {
- // Only replan once we've been disabled for 5 seconds.
- replan_timer_->Schedule(now + std::chrono::seconds(5));
- }
- }
- }
- });
-}
+ points_(control_loops::superstructure::arm::PointList()) {}
void AutonomousActor::Replan() {
- if (!drivetrain_status_fetcher_.Fetch()) {
- replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
- AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
- return;
- }
-
- if (alliance_ == aos::Alliance::kInvalid) {
- return;
- }
- sent_starting_position_ = false;
if (FLAGS_spline_auto) {
test_spline_ =
PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
@@ -158,23 +107,7 @@
MaybeSendStartingPosition();
}
-void AutonomousActor::MaybeSendStartingPosition() {
- if (is_planned_ && user_indicated_safe_to_reset_ &&
- !sent_starting_position_) {
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
-}
-
void AutonomousActor::Reset() {
- InitializeEncoders();
- ResetDrivetrain();
-
- joystick_state_fetcher_.Fetch();
- CHECK(joystick_state_fetcher_.get() != nullptr)
- << "Expect at least one JoystickState message before running auto...";
- alliance_ = joystick_state_fetcher_->alliance();
-
wrist_goal_ = 0.0;
roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
arm_goal_position_ = control_loops::superstructure::arm::StartingIndex();
@@ -182,23 +115,8 @@
SendSuperstructureGoal();
}
-bool AutonomousActor::RunAction(
+bool AutonomousActor::Run(
const ::frc971::autonomous::AutonomousActionParams *params) {
- Reset();
-
- AOS_LOG(INFO, "Params are %d\n", params->mode());
-
- if (!user_indicated_safe_to_reset_) {
- AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
- // Clear this so that we don't accidentally resend things as soon as we
- // replan later.
- user_indicated_safe_to_reset_ = false;
- is_planned_ = false;
- starting_position_.reset();
-
AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 31ae4f5..5a5cd06 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -3,7 +3,7 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -14,13 +14,11 @@
namespace y2023 {
namespace autonomous {
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor
+ : public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
- bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) override;
-
private:
void set_arm_goal_position(uint32_t requested_arm_goal_position) {
arm_goal_position_ = requested_arm_goal_position;
@@ -48,39 +46,27 @@
void IntakeCube();
void Balance();
+ bool Run(const ::frc971::autonomous::AutonomousActionParams *params) override;
+ void Replan() override;
+ void SendStartingPosition(const Eigen::Vector3d &start) override;
+ void Reset() override;
+
[[nodiscard]] bool WaitForArmGoal(double distance_to_go = 0.01);
[[nodiscard]] bool WaitForPreloaded();
- void Reset();
-
- void SendStartingPosition(const Eigen::Vector3d &start);
- void MaybeSendStartingPosition();
void SplineAuto();
void ChargedUp();
void ChargedUpCableSide();
- void Replan();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- aos::Fetcher<aos::RobotState> robot_state_fetcher_;
double wrist_goal_;
control_loops::superstructure::RollerGoal roller_goal_ =
control_loops::superstructure::RollerGoal::IDLE;
- aos::TimerHandler *replan_timer_;
- aos::TimerHandler *button_poll_;
-
- aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
- bool user_indicated_safe_to_reset_ = false;
- bool sent_starting_position_ = false;
-
- bool is_planned_ = false;
-
- std::optional<Eigen::Vector3d> starting_position_;
uint32_t arm_goal_position_;
bool preloaded_ = false;
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
index 1d1054a..64dcc47 100644
--- a/y2024/control_loops/python/drivetrain.py
+++ b/y2024/control_loops/python/drivetrain.py
@@ -18,7 +18,7 @@
# TODO(austin): Measure radius a bit better.
robot_radius=0.39,
wheel_radius=2.5 * 0.0254,
- motor_type=control_loop.Falcon(),
+ motor_type=control_loop.KrakenFOC(),
num_motors=3,
G=(14.0 / 52.0) * (26.0 / 58.0),
q_pos=0.24,