Added full babys

frc2053 · Apr 12, 2024 · f5914f4 · f5914f4
1 parent 1434d5c
commit f5914f4
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 2 deletions.
diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp
@@ -14,12 +14,19 @@ RobotContainer::RobotContainer() {
 }
 
 void RobotContainer::ConfigureBindings() {
+  testController.RightBumper().WhileTrue(BabySpinUpShooter());
+  testController.RightBumper().OnFalse(NotUsingShooter());
+
+  driveSub.SetDefaultCommand(driveSub.BabyDriveFactory(
+      DeadbandAndSquare([this] { return -testController.GetLeftY(); }),
+      DeadbandAndSquare([this] { return -testController.GetLeftX(); }),
+      DeadbandAndSquare([this] { return -testController.GetRightX(); }),
+      [] { return true; }));
+
   operatorController.X().WhileTrue(shooterSub.GoToSpeedCmd([this] {
     return frc::ApplyDeadband<double>(operatorController.GetLeftY(), 0.1);
   }));
 
-  operatorController.RightBumper().WhileTrue(BabySpinUpShooter());
-
   operatorController.RightTrigger().WhileTrue(intakeSub.SuckInNotes());
   operatorController.LeftTrigger().WhileTrue(IntakeNote());
 

diff --git a/src/main/cpp/subsystems/DrivebaseSubsystem.cpp b/src/main/cpp/subsystems/DrivebaseSubsystem.cpp
@@ -141,6 +141,23 @@ frc2::CommandPtr DrivebaseSubsystem::ResetPosition(
   });
 }
 
+frc2::CommandPtr DrivebaseSubsystem::BabyDriveFactory(
+    std::function<double()> fow, std::function<double()> side,
+    std::function<double()> rot, std::function<bool()> fieldOriented) {
+  return frc2::RunCommand(
+             [this, fow, side, rot, fieldOriented]() {
+               swerveDrive.Drive(
+                   fow() * constants::swerve::physical::MAX_LINEAR_SPEED * .10,
+                   side() * constants::swerve::physical::MAX_LINEAR_SPEED * .10,
+                   rot() * constants::swerve::physical::MAX_ROTATION_SPEED *
+                       .10,
+                   true, fieldOriented());
+             },
+             {this})
+      .ToPtr()
+      .WithName("Baby Drive Factory");
+}
+
 frc2::CommandPtr DrivebaseSubsystem::DriveFactory(
     std::function<double()> fow, std::function<double()> side,
     std::function<double()> rot, std::function<bool()> fieldOriented) {

diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h
@@ -42,6 +42,7 @@ class RobotContainer {
   void ConfigureBindings();
   frc2::CommandXboxController driverController{0};
   frc2::CommandXboxController operatorController{1};
+  frc2::CommandXboxController testController{2};
 
   DrivebaseSubsystem driveSub;
   ShooterSubsystem shooterSub;

diff --git a/src/main/include/subsystems/DrivebaseSubsystem.h b/src/main/include/subsystems/DrivebaseSubsystem.h
@@ -35,6 +35,11 @@ class DrivebaseSubsystem : public frc2::SubsystemBase {
   void UpdateOdometry();
   frc2::CommandPtr ResetPosition(std::function<frc::Pose2d()> newPosition);
 
+  frc2::CommandPtr BabyDriveFactory(std::function<double()> fow,
+                                    std::function<double()> side,
+                                    std::function<double()> rot,
+                                    std::function<bool()> fieldOriented);
+
   frc2::CommandPtr DriveFactory(std::function<double()> fow,
                                 std::function<double()> side,
                                 std::function<double()> rot,