Feature/driving improvements

src/main/cpp/RobotContainer.cpp

@@ -29,15 +29,22 @@
 #include "subsystems/AlgaeArmSubsystem.h"
 #include "subsystems/CoralArmSubsystem.h"
 
-RobotContainer::RobotContainer() {
+RobotContainer::RobotContainer() 
+  : m_isReducedSensitivity{false}, m_lowSensitivityCoefficient{DriveConstants::kLowSensivityCoefficient} {
   // Initialize all of your commands and subsystems here
 
+  frc::SmartDashboard::PutNumber("Low Sensitivity Percentage", m_lowSensitivityCoefficient);
+
   frc::SmartDashboard::PutData(&m_chooser);
   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);
+  m_chooser.AddOption(AutoConstants::kRightToRightSourceAuto, AutoConstants::kRightToRightSourceAuto);
+  m_chooser.AddOption(AutoConstants::kLeftToLeftSourceAuto, AutoConstants::kLeftToLeftSourceAuto);
+  m_chooser.AddOption(AutoConstants::kCenterToRightSourceAuto, AutoConstants::kCenterToRightSourceAuto);
+  m_chooser.AddOption(AutoConstants::kCenterToLeftSourceAuto, AutoConstants::kCenterToLeftSourceAuto);
 
   // pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command"));
   pathplanner::NamedCommands::registerCommand("To L1 Position", std::move(m_commandController.MoveToPositionL1()));
@@ -50,24 +57,78 @@ RobotContainer::RobotContainer() {
   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())));
 
+  m_timeSinceControllerInput = 0_s;
+  m_defaultLastCalled = 0_s;
+
   // Configure the button bindings
   ConfigureBindings();
 
+  m_zeroedBeforeSetX = false;
+
   // Set up default drive command
   // The left stick controls translation of the robot.
   // Turning is controlled by the X axis of the right stick.
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.Drive(
+
+        units::second_t current = frc::Timer::GetFPGATimestamp();
+        if (std::hypot(m_driverController.GetLeftX(), m_driverController.GetLeftY()) < OIConstants::kDriveDeadband
+         && std::fabs(m_driverController.GetRightX()) < OIConstants::kDriveDeadband) {
+          units::second_t dif = current - m_defaultLastCalled;
+          m_timeSinceControllerInput += dif;
+
+          if (!m_zeroedBeforeSetX) {
+              m_drive.Drive(
+                0_mps,
+                0_mps,
+                0_deg_per_s,
+                true
+              );
+          }
+
+          if (m_timeSinceControllerInput > DriveConstants::kSetXThreshold) {
+
+            m_zeroedBeforeSetX = true;
+
+            m_drive.SetX();
+          }
+        } else {
+            m_timeSinceControllerInput = 0_s;
+
+            double multiplier = m_isReducedSensitivity ? m_lowSensitivityCoefficient : 1.0;
+
+          m_drive.Drive(
             -units::meters_per_second_t{frc::ApplyDeadband(
-                m_driverController.GetLeftY(), OIConstants::kDriveDeadband)},
+                m_driverController.GetLeftY() * multiplier, OIConstants::kDriveDeadband)},
             -units::meters_per_second_t{frc::ApplyDeadband(
-                m_driverController.GetLeftX(), OIConstants::kDriveDeadband)},
+                m_driverController.GetLeftX() * multiplier, OIConstants::kDriveDeadband)},
             -units::radians_per_second_t{frc::ApplyDeadband(
-                m_driverController.GetRightX(), OIConstants::kDriveDeadband)},
-            true);
+                m_driverController.GetRightX() * multiplier, OIConstants::kDriveDeadband)},
+          true);
+
+          m_zeroedBeforeSetX = false;
+        }
+
+        m_defaultLastCalled = current;
       },
       {&m_drive}));
+
+    // m_climber.SetDefaultCommand(
+    //   frc2::RunCommand(
+    //     [this]() {
+    //       m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() 
+    //         + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar);
+
+    //         std::cout << "Controller input: " << (-m_driverController.GetLeftTriggerAxis() 
+    //         + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar
+    //         << ", Current relative position: " << m_climber.GetCurrentPosition().value() << std::endl;
+    //     },
+    //     {&m_climber}
+    //   )
+    // );
+
+  // Has to be raised before match so coral arm is within frame perimeter
+  // m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition);
 }
 
 void RobotContainer::ConfigureBindings() {
@@ -90,6 +151,8 @@ void RobotContainer::ConfigureBindings() {
   m_driverController.POVUp().OnTrue(m_commandController.ClimbUp());
   m_driverController.POVDown().OnTrue(m_commandController.ClimbDown());
 
+  m_driverController.A().OnTrue(frc2::InstantCommand([this]() { m_isReducedSensitivity = !m_isReducedSensitivity; }).ToPtr());
+
   m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator());
   m_driverController.RightBumper().OnTrue(frc2::InstantCommand(
     [this]() {
@@ -116,6 +179,12 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
 void RobotContainer::Periodic() {
   frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech);
 
+  frc::SmartDashboard::PutBoolean("Is Slow Mde Active", m_isReducedSensitivity);
+
+  m_lowSensitivityCoefficient = frc::SmartDashboard::GetNumber("Low Sensitivity Percentage", DriveConstants::kLowSensivityCoefficient);
+
+  // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); }));
+
   // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); }));
 }
 

src/main/cpp/subsystems/DriveSubsystem.cpp

@@ -81,20 +81,33 @@ DriveSubsystem::DriveSubsystem()
 }
 
 void DriveSubsystem::Periodic() {
+  // The `poseEstimator` object replaces our `m_odometry`
   // Implementation of subsystem periodic method goes here.
-  m_odometry.Update(frc::Rotation2d(units::radian_t{
-                        m_pidgey.GetYaw().GetValue()}),
-                    {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
-                  m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
-    auto &yaw = m_pidgey.GetYaw();
+  // m_odometry.Update(frc::Rotation2d(units::radian_t{
+  //                       m_pidgey.GetYaw().GetValue()}),
+  //                   {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
+  //                 m_rearLeft.GetPosition(), m_rearRight.GetPosition()});
+  //   auto &yaw = m_pidgey.GetYaw();
 
-  m_field.SetRobotPose(m_odometry.GetPose());
+  poseEstimator.UpdateWithTime(frc::Timer::GetFPGATimestamp(), GetHeading(), GetModulePositions());
+
+  m_vision.UpdateEstimatedGlobalPose(poseEstimator, true);
+
+  auto updatedPose = poseEstimator.GetEstimatedPosition();
+
+  m_lastGoodPosition = updatedPose;
+  m_field.SetRobotPose(updatedPose);
+
+  logDrivebase();
+
+  frc::SmartDashboard::PutNumber("Robot X", poseEstimator.GetEstimatedPosition().X().value());
+  frc::SmartDashboard::PutNumber("Robot Y", poseEstimator.GetEstimatedPosition().Y().value());
 
   if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) {
 
-    std::cout << "Yaw: " << yaw.GetValue().value() << std::endl;
+  //   std::cout << "Yaw: " << yaw.GetValue().value() << std::endl;
 
-    currentTime += GyroConstants::kPrintPeriod;
+  //   currentTime += GyroConstants::kPrintPeriod;
   //   /**
   //    * GetYaw automatically calls Refresh(), no need to manually refresh.
   //    *
@@ -128,13 +141,42 @@ void DriveSubsystem::Periodic() {
   //    * timestamps when it receives the frame. This can be further used for latency compensation.
   //    */
   //   std::cout << std::endl;
+  // }
   }
 }
 
+wpi::array<frc::SwerveModulePosition, 4U> DriveSubsystem::GetModulePositions()
+    const {
+  return {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
+          m_rearLeft.GetPosition(), m_rearRight.GetPosition()};
+}
+
+void DriveSubsystem::SimulationPeriodic() {
+  frc::ChassisSpeeds chassisSpeeds = m_driveKinematics.ToChassisSpeeds(
+      m_frontLeft.GetState(), m_frontRight.GetState(), m_rearLeft.GetState(),
+      m_rearRight.GetState());
+
+  m_simPidgey.SetSupplyVoltage(frc::RobotController::GetBatteryVoltage());
+  m_simPidgey.SetRawYaw(
+      GetHeading() +
+      units::degree_t(chassisSpeeds.omega.convert<units::deg_per_s>().value() *
+                      DriveConstants::kPeriodicInterval.value()));
+
+  poseEstimator.Update(GetHeading(), GetModulePositions());
+  m_field.SetRobotPose(poseEstimator.GetEstimatedPosition());
+}
+
 void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
                            units::meters_per_second_t ySpeed,
                            units::radians_per_second_t rot,
                            bool fieldRelative) {
+
+  auto now = frc::Timer::GetFPGATimestamp();
+  auto dif = now - m_lastUpdatedTime;
+
+  auto joystickSpeeds =
+      GetSpeedsFromJoystick(xSpeed, ySpeed, rot, fieldRelative);
+
   // Convert the commanded speeds into the correct units for the drivetrain
   units::meters_per_second_t xSpeedDelivered =
       xSpeed.value() * DriveConstants::kMaxSpeed;
@@ -144,12 +186,9 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
       rot.value() * DriveConstants::kMaxAngularSpeed;
 
   auto states = m_driveKinematics.ToSwerveModuleStates(
-      fieldRelative
-          ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-                xSpeedDelivered, ySpeedDelivered, rotDelivered,
-                frc::Rotation2d(units::radian_t{
-                    m_pidgey.GetYaw().GetValue()}))
-          : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered});
+      frc::ChassisSpeeds::Discretize(joystickSpeeds, dif));
+
+  m_lastUpdatedTime = now;
 
   m_driveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed);
 
@@ -161,6 +200,17 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed,
   m_rearRight.SetDesiredState(br);
 }
 
+frc::ChassisSpeeds DriveSubsystem::GetSpeedsFromJoystick(
+    units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed,
+    units::radians_per_second_t rot, bool fieldRelative) {
+  return fieldRelative
+             ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                   xSpeed * DriveConstants::kMaxSpeed.value(),
+                   ySpeed * DriveConstants::kMaxSpeed.value(),
+                   rot * DriveConstants::kMaxAngularSpeed.value(), GetHeading())
+             : frc::ChassisSpeeds{xSpeed, ySpeed, rot};
+}
+
 void DriveSubsystem::Drive(frc::ChassisSpeeds speeds) {
   DriveSubsystem::SetModuleStates(
       m_driveKinematics.ToSwerveModuleStates(speeds));
@@ -215,37 +265,55 @@ double DriveSubsystem::GetTurnRate() {
   return -m_pidgey.GetAngularVelocityZWorld().GetValue().value();
 }
 
-frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::Pose2d DriveSubsystem::GetPose() { return poseEstimator.GetEstimatedPosition(); }
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  m_odometry.ResetPosition(
-      GetHeading(),
-      {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
-        m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
-      pose);
+  // m_odometry.ResetPosition(
+  //     GetHeading(),
+  //     {m_frontLeft.GetPosition(), m_frontRight.GetPosition(),
+  //       m_rearLeft.GetPosition(), m_rearRight.GetPosition()},
+  //     pose);
+
+  m_lastGoodPosition = pose;
+  poseEstimator.ResetPosition(GetHeading(), GetModulePositions(), pose);
 }
 
 void DriveSubsystem::ResetRotation() {
   m_pidgey.Reset();
 }
 
+void DriveSubsystem::logDrivebase() {
+  std::vector states_vec = {m_frontLeft.GetState(), m_frontRight.GetState(),
+                            m_rearLeft.GetState(), m_rearRight.GetState()};
+  std::span<frc::SwerveModuleState, 4> states(states_vec.begin(),
+                                              states_vec.end());
+  std::vector desired_vec = {
+      m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(),
+      m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()};
+  std::span<frc::SwerveModuleState, 4> desiredStates(desired_vec.begin(),
+                                                     desired_vec.end());
+
+  m_publisher.Set(states);
+  m_desiredPublisher.Set(desiredStates);
+}
+
 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());
+    // 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);
+  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);
+  wpi::array<double, 3> newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)};
+  poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs);
 }

src/main/cpp/subsystems/ElevatorSubsystem.cpp

@@ -15,10 +15,10 @@ bool ElevatorSubsystem::GetMaxLimitSwitch() {
 
 void ElevatorSubsystem::Periodic() {
   if (GetMaxLimitSwitch()) {
-    std::cout << "Elevator at max" << std::endl;
+    // std::cout << "Elevator at max" << std::endl;
     SetEncoderPosition(ElevatorConstants::kMaxDistance);
   } else if (GetMinLimitSwitch()) {
-    std::cout << "Elevator at min" << std::endl;
+    // std::cout << "Elevator at min" << std::endl;
     SetEncoderPosition(ElevatorConstants::kMinDistance);
   }