incorporate arm into superstructure.cc by adding arm constants
Change-Id: Icfd6ba91044b7a8f29f10edf8c4c44ae1a443d2c
Signed-off-by: Yash Maheshwari <yashmahe2018@gmail.com>
Signed-off-by: James (Peilun) Li <jamespeilunli@gmail.com>
diff --git a/y2024_bot3/control_loops/superstructure/BUILD b/y2024_bot3/control_loops/superstructure/BUILD
index 8d909bf..bcac518 100644
--- a/y2024_bot3/control_loops/superstructure/BUILD
+++ b/y2024_bot3/control_loops/superstructure/BUILD
@@ -142,6 +142,7 @@
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:subsystem_simulator",
"//frc971/control_loops:team_number_test_environment",
+ "//y2024_bot3/control_loops/superstructure/arm:arm_plants",
],
)
diff --git a/y2024_bot3/control_loops/superstructure/arm/BUILD b/y2024_bot3/control_loops/superstructure/arm/BUILD
new file mode 100644
index 0000000..d6d2fec
--- /dev/null
+++ b/y2024_bot3/control_loops/superstructure/arm/BUILD
@@ -0,0 +1,42 @@
+package(default_visibility = ["//y2024_bot3:__subpackages__"])
+
+genrule(
+ name = "genrule_arm",
+ outs = [
+ "arm_plant.h",
+ "arm_plant.cc",
+ "arm_plant.json",
+ "integral_arm_plant.h",
+ "integral_arm_plant.cc",
+ "integral_arm_plant.json",
+ ],
+ cmd = "$(location //y2024_bot3/control_loops/python:arm) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024_bot3/control_loops/python:arm",
+ ],
+)
+
+cc_library(
+ name = "arm_plants",
+ srcs = [
+ "arm_plant.cc",
+ "integral_arm_plant.cc",
+ ],
+ hdrs = [
+ "arm_plant.h",
+ "integral_arm_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+filegroup(
+ name = "arm_json",
+ srcs = ["integral_arm_plant.json"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.cc b/y2024_bot3/control_loops/superstructure/superstructure.cc
index c18da15..2c510ad 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2024_bot3/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,9 @@
constants_fetcher_(event_loop),
robot_constants_(&constants_fetcher_.constants()),
joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ arm_(robot_constants_->common()->arm(),
+ robot_constants_->robot()->arm_constants()->zeroing_constants()) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -38,33 +40,71 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- (void)position;
-
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
}
OutputT output_struct;
- if (unsafe_goal != nullptr) {
- }
-
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
}
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ arm_goal_buffer;
+
+ ArmStatus arm_status = ArmStatus::IDLE;
+ double arm_position =
+ robot_constants_->robot()->arm_constants()->arm_positions()->idle();
+ if (unsafe_goal != nullptr) {
+ switch (unsafe_goal->arm_position()) {
+ case PivotGoal::INTAKE:
+ arm_status = ArmStatus::INTAKE;
+ arm_position = robot_constants_->robot()
+ ->arm_constants()
+ ->arm_positions()
+ ->intake();
+ break;
+ case PivotGoal::AMP:
+ arm_status = ArmStatus::AMP;
+ arm_position =
+ robot_constants_->robot()->arm_constants()->arm_positions()->amp();
+ break;
+ default:
+ arm_position =
+ robot_constants_->robot()->arm_constants()->arm_positions()->idle();
+ }
+ }
+
+ arm_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *arm_goal_buffer.fbb(), arm_position));
+
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *arm_goal = &arm_goal_buffer.message();
+
+ // static_zeroing_single_dof_profiled_subsystem.h
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ arm_offset =
+ arm_.Iterate(arm_goal, position->arm(),
+ output != nullptr ? &output_struct.arm_voltage : nullptr,
+ status->fbb());
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- const bool zeroed = true;
- const bool estopped = false;
+ const bool zeroed = arm_.zeroed();
+ const bool estopped = arm_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
+ status_builder.add_arm(arm_offset);
+ status_builder.add_arm_status(arm_status);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024_bot3/control_loops/superstructure/superstructure.h b/y2024_bot3/control_loops/superstructure/superstructure.h
index 6362bc0..b7499c2 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure.h
+++ b/y2024_bot3/control_loops/superstructure/superstructure.h
@@ -35,6 +35,8 @@
double robot_velocity() const;
+ inline const PotAndAbsoluteEncoderSubsystem &arm() const { return arm_; }
+
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -47,6 +49,8 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ PotAndAbsoluteEncoderSubsystem arm_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
index fe26af2..d9f684c 100644
--- a/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -12,6 +12,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "frc971/zeroing/absolute_encoder.h"
#include "y2024_bot3/constants/simulated_constants_sender.h"
+#include "y2024_bot3/control_loops/superstructure/arm/arm_plant.h"
#include "y2024_bot3/control_loops/superstructure/superstructure.h"
#include "y2024_bot3/control_loops/superstructure/superstructure_can_position_generated.h"
@@ -56,10 +57,28 @@
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<Output>("/superstructure")) {
- (void)dt_;
- (void)simulated_robot_constants;
+ event_loop_->MakeFetcher<Output>("/superstructure")),
+ arm_(new CappedTestPlant(arm::MakeArmPlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->arm_constants()
+ ->zeroing_constants()
+ ->one_revolution_distance()),
+ {.subsystem_params = {simulated_robot_constants->common()->arm(),
+ simulated_robot_constants->robot()
+ ->arm_constants()
+ ->zeroing_constants()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->arm_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->arm()->range()),
+ simulated_robot_constants->robot()
+ ->arm_constants()
+ ->zeroing_constants()
+ ->measured_absolute_position(),
+ dt_)
+ {
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -78,8 +97,15 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
+ frc971::PotAndAbsolutePosition::Builder arm_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> arm_offset =
+ arm_.encoder()->GetSensorValues(&arm_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_arm(arm_offset);
+
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -94,6 +120,8 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
+ PotAndAbsoluteEncoderSimulator arm_;
+
bool first_ = true;
};
@@ -154,6 +182,29 @@
<< ": No status";
ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr)
<< ": No output";
+
+ double set_point;
+ auto arm_positions =
+ simulated_robot_constants_->robot()->arm_constants()->arm_positions();
+
+ switch (superstructure_goal_fetcher_->arm_position()) {
+ case PivotGoal::IDLE:
+ set_point = arm_positions->idle();
+ break;
+ case PivotGoal::INTAKE:
+ set_point = arm_positions->intake();
+ break;
+ case PivotGoal::AMP:
+ set_point = arm_positions->amp();
+ break;
+ default:
+ LOG(FATAL) << "PivotGoal is not IDLE, INTAKE, or AMP";
+ }
+
+ EXPECT_NEAR(
+ set_point,
+ superstructure_status_fetcher_->arm()->unprofiled_goal_position(),
+ 0.03);
}
void CheckIfZeroed() {
@@ -211,7 +262,6 @@
// Tests that the superstructure does nothing when the goal is to remain
// still.
-
TEST_F(SuperstructureTest, DoesNothing) {
SetEnabled(true);
WaitUntilZeroed();
@@ -240,4 +290,50 @@
CheckIfZeroed();
}
+TEST_F(SuperstructureTest, ArmPivotMovement) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_position(PivotGoal::INTAKE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(20));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::INTAKE);
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_position(PivotGoal::AMP);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::AMP);
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_arm_position(PivotGoal::IDLE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(10));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->arm_status(), ArmStatus::IDLE);
+
+ VerifyNearGoal();
+}
+
} // namespace y2024_bot3::control_loops::superstructure::testing