Set dummy intake constants
Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I9729322dbc5bdc161550a34bfaf3a27714784a48
diff --git a/y2022/BUILD b/y2022/BUILD
index 9c75c26..979d4a7 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -166,6 +166,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_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 98f6511..73e72f3 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,6 +11,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
namespace y2022 {
namespace constants {
@@ -26,6 +27,37 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ // TODO(Yash): Set constants
+ // Intake constants.
+ auto *const intake = &r->intake;
+
+ 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;
+
+ // The number of samples in the moving average filter.
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ // Buffer size for deciding if we are moving.
+ intake->zeroing_constants.moving_buffer_size = 20;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Measured absolute position of the encoder when at zero.
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+
switch (team) {
// A set of constants for tests.
case 1:
diff --git a/y2022/constants.h b/y2022/constants.h
index 050a363..050c5c7 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/intake/intake_plant.h"
namespace y2022 {
namespace constants {
@@ -32,6 +33,19 @@
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.5, // Back Hard
+ .upper_hard = 2.85 + 0.05, // Front Hard
+ .lower = -0.300, // Back Soft
+ .upper = 2.725 // Front Soft
+ };
+ }
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }