commit 8507c9fe588e5f21cf06fbeb434677e4988c8fab
author Austin Schuh <austin.linux@gmail.com> Sun Mar 13 18:08:28 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Mar 13 18:11:56 2022 -0700
tree 00a0766b75774b65aba82a93585a3906d55d987d
parent b7e3c248231fee02be1c0d17610664407edea29a

Fix all the climber signs and make them all work

Now that the HW exists, let's do it.  Zero is all the way down against
the hard stop.

Change-Id: I8661c25e6ce8badab26a6d1b642bcc9d754d4878
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2022/constants.cc

diff --git a/y2022/constants.cc b/y2022/constants.cc
index 2e7c5a9..0b4a591 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -88,7 +88,7 @@
   climber->subsystem_params.zeroing_voltage = 3.0;
   climber->subsystem_params.operating_voltage = 12.0;
   climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
-  climber->subsystem_params.default_profile_params = {6.0, 1.0};
+  climber->subsystem_params.default_profile_params = {5.0, 1.0};
   climber->subsystem_params.range = Values::kClimberRange();
   climber->subsystem_params.make_integral_loop =
       control_loops::superstructure::climber::MakeIntegralClimberLoop;
@@ -162,7 +162,7 @@
       break;
 
     case kCompTeamNumber:
-      climber->potentiometer_offset = 0.0;
+      climber->potentiometer_offset = -0.0463847608752;
 
       intake_front->potentiometer_offset = 2.79628370453323;
       intake_front->subsystem_params.zeroing_constants

y2022/constants.h

diff --git a/y2022/constants.h b/y2022/constants.h
index 21ea7a4..2c0c5a1 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -45,7 +45,7 @@
   // Climber
   static constexpr ::frc971::constants::Range kClimberRange() {
     return ::frc971::constants::Range{
-        .lower_hard = -0.01, .upper_hard = 0.6, .lower = 0.0, .upper = 0.5};
+        .lower_hard = -0.01, .upper_hard = 0.55, .lower = 0.005, .upper = 0.48};
   }
   static constexpr double kClimberPotMetersPerRevolution() {
     return 22 * 0.25 * 0.0254;

y2022/joystick_reader.cc

diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 8423f47..f73058b 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -57,6 +57,8 @@
 const ButtonLocation kTurret(4, 15);
 const ButtonLocation kAutoAim(4, 2);
 
+const ButtonLocation kClimberExtend(4, 6);
+
 const ButtonLocation kIntakeFrontOut(4, 10);
 const ButtonLocation kIntakeBackOut(4, 9);
 const ButtonLocation kSpit(3, 3);
@@ -169,6 +171,8 @@
 
     double turret_pos = 0.0;
 
+    double climber_position = 0.01;
+
     double catapult_pos = 0.03;
     double catapult_speed = 18.0;
     double catapult_return_pos = 0.0;
@@ -185,6 +189,12 @@
       BlueResetLocalizer();
     }
 
+    if (data.IsPressed(kClimberExtend)) {
+      climber_position = 0.50;
+    } else {
+      climber_position = 0.01;
+    }
+
     if (data.IsPressed(kTurret)) {
       if (setpoint_fetcher_.get()) {
         turret_pos = setpoint_fetcher_->turret();
@@ -273,6 +283,11 @@
                   *builder.fbb(), catapult_return_pos,
                   frc971::CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
 
+      flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+          climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+              *builder.fbb(), climber_position,
+              frc971::CreateProfileParameters(*builder.fbb(), 1.0, 5.0));
+
       superstructure::CatapultGoal::Builder catapult_builder =
           builder.MakeBuilder<superstructure::CatapultGoal>();
       catapult_builder.add_return_position(catapult_return_offset);
@@ -287,6 +302,7 @@
       superstructure_goal_builder.add_intake_front(intake_front_offset);
       superstructure_goal_builder.add_intake_back(intake_back_offset);
       superstructure_goal_builder.add_turret(turret_offset);
+      superstructure_goal_builder.add_climber(climber_offset);
       superstructure_goal_builder.add_catapult(catapult_offset);
       superstructure_goal_builder.add_fire(fire);
 

y2022/wpilib_interface.cc

diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 69883b4..f23d3e2 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -79,7 +79,7 @@
 
 double climber_pot_translate(double voltage) {
   return voltage * Values::kClimberPotRatio() *
-         (10.0 /*turns*/ / 5.0 /*volts*/) *
+         (5.0 /*turns*/ / 5.0 /*volts*/) *
          Values::kClimberPotMetersPerRevolution();
 }
 
@@ -510,7 +510,7 @@
   }
 
   void Write(const superstructure::Output &output) override {
-    WritePwm(output.climber_voltage(), climber_falcon_.get());
+    WritePwm(-output.climber_voltage(), climber_falcon_.get());
 
     WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
     WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());