Update the sensor ratios
Pull sensor output ratios from the cad. This process found that the
hood was being operated in such a way that the hood encoder was blowing
past 360 degrees and wrapping. So fix that too.
Change-Id: I896458bb81461e7f032a464faa0b78e07d257c10
diff --git a/frc971/constants.h b/frc971/constants.h
index a52add2..debfb55 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -98,6 +98,8 @@
double upper;
double middle() const { return (lower_hard + upper_hard) / 2.0; }
+
+ double range() const { return upper_hard - lower_hard; }
};
} // namespace constants
diff --git a/y2020/BUILD b/y2020/BUILD
index 79dc651..9a233d4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -33,10 +33,12 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
"//y2020/control_loops/superstructure/intake:intake_plants",
"//y2020/control_loops/superstructure/turret:turret_plants",
- "//y2020/control_loops/superstructure/control_panel:control_panel_plants",
"@com_google_absl//absl/base",
],
)
diff --git a/y2020/constants.cc b/y2020/constants.cc
index cc5c1fb..a35798f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -92,6 +92,11 @@
turret_params->zeroing_constants.moving_buffer_size = 20;
turret_params->zeroing_constants.allowable_encoder_error = 0.9;
+ CHECK_LE(hood->range.range(),
+ hood->zeroing_constants.one_revolution_distance);
+ CHECK_LE(intake->range.range(),
+ intake->zeroing_constants.one_revolution_distance);
+
switch (team) {
// A set of constants for tests.
case 1:
diff --git a/y2020/constants.h b/y2020/constants.h
index 16a3e8b..ed58777 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -10,7 +10,9 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
+#include "y2020/control_loops/superstructure/finisher/finisher_plant.h"
#include "y2020/control_loops/superstructure/hood/hood_plant.h"
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/turret/turret_plant.h"
@@ -25,10 +27,10 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() { return (24.0 / 52.0); }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
- control_loops::drivetrain::kHighOutputRatio /
+ control_loops::drivetrain::kHighGearRatio /
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
@@ -36,22 +38,20 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- // TODO(sabina): Update constants
- static constexpr double kHoodEncoderRatio() { return 1.0; }
+ static constexpr double kHoodEncoderRatio() { return 8.0 / 72.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
- return control_loops::superstructure::hood::kFreeSpeed *
+ return control_loops::superstructure::hood::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::hood::kOutputRatio /
- kHoodEncoderRatio() / (2.0 * M_PI) *
- kHoodEncoderCountsPerRevolution();
+ kHoodEncoderRatio() * kHoodEncoderCountsPerRevolution();
}
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- 0.00, // Back Hard
- 0.79, // Front Hard
- 0.14, // Back Soft
- 0.78 // Front Soft
+ -0.01, // Back Hard
+ 0.65, // Front Hard
+ 0.0, // Back Soft
+ 0.64 // Front Soft
};
}
@@ -65,10 +65,9 @@
static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
- return control_loops::superstructure::intake::kFreeSpeed *
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::intake::kOutputRatio /
- kIntakeEncoderRatio() / (2.0 * M_PI) *
- kIntakeEncoderCountsPerRevolution();
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
}
// TODO(sabina): update range
@@ -92,18 +91,16 @@
static constexpr double kTurretEncoderCountsPerRevolution() { return 4096.0; }
static constexpr double kTurretEncoderRatio() {
- return 1.0; // TODO (Kai): Get Gear Ratios when ready
+ return (26.0 / 150.0) * (130.0 / 40.0);
}
static constexpr double kMaxTurretEncoderPulsesPerSecond() {
- return control_loops::superstructure::turret::kFreeSpeed *
+ return control_loops::superstructure::turret::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::turret::kOutputRatio /
- kTurretEncoderRatio() / (2.0 * M_PI) *
- kTurretEncoderCountsPerRevolution();
+ kTurretEncoderRatio() * kTurretEncoderCountsPerRevolution();
}
- // TODO(austin): Figure out the actual constant here.
- static constexpr double kTurretPotRatio() { return 1.0; }
+ static constexpr double kTurretPotRatio() { return (26.0 / 150.0); }
static constexpr ::frc971::constants::Range kTurretRange() {
return ::frc971::constants::Range{
@@ -135,23 +132,40 @@
static constexpr double kControlPanelEncoderRatio() { return (56.0 / 28.0); }
static constexpr double kMaxControlPanelEncoderPulsesPerSecond() {
- return control_loops::superstructure::control_panel::kFreeSpeed *
+ return control_loops::superstructure::control_panel::kFreeSpeed /
+ (2.0 * M_PI) *
control_loops::superstructure::control_panel::kOutputRatio /
- kControlPanelEncoderRatio() / (2.0 * M_PI) *
+ kControlPanelEncoderRatio() *
kControlPanelEncoderCountsPerRevolution();
}
// Shooter
- // TODO: Get actual constants.
static constexpr double kFinisherEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kFinisherEncoderRatio() { return 1.0; }
+ static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+
+ static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
+ return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::finisher::kOutputRatio /
+ kFinisherEncoderRatio() * kFinisherEncoderCountsPerRevolution();
+ }
+
static constexpr double kAcceleratorEncoderCountsPerRevolution() {
return 4096.0;
}
- static constexpr double kAcceleratorEncoderRatio() { return 1.0; }
+ static constexpr double kAcceleratorEncoderRatio() {
+ return (1.2 * 1.2 * 1.2) * (30.0 / 40.0);
+ }
+
+ static constexpr double kMaxAcceleratorEncoderPulsesPerSecond() {
+ return control_loops::superstructure::accelerator::kFreeSpeed /
+ (2.0 * M_PI) *
+ control_loops::superstructure::accelerator::kOutputRatio /
+ kAcceleratorEncoderRatio() *
+ kAcceleratorEncoderCountsPerRevolution();
+ }
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 0457985..630d223 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -273,10 +273,10 @@
loop_writer = control_loop.ControlLoopWriter(
name, flywheels, namespaces=namespace)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio' + params.name, '%f',
+ control_loop.Constant('kOutputRatio', '%f',
flywheels[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed' + params.name, '%f',
+ control_loop.Constant('kFreeSpeed', '%f',
flywheels[0].motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
diff --git a/y2020/control_loops/superstructure/accelerator/BUILD b/y2020/control_loops/superstructure/accelerator/BUILD
index 2c680bf..30fca08 100644
--- a/y2020/control_loops/superstructure/accelerator/BUILD
+++ b/y2020/control_loops/superstructure/accelerator/BUILD
@@ -7,11 +7,28 @@
outs = [
"accelerator_plant.h",
"accelerator_plant.cc",
- "accelerator_integral_plant.h",
- "accelerator_integral_plant.cc",
+ "integral_accelerator_plant.h",
+ "integral_accelerator_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:accelerator) $(OUTS)",
tools = [
"//y2020/control_loops/python:accelerator",
],
)
+
+cc_library(
+ name = "accelerator_plants",
+ srcs = [
+ "accelerator_plant.cc",
+ "integral_accelerator_plant.cc",
+ ],
+ hdrs = [
+ "accelerator_plant.h",
+ "integral_accelerator_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/finisher/BUILD b/y2020/control_loops/superstructure/finisher/BUILD
index b9bfc4f..2009886 100644
--- a/y2020/control_loops/superstructure/finisher/BUILD
+++ b/y2020/control_loops/superstructure/finisher/BUILD
@@ -7,11 +7,28 @@
outs = [
"finisher_plant.h",
"finisher_plant.cc",
- "finisher_integral_plant.h",
- "finisher_integral_plant.cc",
+ "integral_finisher_plant.h",
+ "integral_finisher_plant.cc",
],
cmd = "$(location //y2020/control_loops/python:finisher) $(OUTS)",
tools = [
"//y2020/control_loops/python:finisher",
],
)
+
+cc_library(
+ name = "finisher_plants",
+ srcs = [
+ "finisher_plant.cc",
+ "integral_finisher_plant.cc",
+ ],
+ hdrs = [
+ "finisher_plant.h",
+ "integral_finisher_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/shooter/BUILD b/y2020/control_loops/superstructure/shooter/BUILD
index d63231b..fe8a29a 100644
--- a/y2020/control_loops/superstructure/shooter/BUILD
+++ b/y2020/control_loops/superstructure/shooter/BUILD
@@ -4,18 +4,6 @@
cc_library(
name = "shooter_plants",
- srcs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.cc",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.cc",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.cc",
- ],
- hdrs = [
- "//y2020/control_loops/superstructure/accelerator:accelerator_integral_plant.h",
- "//y2020/control_loops/superstructure/accelerator:accelerator_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_integral_plant.h",
- "//y2020/control_loops/superstructure/finisher:finisher_plant.h",
- ],
deps = [
"//frc971/control_loops:state_feedback_loop",
],
@@ -49,10 +37,11 @@
"flywheel_controller.h",
],
deps = [
- ":shooter_plants",
"//aos/controls:control_loop",
"//frc971/control_loops:profiled_subsystem",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
+ "//y2020/control_loops/superstructure/accelerator:accelerator_plants",
+ "//y2020/control_loops/superstructure/finisher:finisher_plants",
],
)
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 20cd681..0d162c4 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -6,8 +6,8 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2020/control_loops/superstructure/accelerator/accelerator_integral_plant.h"
-#include "y2020/control_loops/superstructure/finisher/finisher_integral_plant.h"
+#include "y2020/control_loops/superstructure/accelerator/integral_accelerator_plant.h"
+#include "y2020/control_loops/superstructure/finisher/integral_finisher_plant.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 37ee0bb..fe69479 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -435,8 +435,10 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
+ superstructure_plant_.InitializeHoodPosition(
+ constants::Values::kHoodRange().middle());
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().middle());
WaitUntilZeroed();
{
@@ -444,12 +446,12 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kHoodRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.2,
+ *builder.fbb(), constants::Values::kIntakeRange().upper,
CreateProfileParameters(*builder.fbb(), 1.0, 0.2));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
@@ -569,9 +571,6 @@
SetEnabled(true);
// Set a reasonable goal.
- superstructure_plant_.InitializeHoodPosition(0.7);
- superstructure_plant_.InitializeIntakePosition(0.7);
-
WaitUntilZeroed();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 023a242..e8f7637 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -21,7 +21,7 @@
finisher:FlywheelControllerStatus;
// The subsystem to accelerate the ball before the finisher
- // Velocity is the slowest (lowest) wheel
+ // Velocity is the fastest (top) wheel
accelerator_left:FlywheelControllerStatus;
accelerator_right:FlywheelControllerStatus;
}
@@ -33,7 +33,7 @@
// If true, we have aborted. This is if one or all subsystem estops.
estopped:bool;
- //Subsystem status.
+ // Subsystem status.
hood:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
intake:frc971.control_loops.AbsoluteEncoderProfiledJointStatus;
turret:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index ab8b967..a1083eb 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -66,18 +66,6 @@
// DMA stuff and then removing the * 2.0 in *_translate.
// The low bit is direction.
-// TODO(brian): Use ::std::max instead once we have C++14 so that can be
-// constexpr.
-template <typename T>
-constexpr T max(T a, T b) {
- return (a > b) ? a : b;
-}
-
-template <typename T, typename... Rest>
-constexpr T max(T a, T b, T c, Rest... rest) {
- return max(max(a, b), c, rest...);
-}
-
double drivetrain_translate(int32_t in) {
return ((static_cast<double>(in) /
Values::kDrivetrainEncoderCountsPerRevolution()) *
@@ -98,15 +86,19 @@
(10.0 /*turns*/ / 5.0 /*volts*/) * (2 * M_PI /*radians*/);
}
-// TODO(Ravago): check with constants which are fast.
constexpr double kMaxFastEncoderPulsesPerSecond =
- Values::kMaxDrivetrainEncoderPulsesPerSecond();
-static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ std::max({Values::kMaxControlPanelEncoderPulsesPerSecond(),
+ Values::kMaxFinisherEncoderPulsesPerSecond(),
+ Values::kMaxAcceleratorEncoderPulsesPerSecond()});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1000000.0,
"fast encoders are too fast");
constexpr double kMaxMediumEncoderPulsesPerSecond =
- kMaxFastEncoderPulsesPerSecond;
+ std::max({Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+ Values::kMaxHoodEncoderPulsesPerSecond(),
+ Values::kMaxIntakeEncoderPulsesPerSecond(),
+ Values::kMaxTurretEncoderPulsesPerSecond()});
-static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000,
+static_assert(kMaxMediumEncoderPulsesPerSecond <= 400000.0,
"medium encoders are too fast");
} // namespace
@@ -193,12 +185,13 @@
// Control Panel
void set_control_panel_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- medium_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
control_panel_encoder_ = ::std::move(encoder);
}
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ medium_encoder_filter_.Add(sensor.get());
autonomous_modes_.at(i) = ::std::move(sensor);
}