commit a47ae66ea2dffc54809703a068be9945b3ec249e
author Thiago Monteiro <oculusdev124@gmail.com> Sat Aug 27 16:05:54 2022 -0700
committer Thiago Monteiro <oculusdev124@gmail.com> Sat Aug 27 16:05:54 2022 -0700
tree 71477243a0a60e16b75e9d5ec2f13e6b88bd6294
parent 7abc7db68d71d0f62ea04ee10a43d2167defb880

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

y2022_bot3/constants.cc

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: