add auto to place and leave community

Spartronics4915 · Mar 4, 2023 · f830ac2 · f830ac2
1 parent 3fe3b1c
commit f830ac2
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 1 deletion.
diff --git a/src/main/java/com/spartronics4915/frc2023/Constants.java b/src/main/java/com/spartronics4915/frc2023/Constants.java
@@ -523,7 +523,7 @@ public static final class OI {
         public static final double kTriggerDeadband = 0.08;
         public static final double kResponseCurveExponent = 5.0 / 3.0;
 
-		public static final int kDefaultAutoIndex = 0;
+		public static final int kDefaultAutoIndex = 3;
 
     }
 }
diff --git a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java
@@ -44,13 +44,15 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 /**
@@ -198,6 +200,21 @@ mAutos.new Strategy(
                 "do nothing",
                 (Pose2d initialPose) -> new CommandBase() {}
             ),
+            mAutos.new Strategy(
+                "place, do nothing",
+                (Pose2d initialPose) -> new SequentialCommandGroup(
+                    mArmCommands.new ReleasePiece(ArmState.FLOOR_POS)
+                )
+            ),
+            mAutos.new Strategy(
+                "place, leave community",
+                (Pose2d initialPose) -> new SequentialCommandGroup(
+                    mArmCommands.new ReleasePiece(ArmState.FLOOR_POS),
+                    mSwerve.driveCommand(new ChassisSpeeds(-2, 0, 0), false, true),
+                    new WaitCommand(3),
+                    mSwerve.driveCommand(new ChassisSpeeds(), false, true)
+                )
+            )
             // mAutos.new Strategy(
             //     "drop, leave",
             //     (Pose2d initialPose) -> new SequentialCommandGroup(

diff --git a/src/main/java/com/spartronics4915/frc2023/subsystems/Swerve.java b/src/main/java/com/spartronics4915/frc2023/subsystems/Swerve.java
@@ -22,6 +22,7 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import static com.spartronics4915.frc2023.Constants.Swerve.*;
@@ -146,6 +147,10 @@ public void drive(ChassisSpeeds chassisSpeeds, boolean fieldRelative, boolean is
         setModuleStates(moduleStates, isOpenLoop);
     }
 
+    public CommandBase driveCommand(ChassisSpeeds chassisSpeeds, boolean fieldRelative, boolean isOpenLoop) {
+        return runOnce(() -> drive(chassisSpeeds, fieldRelative, isOpenLoop));
+    }
+
     /**
      * Set the module states. 
      * @param desiredStates The desired module states.