diff --git a/src/main/java/com/spartronics4915/frc2023/Constants.java b/src/main/java/com/spartronics4915/frc2023/Constants.java index 7871288..696071b 100644 --- a/src/main/java/com/spartronics4915/frc2023/Constants.java +++ b/src/main/java/com/spartronics4915/frc2023/Constants.java @@ -460,6 +460,12 @@ public ArmMotorConstants(int MotorID, double PositionConversionFactor, boolean I Rotation2d.fromDegrees(23), Rotation2d.fromDegrees(-36) ); + + public static final ArmPositionConstants kCubeTopShootConstants = new ArmPositionConstants( + 0, + Rotation2d.fromDegrees(9.996031), + Rotation2d.fromDegrees(30.441) + ); public static final Rotation2d kTransformAmount = Rotation2d.fromDegrees(0.5); public static final double kArmRetractedPriorWaitDuration = 1; // seconds diff --git a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java index 3d3b801..5d9e867 100644 --- a/src/main/java/com/spartronics4915/frc2023/RobotContainer.java +++ b/src/main/java/com/spartronics4915/frc2023/RobotContainer.java @@ -210,8 +210,29 @@ mAutos.new Strategy( "place, leave community", (Pose2d initialPose) -> new SequentialCommandGroup( mArmCommands.new ReleasePiece(ArmState.FLOOR_POS), + mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE), + new WaitCommand(1), mSwerve.driveCommand(new ChassisSpeeds(-2, 0, 0), false, true), - new WaitCommand(3), + new WaitCommand(2.5), + mSwerve.driveCommand(new ChassisSpeeds(), false, true) + ) + ), + mAutos.new Strategy( + "cube high (test)", + (Pose2d initialPose) -> new SequentialCommandGroup( + mArmCommands.new ReleasePiece(ArmState.SHOOT_HIGH_CUBE) + ) + ), + mAutos.new Strategy( + "place, move short (test)", + (Pose2d initialPose) -> new SequentialCommandGroup( + mArmCommands.new ReleasePiece(ArmState.FLOOR_POS), + mArmCommands.new SetArmLocalState(ArmState.TUCK_INTERMEDIATE), + new WaitCommand(1), + mSwerve.driveCommand(new ChassisSpeeds(-0.5, 0, 0), false, true), + new WaitCommand(2), + mSwerve.driveCommand(new ChassisSpeeds(-1, 0, 0), false, true), + new WaitCommand(1), mSwerve.driveCommand(new ChassisSpeeds(), false, true) ) ) diff --git a/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java b/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java index a148560..90c94df 100644 --- a/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java +++ b/src/main/java/com/spartronics4915/frc2023/subsystems/ArmSubsystem.java @@ -58,7 +58,9 @@ public enum ArmState { CONE_LEVEL_1(kConeLevel1Constants), CONE_LEVEL_2(kConeLevel2Constants), CUBE_LEVEL_1(kCubeLevel1Constants), - CUBE_LEVEL_2(kCubeLevel2Constants); + CUBE_LEVEL_2(kCubeLevel2Constants), + + SHOOT_HIGH_CUBE(kCubeTopShootConstants); // CONE_LEVEL_3(kConeLevel3Constants);