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/python/BUILD b/y2022/control_loops/python/BUILD
index 54bce63..d3a5a62 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -107,6 +107,22 @@
],
)
+py_binary(
+ name = "climber",
+ srcs = [
+ "climber.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ "//frc971/control_loops/python:linear_system",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2022/control_loops/python/climber.py b/y2022/control_loops/python/climber.py
new file mode 100755
index 0000000..fd1707b
--- /dev/null
+++ b/y2022/control_loops/python/climber.py
@@ -0,0 +1,51 @@
+#!/usr/bin/python3
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import linear_system
+import numpy
+import sys
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kClimber = linear_system.LinearSystemParams(
+ name='Climber',
+ motor=control_loop.Falcon(),
+ G=(1.0 / 10.0) * (1.0 / 3.0) * (1.0 / 3.0),
+ radius=22 * 0.25 / numpy.pi / 2.0 * 0.0254,
+ mass=2.0,
+ q_pos=0.10,
+ q_vel=1.35,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.00,
+ kalman_q_voltage=35.0,
+ kalman_r_position=0.05)
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[0.2], [0.0]])
+ linear_system.PlotKick(kClimber, R, plant_params=kClimber)
+ linear_system.PlotMotion(
+ kClimber, R, max_velocity=5.0, plant_params=kClimber)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the climber and integral climber.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'climber']
+ linear_system.WriteLinearSystem(kClimber,
+ argv[1:3], argv[3:5], namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
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