Added intake joint constants
Change-Id: I4ed4d27fb334c9cd5e5a8959a0bb708252bbc05d
diff --git a/y2020/BUILD b/y2020/BUILD
index 1d281bb..d2e6082 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -34,6 +34,7 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/hood:hood_plants",
+ "//y2020/control_loops/superstructure/intake:intake_plants",
"@com_google_absl//absl/base",
],
)
diff --git a/y2020/constants.cc b/y2020/constants.cc
index a16b2f1..aee797f 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -12,6 +12,7 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
+#include "y2020/control_loops/superstructure/intake/integral_intake_plant.h"
#include "y2020/control_loops/superstructure/hood/integral_hood_plant.h"
@@ -48,6 +49,26 @@
hood->zeroing_constants.allowable_encoder_error = 0.9;
hood->zeroing_constants.middle_position = Values::kHoodRange().middle();
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator> *const intake =
+ &r->intake;
+
+ // Intake constants.
+ intake->zeroing_voltage = 3.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 30.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ intake->zeroing_constants.moving_buffer_size = 20;
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+ intake->zeroing_constants.middle_position = Values::kIntakeRange().middle();
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -55,14 +76,17 @@
case kCompTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
case kPracticeTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
case kCodingRobotTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.0;
break;
default:
diff --git a/y2020/constants.h b/y2020/constants.h
index 47b8543..69e7f28 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -11,6 +11,7 @@
#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/hood/hood_plant.h"
+#include "y2020/control_loops/superstructure/intake/intake_plant.h"
namespace y2020 {
namespace constants {
@@ -33,7 +34,7 @@
// Hood
static constexpr double kHoodEncoderCountsPerRevolution() { return 4096.0; }
- //TODO(sabina): Update constants
+ // TODO(sabina): Update constants
static constexpr double kHoodEncoderRatio() { return 1.0; }
static constexpr double kMaxHoodEncoderPulsesPerSecond() {
@@ -52,17 +53,42 @@
};
}
+ // Intake
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kIntakeEncoderRatio() { return (16.0 / 32.0); }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() / (2.0 * M_PI) *
+ kIntakeEncoderCountsPerRevolution();
+ }
+
+ // TODO(sabina): update range
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ -1, // Back Hard
+ 1, // Front Hard
+ -0.95, // Back Soft
+ 0.95 // Front Soft
+ };
+ }
+
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
hood;
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ intake;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
// returns a reference to it.
const Values &GetValues();
-// Creates Values instances for each team number it is called with and returns
-// them.
+// Creates Values instances for each team number it is called with and
+// returns them.
const Values &GetValuesForTeam(uint16_t team_number);
} // namespace constants