Feature/week zero changes

.vscode/settings.json

@@ -96,6 +96,20 @@
     "variant": "cpp",
     "xmemory": "cpp",
     "xstring": "cpp",
-    "xutility": "cpp"
+    "xutility": "cpp",
+    "charconv": "cpp",
+    "format": "cpp",
+    "ios": "cpp",
+    "locale": "cpp",
+    "xfacet": "cpp",
+    "xhash": "cpp",
+    "xiosbase": "cpp",
+    "xlocale": "cpp",
+    "xlocbuf": "cpp",
+    "xlocinfo": "cpp",
+    "xlocmes": "cpp",
+    "xlocmon": "cpp",
+    "xlocnum": "cpp",
+    "xloctime": "cpp"
   }
 }

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)

src/main/cpp/RobotContainer.cpp

@@ -1,3 +1,4 @@
+
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
@@ -17,8 +18,11 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <units/angle.h>
 #include <units/velocity.h>
+#include <frc2/command/DeferredCommand.h>
+#include <frc2/command/StartEndCommand.h>
 
 #include <iostream>
+#include <memory>
 
 #include "commands/Autos.h"
 #include "commands/ExampleCommand.h"
@@ -28,11 +32,23 @@
 RobotContainer::RobotContainer() {
   // Initialize all of your commands and subsystems here
 
-  // m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName);
-
   frc::SmartDashboard::PutData(&m_chooser);
-  m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto);
-  m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto);
+  m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto);
+  m_chooser.AddOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto);
+  m_chooser.AddOption(AutoConstants::kLeftToLeftReefAuto, AutoConstants::kLeftToLeftReefAuto);
+  m_chooser.AddOption(AutoConstants::kRightToRightReefAuto, AutoConstants::kRightToRightReefAuto);
+  m_chooser.AddOption(AutoConstants::kForwardAuto, AutoConstants::kForwardAuto);
+
+  // pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command"));
+  pathplanner::NamedCommands::registerCommand("To L1 Position", std::move(m_commandController.MoveToPositionL1()));
+  pathplanner::NamedCommands::registerCommand("To L2 Position", std::move(m_commandController.MoveToPositionL2()));
+  pathplanner::NamedCommands::registerCommand("To L3 Position", std::move(m_commandController.MoveToPositionL3()));
+  pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral()));
+  pathplanner::NamedCommands::registerCommand("Remove Algae", std::move(m_commandController.RemoveAlgaeFromL2()));
+  pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral()));
+  pathplanner::NamedCommands::registerCommand("Expel Coral", std::move(m_commandController.ExpelCoral().WithTimeout(CommandConstants::kExpelCoralTimeout)));
+  pathplanner::NamedCommands::registerCommand("Home", std::move(m_commandController.HomeElevator()));
+  pathplanner::NamedCommands::registerCommand("Delayed To L2 Position", std::move(frc2::cmd::Wait(CommandConstants::kWaitBeforeL2).AndThen(m_commandController.MoveToPositionL2())));
 
   // Configure the button bindings
   ConfigureBindings();
@@ -55,33 +71,56 @@ RobotContainer::RobotContainer() {
 }
 
 void RobotContainer::ConfigureBindings() {
-  m_driverController.A().OnTrue(m_algaeArm.MoveToPositionAbsolute(50_deg));
-  m_driverController.B().OnTrue(m_algaeArm.MoveToPositionAbsolute(5_deg));
-  m_driverController.X().OnTrue(m_coralArm.MoveToPositionAbsolute(120_deg));
-  m_driverController.Y().OnTrue(m_coralArm.MoveToPositionAbsolute(5_deg));
+  // NOT FINAL
+
+  m_operatorController.POVLeft().OnTrue(m_commandController.MoveToPositionL1());
+  m_operatorController.POVUp().OnTrue(m_commandController.MoveToPositionL2());
+  m_operatorController.POVRight().OnTrue(m_commandController.MoveToPositionL3());
+  m_operatorController.POVDown().OnTrue(m_commandController.FeedCoral());
+
+  m_operatorController.A().WhileTrue(m_commandController.ExpelCoral());
+  m_operatorController.B().WhileTrue(m_coralIntake.MoveAtPercent(CommandConstants::kCoralFeedSpeed));
+  m_operatorController.X().WhileTrue(m_commandController.ExpelAlgae());
+  m_operatorController.Y().WhileTrue(m_commandController.IntakeAlgae());
+
+  m_operatorController.LeftBumper().OnTrue(m_commandController.RemoveAlgaeFromL2());
+  // m_operatorController.RightBumper().OnTrue(m_commandController.RemoveAlgaeFromL3());
+  m_operatorController.RightBumper().OnTrue(m_algaeArm.MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition));
+
+  m_driverController.POVUp().OnTrue(m_commandController.ClimbUp());
+  m_driverController.POVDown().OnTrue(m_commandController.ClimbDown());
+
+  m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator());
+  m_driverController.RightBumper().OnTrue(frc2::InstantCommand(
+    [this]() {
+      std::cout << "Resetting rotation" << std::endl;
+      m_drive.ResetRotation();
+    }
+  ).ToPtr());
 }
 
 frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
-  auto command = pathplanner::PathPlannerAuto("Spin Auto");
+  m_autoSelected = m_chooser.GetSelected();
+
+  std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n";
+
+  auto command = pathplanner::PathPlannerAuto(m_autoSelected);
   auto rot = command.getStartingPose().Rotation().Degrees();
 
   auto offset = m_drive.GetHeading() - rot;
-
-  m_autoSelected = m_chooser.GetSelected();
 
-  std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n";
-
   return pathplanner::PathPlannerAuto(m_autoSelected)
     .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr());
 }
 
 void RobotContainer::Periodic() {
   frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech);
+
+  // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); }));
 }
 
 void RobotContainer::Initialize() {
   m_elevator.OnInit();
   m_coralArm.OnInit();
   m_algaeArm.OnInit();
-  m_climber.OnInit();
 }

src/main/cpp/commands/CommandController.cpp

@@ -0,0 +1,107 @@
+#include "commands/CommandController.h"
+
+#include <frc2/command/FunctionalCommand.h>
+
+#include "Constants.h"
+
+frc2::CommandPtr CommandController::MoveToPositionL1() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL1Position)
+    .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed))
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL1Position))
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition));
+}
+
+frc2::CommandPtr CommandController::MoveToPositionL2() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL2Position)
+    .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed))
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL2Position))
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition));
+}
+
+frc2::CommandPtr CommandController::MoveToPositionL3() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL3Position)
+    .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed))
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL3Position))
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition));
+}
+
+frc2::CommandPtr CommandController::FeedCoral() {
+    return m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition)
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition))
+    .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorFeedPosition))
+    .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed))
+    .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)
+    .Until([this]() {
+        return m_subsystems.coralIntake->HasGamePiece();
+    })
+    .WithTimeout(CommandConstants::kCoralFeedTimeout));
+}
+
+frc2::CommandPtr CommandController::ExpelCoral() {
+    return m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed);
+}
+
+frc2::CommandPtr CommandController::IntakeAlgae() {
+    return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeIntakePosition)
+    .AndThen(m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed))
+    // .Until([this]() {
+    //     return m_subsystems.algaeIntake->HasGamePiece();
+    // })
+    // .WithTimeout(CommandConstants::kAlgaeIntakeTimeout);
+    ;
+}
+
+frc2::CommandPtr CommandController::ExpelAlgae() {
+    return m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed);
+}
+
+frc2::CommandPtr CommandController::RemoveAlgaeFromL2() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position)
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kRemoveAlgaeAlgaeArmPosition))
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position))
+    .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed))
+    .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout)
+    .RaceWith(m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed))
+    ;
+}
+
+frc2::CommandPtr CommandController::RemoveAlgaeFromL3() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL3Position)
+    .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kRemoveAlgaeAlgaeArmPosition))
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL3Position))
+    .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed))
+    .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout);
+}
+
+frc2::CommandPtr CommandController::HomeElevator() {
+    return
+    m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition)
+    .AndThen(frc2::InstantCommand(
+        [this]() {
+            m_subsystems.elevator->SetEncoderPosition(ElevatorConstants::kMaxDistance);
+        }
+    ).ToPtr())
+    .AndThen(frc2::InstantCommand(
+        [this]() {
+            std::cout << "Setting homing PID settings" << std::endl;
+            m_subsystems.elevator->UpdatePidSettings(ElevatorConstants::kHomePidSettings);
+        }
+    ).ToPtr())
+    .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance))
+    .FinallyDo(
+        [this]() {
+            std::cout << "Setting typical PID settings" << std::endl;
+            m_subsystems.elevator->UpdatePidSettings(ElevatorConstants::kElevatorPidSettings);
+        }
+    );
+}
+
+frc2::CommandPtr CommandController::ClimbUp() {
+    return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition)
+    .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralClimbPosition))
+    .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbUpPosition));
+}
+
+frc2::CommandPtr CommandController::ClimbDown() {
+    return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbDownPosition);
+}

src/main/cpp/subsystems/CoralArmSubsystem.cpp

@@ -1,3 +1,3 @@
 #include "subsystems/CoralArmSubsystem.h"
 
-#include <frc2/command/RunCommand.h>
+#include <frc2/command/RunCommand.h>

src/main/cpp/subsystems/DriveSubsystem.cpp

@@ -2,17 +2,24 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "subsystems/DriveSubsystem.h"
-
+#include "subsystems/DriveSubsystem.h" 
+#include <frc/DriverStation.h>
+#include <frc/RobotBase.h>
+#include <frc/RobotController.h>
+#include <frc/Timer.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <pathplanner/lib/path/PathPlannerPath.h>
 #include <pathplanner/lib/auto/AutoBuilder.h>
 #include <pathplanner/lib/config/RobotConfig.h>
 #include <pathplanner/lib/controllers/PPHolonomicDriveController.h>
+#include <subzero/drivetrain/SwerveUtils.h>
 #include <frc/geometry/Rotation2d.h>
 #include <hal/FRCUsageReporting.h>
 #include <frc/DriverStation.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
 #include <units/velocity.h>
+#include <units/length.h>
 #include <iostream>
 
 #include "Constants.h"
@@ -38,6 +45,8 @@ DriveSubsystem::DriveSubsystem()
   HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
              HALUsageReporting::kRobotDriveSwerve_MaxSwerve);
 
+  frc::SmartDashboard::PutData("Field", &m_field);
+
   pathplanner::RobotConfig config = pathplanner::RobotConfig::fromGUISettings();
 
    pathplanner::AutoBuilder::configure(
@@ -68,7 +77,7 @@ DriveSubsystem::DriveSubsystem()
 
   m_pidgey.SetYaw(0_deg, 100_ms); // Set our yaw to 0 degrees and wait up to 100 milliseconds for the setter to take affect
   m_pidgey.GetYaw().WaitForUpdate(100_ms); // And wait up to 100 milliseconds for the yaw to take affect
-  std::cout << "Set the yaw to 144 degrees, we are currently at " << m_pidgey.GetYaw() << std::endl;
+  // std::cout << "Set the yaw to 144 degrees, we are currently at " << m_pidgey.GetYaw() << std::endl;
 }
 
 void DriveSubsystem::Periodic() {
@@ -78,6 +87,9 @@ void DriveSubsystem::Periodic() {
                     {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
                   m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
     auto &yaw = m_pidgey.GetYaw();
+
+  m_field.SetRobotPose(m_odometry.GetPose());
+
   if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) {
 
     std::cout << "Yaw: " << yaw.GetValue().value() << std::endl;
@@ -213,11 +225,27 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
       pose);
 }
 
+void DriveSubsystem::ResetRotation() {
+  m_pidgey.Reset();
+}
+
 void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) {
   m_pidgey.SetYaw(offset.Degrees() + m_pidgey.GetYaw().GetValue());
     m_odometry.ResetPosition(
       m_pidgey.GetYaw().GetValue(),
       {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
         m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
       m_odometry.GetPose());
+}
+
+void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                          units::second_t timestamp) {
+  // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp);
+}
+
+void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement,
+                                          units::second_t timestamp,
+                                          const Eigen::Vector3d& stdDevs) {
+  // wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+  // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
 }

src/main/cpp/subsystems/ElevatorSubsystem.cpp

@@ -0,0 +1,33 @@
+#include <subsystems/ElevatorSubsystem.h>
+
+void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) {
+    // Basesingleaxis applies a conversion factor so we don't provide rotations
+    m_enc.SetPosition(pos.value());
+}
+
+bool ElevatorSubsystem::GetMinLimitSwitch() {
+    return m_leadMotor.GetReverseLimitSwitch().Get();
+}
+
+bool ElevatorSubsystem::GetMaxLimitSwitch() {
+    return m_leadMotor.GetForwardLimitSwitch().Get();
+}
+
+void ElevatorSubsystem::Periodic() {
+  if (GetMaxLimitSwitch()) {
+    std::cout << "Elevator at max" << std::endl;
+    SetEncoderPosition(ElevatorConstants::kMaxDistance);
+  } else if (GetMinLimitSwitch()) {
+    std::cout << "Elevator at min" << std::endl;
+    SetEncoderPosition(ElevatorConstants::kMinDistance);
+  }
+
+  frc::SmartDashboard::PutBoolean("Forward limit switch state", GetMaxLimitSwitch());
+  frc::SmartDashboard::PutBoolean("Reverse limit switch state", GetMinLimitSwitch());
+
+  subzero::LinearSingleAxisSubsystem<subzero::IPidMotorController>::Periodic();
+}
+
+void ElevatorSubsystem::UpdatePidSettings(subzero::PidSettings settings) {
+    m_elevatorController.UpdatePidSettings(settings);
+}

src/main/cpp/subsystems/IntakeSubsystem.cpp

@@ -1,5 +1,7 @@
 #include <subsystems/IntakeSubsystem.h>
 
+#include <iostream>
+
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/FunctionalCommand.h>
 
@@ -31,4 +33,9 @@ frc2::CommandPtr IntakeSubsystem::Stop() {
         },
         {this}
     ).ToPtr();
+}
+
+bool IntakeSubsystem::HasGamePiece() {
+    std::cout << "Motor amperage is " << m_motor.GetOutputCurrent() << " amps." << std::endl;
+    return m_motor.GetOutputCurrent() > m_hasGamePieceCurrent;
 }