Updated constants and wpilib_interface.
Updated wpilib_interface and the constants files to have the
superstructure components.
Updated the linear_system codegen to output free speed in radians.
Updated the intake python file with the correct gear ratio.
Created the superstructure plants.
Change-Id: I5a2b54fe3de8d9ae9b0f79820465a2f97baed22d
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index 322e41a..e1f55cc 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -379,10 +379,12 @@
linear_system.name, [linear_system], namespaces=year_namespaces)
loop_writer.AddConstant(
control_loop.Constant('kFreeSpeed', '%f', linear_system.motor.
- free_speed / (2.0 * numpy.pi)))
+ free_speed))
loop_writer.AddConstant(
control_loop.Constant('kOutputRatio', '%f', linear_system.G *
linear_system.radius))
+ loop_writer.AddConstant(
+ control_loop.Constant('kRadius', '%f', linear_system.radius))
loop_writer.Write(plant_files[0], plant_files[1])
integral_linear_system = IntegralLinearSystem(params,
diff --git a/y2019/BUILD b/y2019/BUILD
index 2eb5e24..576cf6c 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -24,6 +24,10 @@
"//aos/network:team_number",
"//frc971:constants",
"//y2019/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2019/control_loops/superstructure/elevator:elevator_plants",
+ "//y2019/control_loops/superstructure/intake:intake_plants",
+ "//y2019/control_loops/superstructure/stilts:stilts_plants",
+ "//y2019/control_loops/superstructure/wrist:wrist_plants",
],
)
@@ -63,6 +67,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
"//third_party/Phoenix-frc-lib:phoenix",
+ "//y2019/control_loops/superstructure:superstructure_queue",
],
)
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 0851577..e91a49c 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -26,19 +26,99 @@
const uint16_t kCompTeamNumber = 971;
const uint16_t kPracticeTeamNumber = 9971;
+const uint16_t kCodingRobotTeamNumber = 7971;
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ Values::PotAndAbsConstants *const elevator = &r->elevator;
+ Values::Intake *const intake = &r->intake;
+ Values::PotAndAbsConstants *const stilts = &r->stilts;
+ Values::PotAndAbsConstants *const wrist = &r->wrist;
+
+ elevator->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ elevator->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kElevatorEncoderRatio();
+ elevator->zeroing.zeroing_threshold = 0.0005;
+ elevator->zeroing.moving_buffer_size = 20;
+ elevator->zeroing.allowable_encoder_error = 0.9;
+
+ intake->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing.zeroing_threshold = 0.0005;
+ intake->zeroing.moving_buffer_size = 20;
+ intake->zeroing.allowable_encoder_error = 0.9;
+
+ stilts->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ stilts->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kStiltsEncoderRatio();
+ stilts->zeroing.zeroing_threshold = 0.0005;
+ stilts->zeroing.moving_buffer_size = 20;
+ stilts->zeroing.allowable_encoder_error = 0.9;
+
+ wrist->zeroing.average_filter_size = Values::kZeroingSampleSize;
+ wrist->zeroing.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kWristEncoderRatio();
+ wrist->zeroing.zeroing_threshold = 0.0005;
+ wrist->zeroing.moving_buffer_size = 20;
+ wrist->zeroing.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
case kCompTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
case kPracticeTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
+ break;
+
+ case kCodingRobotTeamNumber:
+ elevator->zeroing.measured_absolute_position = 0.0;
+ elevator->potentiometer_offset = 0.0;
+
+ intake->zeroing.measured_absolute_position = 0.0;
+ intake->zeroing.middle_position = 0.0;
+
+ stilts->zeroing.measured_absolute_position = 0.0;
+ stilts->potentiometer_offset = 0.0;
+
+ wrist->zeroing.measured_absolute_position = 0.0;
+ wrist->potentiometer_offset = 0.0;
break;
default:
@@ -78,6 +158,5 @@
return *values[team_number];
}
-
} // namespace constants
} // namespace y2019
diff --git a/y2019/constants.h b/y2019/constants.h
index e68143c..5e1de7b 100644
--- a/y2019/constants.h
+++ b/y2019/constants.h
@@ -6,6 +6,10 @@
#include "frc971/constants.h"
#include "y2019/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2019/control_loops/superstructure/intake/intake_plant.h"
+#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
+#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
+#include "y2019/control_loops/superstructure/wrist/wrist_plant.h"
namespace y2019 {
namespace constants {
@@ -27,15 +31,94 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() {
- return (24.0 / 52.0);
- }
+ static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
control_loops::drivetrain::kHighOutputRatio /
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
+
+ // Elevator
+ static constexpr double kElevatorEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kElevatorEncoderRatio() {
+ return (1.0) * control_loops::superstructure::elevator::kRadius;
+ }
+
+ static constexpr double kMaxElevatorEncoderPulsesPerSecond() {
+ return control_loops::superstructure::elevator::kFreeSpeed *
+ control_loops::superstructure::elevator::kOutputRatio /
+ kElevatorEncoderRatio() / (2.0 * M_PI) *
+ kElevatorEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kElevatorPotRatio() {
+ return (1.0) * control_loops::superstructure::elevator::kRadius;
+ }
+
+ // Intake
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kIntakeEncoderRatio() { return (18.0 / 38.0); }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() / (2.0 * M_PI) *
+ kIntakeEncoderCountsPerRevolution();
+ }
+
+ // Wrist
+ static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kWristEncoderRatio() {
+ return (20.0 / 100.0) * (24.0 / 84.0);
+ }
+
+ static constexpr double kMaxWristEncoderPulsesPerSecond() {
+ return control_loops::superstructure::wrist::kFreeSpeed *
+ control_loops::superstructure::wrist::kOutputRatio /
+ kWristEncoderRatio() / (2.0 * M_PI) *
+ kWristEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kWristPotRatio() { return (24.0) / (84.0); }
+
+ // Stilts
+ static constexpr double kStiltsEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kStiltsEncoderRatio() {
+ return (1.0 /* Gear ratio */) *
+ control_loops::superstructure::stilts::kRadius;
+ }
+
+ static constexpr double kMaxStiltsEncoderPulsesPerSecond() {
+ return control_loops::superstructure::stilts::kFreeSpeed *
+ control_loops::superstructure::stilts::kOutputRatio /
+ kStiltsEncoderRatio() / (2.0 * M_PI) *
+ kStiltsEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kStiltsPotRatio() {
+ return (1.0 /* Gear ratio */) *
+ control_loops::superstructure::stilts::kRadius;
+ }
+
+ struct PotAndAbsConstants {
+ ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants zeroing;
+ double potentiometer_offset;
+ };
+ PotAndAbsConstants elevator;
+ PotAndAbsConstants wrist;
+ PotAndAbsConstants stilts;
+
+ struct Intake {
+ ::frc971::constants::AbsoluteEncoderZeroingConstants zeroing;
+ };
+ Intake intake;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2019/control_loops/python/intake.py b/y2019/control_loops/python/intake.py
index 97f8880..d1c5710 100755
--- a/y2019/control_loops/python/intake.py
+++ b/y2019/control_loops/python/intake.py
@@ -20,7 +20,7 @@
kIntake = angular_system.AngularSystemParams(
name='Intake',
motor=control_loop.BAG(),
- G=(1.0 / 9.0) * (1.0 / 9.0) * (16.0 / 38.0),
+ G=(1.0 / 7.0) * (1.0 / 4.0) * (1.0 / 4.0)* (18.0 / 38.0),
# TODO(austin): Pull moments of inertia from CAD when it's done.
J=0.8,
q_pos=0.20,
diff --git a/y2019/control_loops/superstructure/elevator/BUILD b/y2019/control_loops/superstructure/elevator/BUILD
new file mode 100644
index 0000000..f3d99fe
--- /dev/null
+++ b/y2019/control_loops/superstructure/elevator/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_elevator",
+ outs = [
+ "elevator_plant.h",
+ "elevator_plant.cc",
+ "integral_elevator_plant.h",
+ "integral_elevator_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:elevator) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:elevator",
+ ],
+)
+
+cc_library(
+ name = "elevator_plants",
+ srcs = [
+ "elevator_plant.cc",
+ "integral_elevator_plant.cc",
+ ],
+ hdrs = [
+ "elevator_plant.h",
+ "integral_elevator_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/intake/BUILD b/y2019/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..4f7f6b0
--- /dev/null
+++ b/y2019/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:intake) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:intake",
+ ],
+)
+
+cc_library(
+ name = "intake_plants",
+ srcs = [
+ "intake_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_plant.h",
+ "integral_intake_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/stilts/BUILD b/y2019/control_loops/superstructure/stilts/BUILD
new file mode 100644
index 0000000..6beea39
--- /dev/null
+++ b/y2019/control_loops/superstructure/stilts/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_stilts",
+ outs = [
+ "stilts_plant.h",
+ "stilts_plant.cc",
+ "integral_stilts_plant.h",
+ "integral_stilts_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:stilts) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:stilts",
+ ],
+)
+
+cc_library(
+ name = "stilts_plants",
+ srcs = [
+ "stilts_plant.cc",
+ "integral_stilts_plant.cc",
+ ],
+ hdrs = [
+ "stilts_plant.h",
+ "integral_stilts_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index fef9ced..cd004ac 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -86,7 +86,7 @@
.frc971.PotAndAbsolutePosition wrist;
// Position of the intake. 0 when rollers are retracted, positive extended.
- .frc971.PotAndAbsolutePosition intake_joint;
+ .frc971.AbsolutePosition intake_joint;
// Position of the stilts, 0 when retracted (defualt), positive lifts robot.
.frc971.PotAndAbsolutePosition stilts;
@@ -124,4 +124,4 @@
queue Position position;
};
-queue_group SuperstructureQueue superstructure_queue;
\ No newline at end of file
+queue_group SuperstructureQueue superstructure_queue;
diff --git a/y2019/control_loops/superstructure/wrist/BUILD b/y2019/control_loops/superstructure/wrist/BUILD
new file mode 100644
index 0000000..3d007be
--- /dev/null
+++ b/y2019/control_loops/superstructure/wrist/BUILD
@@ -0,0 +1,32 @@
+package(default_visibility = ["//y2019:__subpackages__"])
+
+genrule(
+ name = "genrule_wrist",
+ outs = [
+ "wrist_plant.h",
+ "wrist_plant.cc",
+ "integral_wrist_plant.h",
+ "integral_wrist_plant.cc",
+ ],
+ cmd = "$(location //y2019/control_loops/python:wrist) $(OUTS)",
+ tools = [
+ "//y2019/control_loops/python:wrist",
+ ],
+)
+
+cc_library(
+ name = "wrist_plants",
+ srcs = [
+ "wrist_plant.cc",
+ "integral_wrist_plant.cc",
+ ],
+ hdrs = [
+ "wrist_plant.h",
+ "integral_wrist_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index bc6c25f..428e723 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -41,12 +41,14 @@
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2019/constants.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
using ::frc971::control_loops::drivetrain_queue;
+using ::y2019::control_loops::superstructure::superstructure_queue;
using ::y2019::constants::Values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -89,15 +91,30 @@
control_loops::drivetrain::kWheelRadius;
}
+double elevator_pot_translate(double voltage) {
+ return voltage * Values::kElevatorPotRatio() *
+ (3.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
+double wrist_pot_translate(double voltage) {
+ return voltage * Values::kWristPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (2 * M_PI /*radians*/);
+}
+
+double stilts_pot_translate(double voltage) {
+ return voltage * Values::kStiltsPotRatio() *
+ (10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
+}
+
constexpr double kMaxFastEncoderPulsesPerSecond =
- max(/*Values::kMaxDrivetrainEncoderPulsesPerSecond(),
- Values::kMaxIntakeMotorEncoderPulsesPerSecond()*/ 1.0, 1.0);
+ max(Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond());
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
constexpr double kMaxMediumEncoderPulsesPerSecond =
- max(/*Values::kMaxProximalEncoderPulsesPerSecond(),
- Values::kMaxDistalEncoderPulsesPerSecond()*/ 1.0, 1.0);
+ max(Values::kMaxElevatorEncoderPulsesPerSecond(),
+ Values::kMaxWristEncoderPulsesPerSecond());
static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
"medium encoders are too fast");
@@ -111,6 +128,69 @@
UpdateMediumEncoderFilterHz(kMaxMediumEncoderPulsesPerSecond);
}
+ // Elevator
+
+ void set_elevator_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ elevator_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_elevator_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ elevator_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_elevator_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ elevator_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ // Intake
+
+ void set_intake_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ intake_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_intake_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ intake_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ // Wrist
+
+ void set_wrist_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ wrist_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_wrist_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ wrist_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_wrist_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ wrist_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
+ // Stilts
+
+ void set_stilts_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ medium_encoder_filter_.Add(encoder.get());
+ stilts_encoder_.set_encoder(::std::move(encoder));
+ }
+
+ void set_stilts_absolute_pwm(
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ stilts_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
+ }
+
+ void set_stilts_potentiometer(
+ ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ stilts_encoder_.set_potentiometer(::std::move(potentiometer));
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -127,6 +207,113 @@
drivetrain_message.Send();
}
}
+
+ void RunDmaIteration() {
+ const auto values = constants::GetValues();
+
+ {
+ auto superstructure_message = superstructure_queue.position.MakeMessage();
+
+ // Elevator
+ CopyPosition(elevator_encoder_, &superstructure_message->elevator,
+ Values::kElevatorEncoderCountsPerRevolution(),
+ Values::kElevatorEncoderRatio(), elevator_pot_translate,
+ false, values.elevator.potentiometer_offset);
+ // Intake
+ CopyPosition(intake_encoder_, &superstructure_message->intake_joint,
+ Values::kIntakeEncoderCountsPerRevolution(),
+ Values::kIntakeEncoderRatio(), false);
+
+ // Wrist
+ CopyPosition(wrist_encoder_, &superstructure_message->wrist,
+ Values::kWristEncoderCountsPerRevolution(),
+ Values::kWristEncoderRatio(), wrist_pot_translate, false,
+ values.wrist.potentiometer_offset);
+
+ // Stilts
+ CopyPosition(stilts_encoder_, &superstructure_message->stilts,
+ Values::kStiltsEncoderCountsPerRevolution(),
+ Values::kStiltsEncoderRatio(), stilts_pot_translate, false,
+ values.stilts.potentiometer_offset);
+
+ superstructure_message.Send();
+ }
+ }
+
+ private:
+ ::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
+ wrist_encoder_, stilts_encoder_;
+
+ ::frc971::wpilib::AbsoluteEncoder intake_encoder_;
+ // TODO(sabina): Add wrist and elevator hall effects.
+};
+
+class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ elevator_victor_ = ::std::move(t);
+ }
+
+ void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ intake_victor_ = ::std::move(t);
+ }
+ void set_intake_rollers_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ intake_rollers_victor_ = ::std::move(t);
+ }
+
+ void set_wrist_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ wrist_victor_ = ::std::move(t);
+ }
+
+ void set_stilts_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ stilts_victor_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2019::control_loops::superstructure::superstructure_queue.output
+ .FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue =
+ ::y2019::control_loops::superstructure::superstructure_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ elevator_victor_->SetSpeed(::aos::Clip(-queue->elevator_voltage,
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ intake_victor_->SetSpeed(::aos::Clip(-queue->intake_joint_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ intake_rollers_victor_->SetSpeed(::aos::Clip(-queue->intake_roller_voltage,
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+
+ wrist_victor_->SetSpeed(::aos::Clip(-queue->wrist_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+
+ stilts_victor_->SetSpeed(::aos::Clip(-queue->stilts_voltage,
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Superstructure output too old.\n");
+
+ elevator_victor_->SetDisabled();
+ intake_victor_->SetDisabled();
+ intake_rollers_victor_->SetDisabled();
+ wrist_victor_->SetDisabled();
+ stilts_victor_->SetDisabled();
+ }
+
+ ::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
+ intake_rollers_victor_, wrist_victor_, stilts_victor_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -151,6 +338,21 @@
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
+ reader.set_elevator_encoder(make_encoder(2));
+ reader.set_elevator_absolute_pwm(make_unique<frc::DigitalInput>(0));
+ reader.set_elevator_potentiometer(make_unique<frc::AnalogInput>(0));
+
+ reader.set_wrist_encoder(make_encoder(3));
+ reader.set_wrist_absolute_pwm(make_unique<frc::DigitalInput>(1));
+ reader.set_wrist_potentiometer(make_unique<frc::AnalogInput>(2));
+
+ reader.set_intake_encoder(make_encoder(4));
+ reader.set_intake_absolute_pwm(make_unique<frc::DigitalInput>(3));
+
+ reader.set_stilts_encoder(make_encoder(5));
+ reader.set_stilts_absolute_pwm(make_unique<frc::DigitalInput>(2));
+ reader.set_stilts_potentiometer(make_unique<frc::AnalogInput>(3));
+
reader.set_pwm_trigger(make_unique<frc::DigitalInput>(25));
::std::thread reader_thread(::std::ref(reader));
@@ -174,6 +376,21 @@
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+ SuperstructureWriter superstructure_writer;
+ superstructure_writer.set_elevator_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
+ superstructure_writer.set_intake_rollers_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ superstructure_writer.set_intake_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
+ superstructure_writer.set_wrist_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(5)));
+ superstructure_writer.set_stilts_victor(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(6)));
+
+ ::std::thread superstructure_writer_thread(
+ ::std::ref(superstructure_writer));
+
// Wait forever. Not much else to do...
while (true) {
const int r = select(0, nullptr, nullptr, nullptr, nullptr);
@@ -197,6 +414,8 @@
drivetrain_writer.Quit();
drivetrain_writer_thread.join();
+ superstructure_writer.Quit();
+ superstructure_writer_thread.join();
::aos::Cleanup();
}