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/BUILD b/y2022/BUILD
index 97d42a1..640058b 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -189,6 +189,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022/control_loops/superstructure/climber:climber_plants",
"//y2022/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 73e72f3..e71fb64 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -12,6 +12,7 @@
#include "aos/network/team_number.h"
#include "glog/logging.h"
#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
+#include "y2022/control_loops/superstructure/climber/integral_climber_plant.h"
namespace y2022 {
namespace constants {
@@ -58,18 +59,32 @@
// Measured absolute position of the encoder when at zero.
intake->zeroing_constants.measured_absolute_position = 0.0;
+ // Climber constants
+ auto *const climber = &r->climber;
+ climber->subsystem_params.zeroing_voltage = 3.0;
+ climber->subsystem_params.operating_voltage = 12.0;
+ climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
+ climber->subsystem_params.default_profile_params = {6.0, 1.0};
+ climber->subsystem_params.range = Values::kClimberRange();
+ climber->subsystem_params.make_integral_loop =
+ control_loops::superstructure::climber::MakeIntegralClimberLoop;
+
switch (team) {
// A set of constants for tests.
case 1:
+ climber->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
+ climber->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
+ climber->potentiometer_offset = 0.0;
break;
case kCodingRobotTeamNumber:
+ climber->potentiometer_offset = 0.0;
break;
default:
diff --git a/y2022/constants.h b/y2022/constants.h
index 92f3a48..6927567 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -55,8 +55,29 @@
.upper = 2.725 // Front Soft
};
}
+
// Climber
- static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
+ static constexpr ::frc971::constants::Range kClimberRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.01,
+ .upper_hard = 0.6,
+ .lower = 0.0,
+ .upper = 0.5
+ };
+ }
+ static constexpr double kClimberPotMetersPerRevolution() {
+ return 22 * 0.25 * 0.0254;
+ }
+ static constexpr double kClimberPotRatio() { return 1.0; }
+
+ struct PotConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::RelativeEncoderZeroingEstimator>
+ subsystem_params;
+ double potentiometer_offset;
+ };
+
+ PotConstants climber;
// Intake
// two encoders with same gear ratio for intake
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
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 32482ea..6818ae5 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -75,6 +75,12 @@
control_loops::drivetrain::kWheelRadius;
}
+double climber_pot_translate(double voltage) {
+ return voltage * Values::kClimberPotRatio() *
+ (10.0 /*turns*/ / 5.0 /*volts*/) *
+ Values::kClimberPotMetersPerRevolution();
+}
+
constexpr double kMaxFastEncoderPulsesPerSecond =
Values::kMaxDrivetrainEncoderPulsesPerSecond();
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
@@ -114,6 +120,8 @@
}
void RunIteration() override {
+ const constants::Values &values = constants::GetValues();
+
{
auto builder = drivetrain_position_sender_.MakeBuilder();
frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
@@ -135,8 +143,16 @@
{
auto builder = superstructure_position_sender_.MakeBuilder();
+
+ frc971::RelativePositionT climber;
+ CopyPosition(*climber_potentiometer_, &climber, climber_pot_translate,
+ false, values.climber.potentiometer_offset);
+ flatbuffers::Offset<frc971::RelativePosition> climber_offset =
+ frc971::RelativePosition::Pack(*builder.fbb(), &climber);
+
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
+ position_builder.add_climber(climber_offset);
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -159,28 +175,32 @@
}
}
+ void set_climber_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ climber_potentiometer_ = ::std::move(potentiometer);
+ }
+
private:
- ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
- ::aos::Sender<superstructure::Position> superstructure_position_sender_;
- ::aos::Sender<::frc971::control_loops::drivetrain::Position>
+ aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ aos::Sender<superstructure::Position> superstructure_position_sender_;
+ aos::Sender<frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
- ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+ std::unique_ptr<frc::AnalogInput> climber_potentiometer_;
+
+ std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
};
class SuperstructureWriter
: public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
public:
- SuperstructureWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ SuperstructureWriter(aos::EventLoop *event_loop)
+ : frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
void set_climber_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
- climber_falcon_ = ::std::move(t);
- climber_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kClimberSupplyCurrentLimit(),
- Values::kClimberSupplyCurrentLimit(), 0});
+ std::unique_ptr<frc::TalonFX> t) {
+ climber_falcon_ = std::move(t);
}
void set_turret_falcon(::std::unique_ptr<::frc::TalonFX> t) {
@@ -196,15 +216,10 @@
}
private:
- void WriteToFalconCan(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
- }
-
void Write(const superstructure::Output &output) override {
- WriteToFalconCan(output.climber_voltage(), climber_falcon_.get());
+ climber_falcon_->SetSpeed(std::clamp(output.climber_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
catapult_falcon_1_->SetSpeed(std::clamp(output.catapult_voltage(),
-kMaxBringupPower,
kMaxBringupPower) /
@@ -220,17 +235,14 @@
void Stop() override {
AOS_LOG(WARNING, "Superstructure output too old.\n");
- climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ climber_falcon_->SetDisabled();
catapult_falcon_1_->SetDisabled();
catapult_falcon_2_->SetDisabled();
turret_falcon_->SetDisabled();
}
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- climber_falcon_;
-
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
- catapult_falcon_2_;
+ catapult_falcon_2_, climber_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -260,6 +272,7 @@
SensorReader sensor_reader(&sensor_reader_event_loop);
sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
+ sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(0));
AddLoop(&sensor_reader_event_loop);
@@ -273,8 +286,7 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
- superstructure_writer.set_climber_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ superstructure_writer.set_climber_falcon(make_unique<frc::TalonFX>(5));
superstructure_writer.set_turret_falcon(make_unique<::frc::TalonFX>(2));
superstructure_writer.set_catapult_falcon_1(make_unique<::frc::TalonFX>(3));
superstructure_writer.set_catapult_falcon_2(make_unique<::frc::TalonFX>(4));