Remove Climbing Limits when Enabled and Add the Real OwO Binding (#165)

* Add ability to remove limits on climbing in shuffleboard

* Slow down amp arm speed

* Make auto factory generic

* bind the real OwO

* linter

Author: having11

src/main/cpp/Robot.cpp

@@ -38,7 +38,10 @@ void Robot::RobotInit() {
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); m_container.Periodic(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+  m_container.Periodic();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You

src/main/cpp/RobotContainer.cpp

@@ -99,6 +99,9 @@ void RobotContainer::RegisterAutos() {
           .AndThen(IntakingCommands::Intake(&m_intake, &m_scoring)));
 
   m_autoChooser.Initialize();
+  m_ignoreLimitChooser.SetDefaultOption("Don't Ignore", false);
+  m_ignoreLimitChooser.AddOption("Ignore", true);
+  frc::SmartDashboard::PutData("Climb Limits", &m_ignoreLimitChooser);
 }
 
 void RobotContainer::ConfigureButtonBindings() {
@@ -268,22 +271,34 @@ void RobotContainer::ConfigureAutoBindings() {
                                         }).ToPtr());
 
   // Maps to 4 on keyboard
-  m_operatorController.Button(7).OnTrue(frc2::InstantCommand([this] {
-                                          if (!m_state.m_active) {
-                                            m_state.m_currentState =
-                                                RobotState::AutoSequenceAmp;
-                                            m_state.SetDesiredState();
-                                          }
-                                        }).ToPtr());
+  m_operatorController.Button(7).OnTrue(
+      m_leds.OwOFace()
+          .AndThen(m_leds.Error())
+          .AndThen(m_rightClimb.MoveToPositionAbsolute(10_in).AlongWith(
+              m_leftClimb.MoveToPositionAbsolute(10_in)))
+          .AndThen(m_arm.MoveToPositionAbsolute(ArmConstants::kAmpRotation))
+          .AndThen(frc2::WaitCommand(8_s).ToPtr())
+          .AndThen(m_rightClimb.MoveToPositionAbsolute(1_in).AlongWith(
+              m_leftClimb.MoveToPositionAbsolute(1_in)))
+          .AndThen(m_arm.MoveToPositionAbsolute(ArmConstants::kHomeRotation))
+          .AndThen(m_leds.Idling()));
+
+  // .OnTrue(frc2::InstantCommand([this] {
+  //                                         if (!m_state.m_active) {
+  //                                           m_state.m_currentState =
+  //                                               RobotState::AutoSequenceAmp;
+  //                                           m_state.SetDesiredState();
+  //                                         }
+  //                                       }).ToPtr());
 
   // Maps to 5 on keyboard
   m_operatorController.Button(8).OnTrue(
       frc2::InstantCommand([this] { m_drive.ZeroHeading(); }).ToPtr());
 
   // Maps to 6 on keyboard
   m_operatorController.Button(9).OnTrue(
-      m_rightClimb.MoveToPositionAbsolute(18_in).AlongWith(
-          m_leftClimb.MoveToPositionAbsolute(18_in)));
+      m_rightClimb.MoveToPositionAbsolute(20_in).AlongWith(
+          m_leftClimb.MoveToPositionAbsolute(20_in)));
 
   // Maps to 1 on keyboard
   m_operatorController.Button(10).OnTrue(frc2::InstantCommand([this] {
@@ -333,7 +348,7 @@ void RobotContainer::ConfigureAutoBindings() {
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
   auto autoType = m_autoChooser.GetSelectedValue();
-  autoCommand = AutoFactory::GetAuto(autoType);
+  autoCommand = m_autoFactory.GetAuto(autoType);
 
   return autoCommand.get();
 }
@@ -383,6 +398,8 @@ units::degree_t RobotContainer::CurveRotation(double sensitivity, double val,
 }
 
 void RobotContainer::Periodic() {
+  frc::SmartDashboard::PutBoolean("Disable Climb Limits", m_ignoreClimbLimits);
+
   frc::SmartDashboard::PutData("Robot2d", &m_mech);
 
   frc::SmartDashboard::PutBoolean("TURN TO POSE AT GOAL",
@@ -476,6 +493,8 @@ void RobotContainer::Periodic() {
       m_shouldAim = false;
     }
   }
+
+  m_ignoreClimbLimits = m_ignoreLimitChooser.GetSelected();
 }
 
 std::optional<frc::Rotation2d> RobotContainer::GetRotationTargetOverride() {

src/main/cpp/subsystems/LedSubsystem.cpp

@@ -10,9 +10,12 @@ using namespace ConnectorX;
 using namespace LEDConstants;
 
 void LedSubsystem::Periodic() {
-  if (abs(m_accel.GetX()) >= kAccelThreshold ||
-      abs(m_accel.GetY()) >= kAccelThreshold) {
-    showFace(EyePattern::Surprised);
+  // if (abs(m_accel.GetX()) >= kAccelThreshold ||
+  //     abs(m_accel.GetY()) >= kAccelThreshold) {
+  //   showFace(EyePattern::Surprised);
+  // }
+  if (m_accel.GetY() <= -kAccelThreshold) {
+    showFace(EyePattern::OwO);
   }
 }
 
@@ -368,6 +371,14 @@ frc2::CommandPtr LedSubsystem::AmogusFace() {
       .ToPtr();
 }
 
+frc2::CommandPtr LedSubsystem::OwOFace() {
+  return frc2::InstantCommand([this] {
+           ConsoleWriter.logInfo("LedSubsystem", "Setting LEDs to %s", "OwO");
+           showFace(EyePattern::OwO);
+         })
+      .ToPtr();
+}
+
 frc2::CommandPtr LedSubsystem::setZoneColorPattern(LedZone zone, LedPort port,
                                                    frc::Color8Bit color,
                                                    PatternType pattern,

src/main/include/RobotContainer.h

@@ -19,6 +19,7 @@
 #include <pathplanner/lib/commands/PathPlannerAuto.h>
 
 #include "Constants.h"
+#include "autos/AutoFactory.h"
 #include "subsystems/ArmSubsystem.h"
 #include "subsystems/DriveSubsystem.h"
 #include "subsystems/IntakeSubsystem.h"
@@ -73,22 +74,32 @@ class RobotContainer {
 
   LedSubsystem m_leds;
 
+  bool m_ignoreClimbLimits = false;
+
   // The chooser for the autonomous routines
   AutoChooser<AutoConstants::AutoType> m_autoChooser{
       AutoConstants::kChooserEntries, AutoConstants::kChooserGroups,
       "Auto Selector"};
 
+  AutoFactory<AutoConstants::AutoType> m_autoFactory{AutoConstants::kPpAutos};
+
+  frc::SendableChooser<bool> m_ignoreLimitChooser;
+
 #ifdef TEST_SWERVE_BOT
 
 #endif
 
 #ifndef TEST_SWERVE_BOT
-  LeftClimbSubsystem m_leftClimb{(frc::MechanismObject2d*)m_mech.GetRoot(
-      "Climber Left", MechanismConstants::kClimberLeftX,
-      MechanismConstants::kClimberLeftY)};
-  RightClimbSubsystem m_rightClimb{(frc::MechanismObject2d*)m_mech.GetRoot(
-      "Climber Right", MechanismConstants::kClimberRightX,
-      MechanismConstants::kClimberRightY)};
+  LeftClimbSubsystem m_leftClimb{
+      [this] { return m_ignoreClimbLimits; },
+      (frc::MechanismObject2d*)m_mech.GetRoot(
+          "Climber Left", MechanismConstants::kClimberLeftX,
+          MechanismConstants::kClimberLeftY)};
+  RightClimbSubsystem m_rightClimb{
+      [this] { return m_ignoreClimbLimits; },
+      (frc::MechanismObject2d*)m_mech.GetRoot(
+          "Climber Right", MechanismConstants::kClimberRightX,
+          MechanismConstants::kClimberRightY)};
   frc::MechanismRoot2d* armRoot = m_mech.GetRoot(
       "Arm Root", MechanismConstants::kArmRootX, MechanismConstants::kArmRootY);
   frc::MechanismLigament2d* armPost = armRoot->Append<frc::MechanismLigament2d>(

src/main/include/autos/AutoFactory.h

@@ -9,77 +9,65 @@
 
 #include <filesystem>
 #include <functional>
+#include <map>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "Constants.h"
 #include "utils/ConsoleLogger.h"
 
-namespace AutoFactory {
-frc2::CommandPtr GetEmptyCommand() { return frc2::WaitCommand(15_s).ToPtr(); }
+template <typename T>
+class AutoFactory {
+ public:
+  explicit AutoFactory(const std::map<T, std::string>& autos)
+      : m_autos{autos} {}
 
-bool AutoFileExists(std::string fileName) {
-  const std::string filePath = frc::filesystem::GetDeployDirectory() +
-                               "/pathplanner/autos/" + fileName + ".auto";
+ private:
+  const std::map<T, std::string>& m_autos;
 
-  std::error_code error_code;
-  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
-      wpi::MemoryBuffer::GetFile(filePath, error_code);
-
-  if (fileBuffer == nullptr || error_code) {
-    return false;
+  inline frc2::CommandPtr GetEmptyCommand() {
+    return frc2::WaitCommand(15_s).ToPtr();
   }
 
-  return true;
-}
+  bool AutoFileExists(const std::string fileName) {
+    const std::string filePath = frc::filesystem::GetDeployDirectory() +
+                                 "/pathplanner/autos/" + fileName + ".auto";
 
-frc2::CommandPtr PathPlannerPathFromName(std::string autoName) {
-  if (!AutoFileExists(autoName)) {
-    ConsoleWriter.logError("Auto Factory",
-                           "AUTO '%s' DOES NOT EXIST HELP US EVAN",
-                           autoName.c_str());
-    return GetEmptyCommand();
-  }
-  return pathplanner::PathPlannerAuto(autoName).ToPtr();
-}
+    std::error_code error_code;
+    std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+        wpi::MemoryBuffer::GetFile(filePath, error_code);
 
-frc2::CommandPtr GetAuto(AutoConstants::AutoType type) {
-  using namespace AutoConstants;
+    if (fileBuffer == nullptr || error_code) {
+      return false;
+    }
+
+    return true;
+  }
 
-  switch (type) {
-    case AutoType::EmptyAuto:
+  frc2::CommandPtr PathPlannerPathFromName(const std::string autoName) {
+    if (!AutoFileExists(autoName)) {
+      ConsoleWriter.logError("Auto Factory",
+                             "AUTO '%s' DOES NOT EXIST HELP US EVAN",
+                             autoName.c_str());
       return GetEmptyCommand();
-    case AutoType::FourNoteAuto:
-      return PathPlannerPathFromName("4 Note Auto");
-    case AutoType::PlaceAndLeave:
-      return PathPlannerPathFromName("Place and leave");
-    case AutoType::ThreeNoteAuto:
-      return PathPlannerPathFromName("3 Note Auto");
-    case AutoType::TwoNoteAmpSide:
-      return PathPlannerPathFromName("2 Note Amp Side");
-    case AutoType::TwoNoteCenter:
-      return PathPlannerPathFromName("2 Note Center Note 3");
-    case AutoType::TwoNoteSource:
-      return PathPlannerPathFromName("2 Note Source Side");
-    case AutoType::ThreeNoteCenter:
-      return PathPlannerPathFromName("3 Note Center Note 3 + 4");
-    case AutoType::TwoNoteAuto:
-      return PathPlannerPathFromName("2 Note Auto");
-    case AutoType::TwoNoteInAmp:
-      return PathPlannerPathFromName("2 In Amp");
-    case AutoType::TwoInSpeakerTwoInAmp:
-      return PathPlannerPathFromName("2 In Speaker 2 In Amp");
-    case AutoType::ThreeNoteSimp:
-      return PathPlannerPathFromName("3 Note Simp");
-    case AutoType::LeaveWing:
-      return PathPlannerPathFromName("Leave Wing");
-    default:
+    }
+    return pathplanner::PathPlannerAuto(autoName).ToPtr();
+  }
+
+ public:
+  frc2::CommandPtr GetAuto(T type) {
+    using namespace AutoConstants;
+
+    if (!m_autos.contains(type)) {
       ConsoleWriter.logWarning(
           "Auto Factory",
           "Auto type %d does not exist, defaulting to empty "
           "auto",
           static_cast<int>(type));
       return GetEmptyCommand();
+    }
+
+    return PathPlannerPathFromName(m_autos.at(type));
   }
-}
-}  // namespace AutoFactory
+};

src/main/include/constants/AutoConstants.h

@@ -56,7 +56,7 @@ static const std::vector<AutoChooser<AutoType>::AutoChooserEntry>
         {{AutoType::ThreeNoteAuto, "3 Note Auto"},
          {"3 notes", "close", "decent", "center"}},
         {{AutoType::ThreeNoteSimp, "3 Note Simp Edition"},
-        {"3 notes", "close", "decent", "center"}},
+         {"3 notes", "close", "decent", "center"}},
         {{AutoType::TwoNoteCenter, "2 Note Center Note Under Stage"},
          {"2 notes", "far", "high", "center"}},
         {{AutoType::LeaveWing, "Leave Wing"},
@@ -83,6 +83,21 @@ static const std::vector<AutoChooser<AutoType>::AutoChooserSelectorGroup>
         {"Motion", {"amp", "center", "source", "complex"}},
     };
 
+static const std::map<AutoType, std::string> kPpAutos = {
+    {AutoType::FourNoteAuto, "4 Note Auto"},
+    {AutoType::ThreeNoteAuto, "3 Note Auto"},
+    {AutoType::ThreeNoteSimp, "3 Note Simp"},
+    {AutoType::TwoNoteCenter, "2 Note Center Note 3"},
+    {AutoType::LeaveWing, "Leave Wing"},
+    {AutoType::PlaceAndLeave, "Place and leave"},
+    {AutoType::TwoNoteAmpSide, "2 Note Amp Side"},
+    {AutoType::TwoNoteSource, "2 Note Center Note 3"},
+    {AutoType::ThreeNoteCenter, "3 Note Center Note 3 + 4"},
+    {AutoType::TwoNoteAuto, "2 Note Auto"},
+    {AutoType::TwoNoteInAmp, "2 In Amp"},
+    {AutoType::TwoInSpeakerTwoInAmp, "2 In Speaker 2 In Amp"},
+};
+
 extern const frc::TrapezoidProfile<units::radians>::Constraints
     kThetaControllerConstraints;
 
@@ -99,9 +114,9 @@ const std::string kLedFunniName = "LedFunni";
 
 const auto PathConfig = pathplanner::HolonomicPathFollowerConfig(
     pathplanner::PIDConstants(1.5, 0.0,
-                              0.0),             // Translation PID constants
+                              0.0),          // Translation PID constants
     pathplanner::PIDConstants(2, 0.0, 0.0),  // Rotation PID constants
-    3.0_mps,                                    // Max module speed, in m/s
+    3.0_mps,                                 // Max module speed, in m/s
 #ifdef TEST_SWERVE_BOT
     0.4579874_m,  // Drive base radius in meters. Distance from robot center to
 #endif

src/main/include/constants/LEDConstants.h

@@ -6,5 +6,5 @@
 namespace LEDConstants {
 constexpr uint8_t kLedAddress = 23;
 constexpr units::second_t kConnectorXDelay = 0.002_s;
-constexpr double kAccelThreshold = 7;
+constexpr double kAccelThreshold = 1;
 }

src/main/include/constants/ScoringConstants.h

@@ -19,8 +19,8 @@ constexpr double kSubwooferVectorSpeed = 1;
 constexpr double kFeedPodiumVectorSpeed = -1;
 
 // These need to be different
-constexpr double kAmpLowerSpeed = -0.254 * 1.9;  // .264
-constexpr double kAmpUpperSpeed = -0.168 * 1.4;  // .278
+constexpr double kAmpLowerSpeed = -0.254 * 1.7;  // .264
+constexpr double kAmpUpperSpeed = -0.168 * 1.3;  // .278
 
 // These should match
 constexpr double kSpeakerLowerSpeed = 1;

src/main/include/moduledrivers/ConnectorX.h

@@ -244,7 +244,7 @@ enum class PatternType {
   SetAll = 1,
   Blink = 2,
   RGBFade = 3,
-  HackerMode = 4, // No worky
+  HackerMode = 4,  // No worky
   Breathe = 5,
   SineRoll = 6,
   Chase = 7,
@@ -253,6 +253,7 @@ enum class PatternType {
   BlinkingEyes = 10,
   SurprisedEyes = 11,
   Amogus = 12,
+  OwOEyes = 14,
 };
 
 enum class PinMode {

src/main/include/subsystems/ArmSubsystem.h

@@ -37,7 +37,9 @@ class ArmSubsystem : public RotationalSingleAxisSubsystem<
              // Mechanism2d
              ArmConstants::kArmMechanism,
              // Conversion Function
-             std::nullopt},
+             std::nullopt,
+
+             [] { return false; }},
             ArmConstants::kArmLength,
             node} {
     m_SpinnyBoi.SetIdleMode(rev::CANSparkBase::IdleMode::kBrake);