Add climber code without tests
This runs a loop to follow the request.
Signed-off-by: Siddhant Kanwar <kanwarsiddhant@gmail.com>
Change-Id: Iad516390561175f74eb33e0cda6a505376a372ed
diff --git a/y2022/control_loops/superstructure/climber/BUILD b/y2022/control_loops/superstructure/climber/BUILD
new file mode 100644
index 0000000..8ec8071
--- /dev/null
+++ b/y2022/control_loops/superstructure/climber/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_climber",
+ outs = [
+ "climber_plant.h",
+ "climber_plant.cc",
+ "integral_climber_plant.h",
+ "integral_climber_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:climber) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:climber",
+ ],
+)
+
+cc_library(
+ name = "climber_plants",
+ srcs = [
+ "climber_plant.cc",
+ "integral_climber_plant.cc",
+ ],
+ hdrs = [
+ "climber_plant.h",
+ "integral_climber_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",
+ ],
+)
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b2c89d1..a7af922 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -8,36 +8,44 @@
using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops:: RelativeEncoderProfiledJointStatus;
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
- name) {
+ name),
+ climber_(constants::GetValues().climber.subsystem_params) {
event_loop->SetRuntimeRealtimePriority(30);
}
void Superstructure::RunIteration(const Goal *unsafe_goal,
- const Position * /*position*/,
+ const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
}
- if (output != nullptr && unsafe_goal != nullptr) {
- OutputT output_struct;
- output_struct.climber_voltage = unsafe_goal->climber_speed();
+ OutputT output_struct;
+
+ flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
+ climber_status_offset = climber_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
+ position->climber(),
+ output != nullptr ? &(output_struct.climber_voltage) : nullptr,
+ status->fbb());
+
+ if (output != nullptr) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(true);
- status_builder.add_estopped(false);
+ // Climber is always zeroed; only has a pot
+ status_builder.add_zeroed(climber_.zeroed());
+ status_builder.add_estopped(climber_.estopped());
+ status_builder.add_climber(climber_status_offset);
- if (unsafe_goal != nullptr) {
- status_builder.add_climber_speed(unsafe_goal->climber_speed());
- }
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index 95e24d3..4c03a08 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -1,8 +1,8 @@
#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
-#include "frc971/control_loops/control_loop.h"
#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loop.h"
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
@@ -16,6 +16,11 @@
class Superstructure
: public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ using RelativeEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::RelativeEncoderZeroingEstimator,
+ ::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
+
explicit Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name = "/superstructure");
@@ -25,6 +30,8 @@
aos::Sender<Status>::Builder *status) override;
private:
+ RelativeEncoderSubsystem climber_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 3e0679e..8107363 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,9 +3,8 @@
namespace y2022.control_loops.superstructure;
table Goal {
- // Voltage of the climber falcon
- // - is down + is up
- climber_speed:double (id: 0);
+ // Height of the climber above rest point
+ climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 0);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index 02002ba..a36f4f6 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -3,7 +3,7 @@
namespace y2022.control_loops.superstructure;
table Position {
-
+ climber:frc971.RelativePosition (id: 0);
}
root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 5f8cdc3..0ee0994 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -10,9 +10,8 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
- // Goal voltage of the climber falcon
- // - is down + is up
- climber_speed:double (id: 2);
+ // Subsystem statuses
+ climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);
}
root_type Status;
\ No newline at end of file