Started retuning old auto routines

#222
FRC1756-Argos · Apr 10, 2023 · abb2d74 · abb2d74
1 parent 899d005
commit abb2d74
Show file tree

Hide file tree

Showing 6 changed files with 22 additions and 20 deletions.
diff --git a/src/main/cpp/commands/autonomous/autonomous_balance.cpp b/src/main/cpp/commands/autonomous/autonomous_balance.cpp
@@ -47,7 +47,7 @@ AutonomousBalance::AutonomousBalance(SwerveDriveSubsystem& drive,
                                          PathType::concaveDown}
                            .ToPtr()
                            .AlongWith(DriveOverChargingStation{&m_drive, 0_deg, 180_deg}.ToPtr()))
-              .AndThen(frc2::WaitCommand(500_ms).ToPtr())
+              .AndThen(frc2::WaitCommand(750_ms).ToPtr())
               .AndThen(BalanceChargingStation{&m_drive, 180_deg, 180_deg}.ToPtr()),
       } {}
 

diff --git a/src/main/cpp/commands/balance_charging_station.cpp b/src/main/cpp/commands/balance_charging_station.cpp
@@ -24,7 +24,7 @@ BalanceChargingStation::BalanceChargingStation(SwerveDriveSubsystem* drive,
     , m_commands{
           DriveUntilPitch{m_pDrive,
                           m_approachAngle,
-                          0.2,
+                          0.35,
                           0,
                           10_deg * m_initialPitchSign,
                           m_approachForward ? ApproachDirection::Increasing : ApproachDirection::Decreasing,
@@ -33,33 +33,34 @@ BalanceChargingStation::BalanceChargingStation(SwerveDriveSubsystem* drive,
               .AndThen(
                   DriveUntilPitch{m_pDrive,
                                   m_approachAngle,
-                                  0.2,
-                                  0.2,
+                                  0.15,
+                                  0.15,
                                   9_deg * m_initialPitchSign,
                                   m_approachForward ? ApproachDirection::Decreasing : ApproachDirection::Increasing,
-                                  2_s}
+                                  4_s}
                       .ToPtr())
               .AndThen(
                   // Final Approach to center
                   DriveUntilPitchRate{m_pDrive,
                                       m_approachAngle,
-                                      0.08,
-                                      0.2,
+                                      0.06,
+                                      0.15,
                                       thresholds::robotTippingPitchRate * m_initialPitchSign,
                                       m_approachForward ? ApproachDirection::Decreasing : ApproachDirection::Increasing,
-                                      2_s}
+                                      4_s}
                       .ToPtr())
               .AndThen(
                   DriveUntilPitch{m_pDrive,
                                   m_approachAngle,
-                                  0.2,
-                                  0.2,
+                                  0.06,
+                                  0.06,
                                   6_deg * m_initialPitchSign,
                                   m_approachForward ? ApproachDirection::Decreasing : ApproachDirection::Increasing,
-                                  2_s}
+                                  4_s}
                       .ToPtr())
-              .AndThen(
-                  frc2::InstantCommand([this, drive]() { drive->SwerveDrive(180_deg + m_approachAngle, 0.1); }).ToPtr())
+              .AndThen(frc2::InstantCommand([this, drive]() {
+                         drive->SwerveDrive(m_approachAngle, m_approachForward ? 0.15 : -0.15);
+                       }).ToPtr())
               .AndThen(frc2::WaitCommand(250_ms).ToPtr())
               .AndThen(frc2::InstantCommand([this, drive]() {
                          drive->StopDrive();

diff --git a/src/main/cpp/commands/drive_over_charging_station.cpp b/src/main/cpp/commands/drive_over_charging_station.cpp
@@ -24,7 +24,7 @@ DriveOverChargingStation::DriveOverChargingStation(SwerveDriveSubsystem* drive,
     , m_commands{
           DriveUntilPitch{m_pDrive,
                           m_approachAngle,
-                          0.2,
+                          0.25,
                           0,
                           thresholds::robotClimbPitch * m_initialPitchSign,
                           m_approachForward ? ApproachDirection::Increasing : ApproachDirection::Decreasing,
@@ -34,7 +34,7 @@ DriveOverChargingStation::DriveOverChargingStation(SwerveDriveSubsystem* drive,
                   DriveUntilPitch{m_pDrive,
                                   m_approachAngle,
                                   0.3,
-                                  0.2,
+                                  0.25,
                                   9_deg * m_initialPitchSign,
                                   m_approachForward ? ApproachDirection::Decreasing : ApproachDirection::Increasing,
                                   2_s}
@@ -69,7 +69,7 @@ DriveOverChargingStation::DriveOverChargingStation(SwerveDriveSubsystem* drive,
                       .ToPtr())
               .AndThen(frc2::InstantCommand([this, drive, approachAngle]() { drive->SwerveDrive(approachAngle, 0.2); })
                            .ToPtr()
-                           .AndThen(frc2::WaitCommand{0.5_s}.ToPtr())
+                           .AndThen(frc2::WaitCommand{750_ms}.ToPtr())
                            .AndThen(frc2::InstantCommand([this, drive]() { drive->StopDrive(); }).ToPtr()))} {}
 
 // Called when the command is initially scheduled.

diff --git a/src/main/cpp/commands/drive_until_pitch.cpp b/src/main/cpp/commands/drive_until_pitch.cpp
@@ -24,7 +24,7 @@ DriveUntilPitch::DriveUntilPitch(SwerveDriveSubsystem* swerveDrive,
     , m_pitchGoal{pitchGoal}
     , m_approachDirection{approachDirection}
     , m_timeout{timeout}
-    , m_velocityRamper{2 / 1_s, 2 / 1_s, initialPower} {
+    , m_velocityRamper{6 / 1_s, 6 / 1_s, initialPower} {
   // * AddRequirements OK here because no child commands are called that depend on subsystem
   AddRequirements(swerveDrive);
 }

diff --git a/src/main/cpp/commands/drive_until_pitch_rate.cpp b/src/main/cpp/commands/drive_until_pitch_rate.cpp
@@ -19,7 +19,7 @@ DriveUntilPitchRate::DriveUntilPitchRate(SwerveDriveSubsystem* swerveDrive,
     , m_pitchRateGoal{pitchRateGoal}
     , m_approachDirection{approachDirection}
     , m_timeout{timeout}
-    , m_velocityRamper{2 / 1_s, 2 / 1_s, initialPower} {
+    , m_velocityRamper{6 / 1_s, 6 / 1_s, initialPower} {
   // * AddRequirements OK here because no child commands are called that depend on subsystem
   AddRequirements(swerveDrive);
 }

diff --git a/src/main/cpp/commands/place_cone_command.cpp b/src/main/cpp/commands/place_cone_command.cpp
@@ -14,6 +14,7 @@
 #include <units/velocity.h>
 
 #include "commands/grip_cone_command.h"
+#include "commands/home_arm_extension_command.h"
 #include "commands/score_cone_command.h"
 #include "commands/set_arm_pose_command.h"
 #include "constants/scoring_positions.h"
@@ -35,18 +36,18 @@ void PlaceConeCommand::Initialize() {
   m_allCommands =
       GripConeCommand(m_intake)
           .ToPtr()
+          .AlongWith(HomeArmExtensionCommand{*m_lifter}.ToPtr())
           .AlongWith(frc2::InstantCommand([this]() {
                        LifterSubsystem& lifterRef = *m_lifter;
                        m_lifter->SetShoulderAngle(PlaceConeCommand::GetShoulderAngle(lifterRef, m_desiredArmPos));
                      }).ToPtr())
-          .AndThen(frc2::WaitUntilCommand([this]() { return m_bashGuard->IsBashGuardHomed(); }).ToPtr())
           .AndThen(SetArmPoseCommand{m_lifter,
                                      m_bashGuard,
                                      m_scoringPosition,
                                      frc::Translation2d{0_in, 0_in},
                                      []() { return true; },
                                      []() { return false; },
-                                     PathType::concaveDown}
+                                     PathType::componentWise}
                        .ToPtr())
           .AndThen(ScoreConeCommand{*m_lifter, *m_bashGuard, *m_intake}.ToPtr());
   m_allCommands.get()->Initialize();