Fill out dummy 3rd robot constants
Waiting until we have the actual hardware to fill out the real constants.
Signed-off-by: Logan Isaacson <100030671@mvla.net>
Change-Id: Ia0f5fc1cefdb8e2509bedf5f727db15ef96db522
diff --git a/y2022_bot3/BUILD b/y2022_bot3/BUILD
index 8af6882..8ccae67 100644
--- a/y2022_bot3/BUILD
+++ b/y2022_bot3/BUILD
@@ -122,6 +122,8 @@
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/shooter_interpolation:interpolation",
"//y2022_bot3/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022_bot3/control_loops/superstructure/climber:climber_plants",
+ "//y2022_bot3/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2022_bot3/constants.cc b/y2022_bot3/constants.cc
index 84e6a0d..2ecae68 100644
--- a/y2022_bot3/constants.cc
+++ b/y2022_bot3/constants.cc
@@ -11,6 +11,8 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2022_bot3/control_loops/superstructure/climber/integral_climber_plant.h"
+#include "y2022_bot3/control_loops/superstructure/intake/integral_intake_plant.h"
namespace y2022_bot3 {
namespace constants {
@@ -30,18 +32,104 @@
Values r;
+ auto *const intake = &r.intake;
+ auto *const climber_left = &r.climber_left;
+ auto *const climber_right = &r.climber_right;
+
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake_params;
+
+ intake_params.zeroing_voltage = 3.0;
+ intake_params.operating_voltage = 12.0;
+ intake_params.zeroing_profile_params = {0.5, 3.0};
+ intake_params.default_profile_params = {6.0, 30.0};
+ intake_params.range = Values::kIntakeRange();
+ intake_params.make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+ intake_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ intake_params.zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+ intake_params.zeroing_constants.zeroing_threshold = 0.0005;
+ intake_params.zeroing_constants.moving_buffer_size = 20;
+ intake_params.zeroing_constants.allowable_encoder_error = 0.9;
+ intake_params.zeroing_constants.measured_absolute_position = 0.0;
+
+ intake->subsystem_params = intake_params;
+
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ climber_params;
+
+ climber_params.zeroing_voltage = 3.0;
+ climber_params.operating_voltage = 12.0;
+ climber_params.zeroing_profile_params = {0.5, 0.1};
+ climber_params.default_profile_params = {5.0, 1.0};
+ climber_params.range = Values::kClimberRange();
+ climber_params.make_integral_loop =
+ control_loops::superstructure::climber::MakeIntegralClimberLoop;
+ climber_params.zeroing_constants.average_filter_size =
+ Values::kZeroingSampleSize;
+ climber_params.zeroing_constants.one_revolution_distance =
+ constants::Values::kClimberEncoderRatio() *
+ constants::Values::kClimberEncoderMetersPerRevolution();
+ climber_params.zeroing_constants.zeroing_threshold = 0.0005;
+ climber_params.zeroing_constants.moving_buffer_size = 20;
+ climber_params.zeroing_constants.allowable_encoder_error = 0.9;
+ climber_params.zeroing_constants.measured_absolute_position = 0.0;
+
+ climber_left->subsystem_params = climber_params;
+ climber_right->subsystem_params = climber_params;
+
switch (team) {
// A set of constants for tests.
case 1:
+ intake->potentiometer_offset = 0.0;
+ intake->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
+ climber_left->potentiometer_offset = 0.0;
+ climber_left->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ climber_right->potentiometer_offset = 0.0;
+ climber_right->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
break;
case kCompTeamNumber:
+ intake->potentiometer_offset = 0.0;
+ intake->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
+ climber_left->potentiometer_offset = 0.0;
+ climber_left->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ climber_right->potentiometer_offset = 0.0;
+ climber_right->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
break;
case kPracticeTeamNumber:
+ intake->potentiometer_offset = 0.0;
+ intake->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
+ climber_left->potentiometer_offset = 0.0;
+ climber_left->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ climber_right->potentiometer_offset = 0.0;
+ climber_right->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
break;
case kCodingRobotTeamNumber:
+ intake->potentiometer_offset = 0.0;
+ intake->subsystem_params.zeroing_constants.measured_absolute_position =
+ 0.0;
+ climber_left->potentiometer_offset = 0.0;
+ climber_left->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
+ climber_right->potentiometer_offset = 0.0;
+ climber_right->subsystem_params.zeroing_constants
+ .measured_absolute_position = 0.0;
break;
default:
diff --git a/y2022_bot3/constants.h b/y2022_bot3/constants.h
index 7f5b623..c1638a2 100644
--- a/y2022_bot3/constants.h
+++ b/y2022_bot3/constants.h
@@ -10,6 +10,8 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
#include "y2022_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022_bot3/control_loops/superstructure/climber/climber_plant.h"
+#include "y2022_bot3/control_loops/superstructure/intake/intake_plant.h"
using ::frc971::shooter_interpolation::InterpolationTable;
@@ -45,10 +47,67 @@
double potentiometer_offset;
};
- // Intake rollers
static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
+ // Intake
+ // TODO (Logan): Constants need to be tuned
+ static constexpr double kIntakeEncoderCountsPerRevolution() { return 4096.0; }
+
+ static constexpr double kIntakeEncoderRatio() {
+ return (16.0 / 64.0) * (18.0 / 62.0);
+ }
+
+ static constexpr double kIntakePotRatio() { return 16.0 / 64.0; }
+
+ static constexpr double kMaxIntakeEncoderPulsesPerSecond() {
+ return control_loops::superstructure::intake::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::intake::kOutputRatio /
+ kIntakeEncoderRatio() * kIntakeEncoderCountsPerRevolution();
+ }
+ PotAndAbsEncoderConstants intake;
+
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.85, // Back Hard
+ .upper_hard = 1.85, // Front Hard
+ .lower = -0.400, // Back Soft
+ .upper = 1.57 // Front Soft
+ };
+ }
+
+ // Climber
+ // TODO (Logan): Constants need to be tuned
+ static constexpr double kClimberEncoderCountsPerRevolution() {
+ return 4096.0;
+ }
+
+ static constexpr double kClimberEncoderRatio() {
+ return (16.0 / 64.0) * (18.0 / 62.0);
+ }
+
+ static constexpr double kClimberEncoderMetersPerRevolution() { return 1.0; }
+
+ static constexpr double kClimberPotMetersPerRevolution() { return 0.125; }
+
+ static constexpr double kClimberPotRatio() { return 16.0 / 64.0; }
+
+ static constexpr double kMaxClimberEncoderPulsesPerSecond() {
+ return control_loops::superstructure::climber::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::climber::kOutputRatio /
+ kClimberEncoderRatio() * kClimberEncoderCountsPerRevolution();
+ }
+ PotAndAbsEncoderConstants climber_left;
+ PotAndAbsEncoderConstants climber_right;
+
+ static constexpr ::frc971::constants::Range kClimberRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.01, // Back Hard
+ .upper_hard = 0.59, // Front Hard
+ .lower = 0.003, // Back Soft
+ .upper = 0.555 // Front Soft
+ };
+ }
};
// Creates and returns a Values instance for the constants.