diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index be7d0d9..3e1b451 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,7 +65,7 @@ public static final class Drivetrain { public static final int SHOULDER_MOTOR_SLAVE = 51; public static final int SHOULDER_ENCODER = 0; //public static final double SHOULDER_ENCODER_OFFSET = 0.542; - public static final double SHOULDER_ENCODER_OFFSET = 0.194; + public static final double SHOULDER_ENCODER_OFFSET = 0.095; public static final double SHOULDER_GEAR_RATIO = (11.0 / 52.0) * (30.0 / 68.0) * (12.0 / 60.0); public static final double SHOULDER_MANUAL_OVERRIDE_RANGE = 20.0; public static final double MAX_SHOULDER_ANGLE = 90.0; @@ -105,7 +105,7 @@ public static final class Drivetrain { // CONE_INTAKE_POSITION - public static final double CONE_INTAKE_POSITION_WRIST = 48.0; + public static final double CONE_INTAKE_POSITION_WRIST = 53.0; public static final double CONE_INTAKE_POSITION_ARM = 5.8; public static final double CONE_INTAKE_POSITION_SHOULDER = -48.0; @@ -116,8 +116,8 @@ public static final class Drivetrain { // CONE_SCORE_HIGH_POSITION public static final double CONE_SCORE_HIGH_POSITION_WRIST = -40.0; - public static final double CONE_SCORE_HIGH_POSITION_ARM = 19.0; - public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 44.0; + public static final double CONE_SCORE_HIGH_POSITION_ARM = 20.0; + public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 37.0; // CUBE_SCORE_HIGH_POSITION public static final double CUBE_SCORE_HIGH_POSITION_WRIST = 130.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 408e123..7ed7318 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.intake.IntakeHoldCommand; +import frc.robot.subsystems.IntakeSubsystem; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -80,17 +82,19 @@ public void autonomousInit() { m_robotContainer.getShoulderZeroCommand().schedule();*/ m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // schedule the autonomous command (example) if (m_autonomousCommand != null) { - (m_robotContainer.getArmHomeCommand() + (((m_robotContainer.getArmHomeCommand() .andThen(m_robotContainer.getGoToZeroWristCommand()) - .andThen(m_robotContainer.getShoulderZeroCommand())) + .andThen(m_robotContainer.getShoulderZeroCommand()))) + .raceWith(m_robotContainer.getIntakeHoldCommand())) + .andThen(m_robotContainer.setSMHomeCommand()) .andThen(m_autonomousCommand).schedule(); } else { (m_robotContainer.getArmHomeCommand() .andThen(m_robotContainer.getGoToZeroWristCommand()) - .andThen(m_robotContainer.getShoulderZeroCommand())).schedule(); + .andThen(m_robotContainer.getShoulderZeroCommand())) + .andThen(m_robotContainer.setSMHomeCommand()).schedule(); } // m_robotContainer.getArmHomeCommand().schedule(); @@ -125,7 +129,8 @@ public void teleopInit() { (m_robotContainer.getArmHomeCommand() .andThen(m_robotContainer.getGoToZeroWristCommand()) - .andThen(m_robotContainer.getShoulderZeroCommand())).schedule(); + .andThen(m_robotContainer.getShoulderZeroCommand()) + .andThen(m_robotContainer.setSMHomeCommand())).schedule(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb75721..f1edf49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,12 +15,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.autos.LeftSide2ConeAuto; -import frc.robot.autos.RightSide2ConeAuto; import frc.robot.autos.AutoBalance; -import frc.robot.autos.ConeHighAndBalanceAuto; +import frc.robot.autos.CubeHighAndBalanceAuto; +import frc.robot.autos.DriveStraightAuto; import frc.robot.autos.ConeHighAndDriveAuto; -import frc.robot.autos.ConeScoreHighAuto; +import frc.robot.autos.procedures.ConeScoreHighAuto; import frc.robot.commands.DefaultDriveCommand; import frc.robot.commands.arm.ArmHoldCommand; import frc.robot.commands.arm.ArmHomeCommand; @@ -44,6 +43,7 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.vision.Vision; +import frc.robot.util.StateMachine; import frc.robot.subsystems.ShoulderSubsystem; import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -70,23 +70,24 @@ public class RobotContainer { private final Vision vision = new Vision(); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); public final LEDSubsystem ledSubsystem = new LEDSubsystem(); + private final StateMachine sm = new StateMachine(); - private final SendableChooser autoChooser = new SendableChooser(); + public final SendableChooser autoChooser = new SendableChooser(); - // private final RightSide2ConeAuto rightConeAuto = new RightSide2ConeAuto(m_drivetrainSubsystem); - - private final LeftSide2ConeAuto leftConeAuto = new LeftSide2ConeAuto(m_drivetrainSubsystem); - - private final ConeHighAndBalanceAuto highConeAndBalanceAuto = new ConeHighAndBalanceAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, - wristSubsystem, armSubsystem, intakeSubsystem, armMessenger); + private final CubeHighAndBalanceAuto highConeAndBalanceAuto = new CubeHighAndBalanceAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, + wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm); private final ConeHighAndDriveAuto highConeAndDriveAuto = new ConeHighAndDriveAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, - wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem); + wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm); + + private final ConeScoreHighAuto highConeAuto = new ConeScoreHighAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, wristSubsystem, + armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm); - private final ConeScoreHighAuto highConeAuto = new ConeScoreHighAuto(shoulderSubsystem, shoulderMessenger, wristSubsystem, - armSubsystem, intakeSubsystem, armMessenger, ledSubsystem); + private final DriveStraightAuto driveStraightAuto = new DriveStraightAuto(m_drivetrainSubsystem); private final Simulator sim = new Simulator(m_drivetrainSubsystem); + + // private final DriveStraightAuto driveStraightAuto = new DriveStraightAuto(m_drivetrainSubsystem, wristSubsystem); // private final EngageStationAuto engageStationAuto = new EngageStationAuto(m_drivetrainSubsystem); //private final Simulator sim = new Simulator(m_drivetrainSubsystem); @@ -121,9 +122,10 @@ public RobotContainer() { public void initializeRobot() { //autoChooser.setDefaultOption("One side, two cargo, balance", leftConeAuto); autoChooser.setDefaultOption("No auto", new WaitCommand(15)); - autoChooser.addOption("High cone and balance", highConeAndBalanceAuto); + autoChooser.addOption("High cube and balance", highConeAndBalanceAuto); autoChooser.addOption("High cone and drive straight", highConeAndDriveAuto); autoChooser.addOption("Only high cone score", highConeAuto); + autoChooser.addOption("Only drive straight", driveStraightAuto); SmartDashboard.putData("Auto Chooser", autoChooser); } @@ -142,12 +144,12 @@ private void configureButtonBindings() { left_controller.button(1).onTrue(new InstantCommand(m_drivetrainSubsystem::zeroGyroscope)); - operatorController.a().onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem))); - operatorController.y().onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem)); - operatorController.b().onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, () -> operatorController.leftBumper().getAsBoolean())); - operatorController.x().onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem)); - operatorController.povUp().onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean(), ledSubsystem)); - operatorController.povDown().onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem, ledSubsystem)); + operatorController.a().and(() -> sm.getAtHome()).onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem, sm))); + operatorController.y().and(() -> sm.getAtHome()).onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem, sm)); + operatorController.b().and(() -> sm.getAtHome()).onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, () -> operatorController.leftBumper().getAsBoolean(), sm)); + operatorController.x().and(() -> !sm.getAtHome()).onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, sm)); + operatorController.povUp().and(() -> sm.getAtHome()).onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean(), ledSubsystem, sm)); + operatorController.povDown().and(() -> sm.getAtHome()).onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem, ledSubsystem, sm)); new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.05) @@ -155,7 +157,9 @@ private void configureButtonBindings() { new Trigger(() -> operatorController.getRightTriggerAxis() > 0.05) .whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> operatorController.getRightTriggerAxis())); - left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY)); + //left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY)); + right_controller.button(1).whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> 1.0)); + left_controller.button(3).whileTrue(new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = true)).whileFalse( new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = false)); } @@ -166,10 +170,8 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - //return rightConeAuto; - // return leftConeAuto; //return null; - return autoChooser.getSelected(); + return autoChooser.getSelected(); //return highConeAndBalanceAuto; } @@ -187,6 +189,14 @@ public Command getGoToZeroWristCommand() { return new WristGoToPositionCommand(wristSubsystem, Constants.HOME_POSITION_WRIST); } + public Command getIntakeHoldCommand() { + return new IntakeHoldCommand(intakeSubsystem); + } + + public Command setSMHomeCommand() { + return new InstantCommand( () -> sm.setAtHome(true)); + } + private static double deadband(double value, double deadband) { if (Math.abs(value) > deadband) { if (value > 0.0) { diff --git a/src/main/java/frc/robot/autos/AutoBalanceCommand.java b/src/main/java/frc/robot/autos/AutoBalanceCommand.java index 78a1c4e..889fdee 100644 --- a/src/main/java/frc/robot/autos/AutoBalanceCommand.java +++ b/src/main/java/frc/robot/autos/AutoBalanceCommand.java @@ -42,7 +42,7 @@ public void execute() { @Override public void end(boolean interrupted) { - this.speeds.vyMetersPerSecond = 0.01; + this.speeds.omegaRadiansPerSecond = 0.0001; dt.drive(this.speeds); } diff --git a/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java b/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java index 8ac2f19..c1c4a18 100644 --- a/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java +++ b/src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java @@ -4,7 +4,8 @@ import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.autos.basic.Drive; -import frc.robot.autos.basic.DriveSpeed; +import frc.robot.autos.basic.LockWheels; +import frc.robot.autos.procedures.ConeScoreHighAuto; import frc.robot.commands.arm.ArmHoldCommand; import frc.robot.commands.assembly.AssemblyHomePositionCommand; import frc.robot.commands.assembly.autoAssembly.AutoAssemblyConeHighScoreCommand; @@ -17,6 +18,7 @@ import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShoulderSubsystem; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; import frc.robot.subsystems.WristSubsystem; public class ConeHighAndDriveAuto extends SequentialCommandGroup { @@ -24,12 +26,11 @@ public class ConeHighAndDriveAuto extends SequentialCommandGroup { public ConeHighAndDriveAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, ArmShoulderMessenger armMessenger, - LEDSubsystem ledSubsystem) { + LEDSubsystem ledSubsystem, + StateMachine sm) { - addCommands(new AutoAssemblyConeHighScoreCommand(shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger), - // .raceWith(new DriveSpeed(dt, -0.5, 0.0)), // New - To be tested!!!! - new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, ledSubsystem), - new Drive(dt, 8.0, 0.0) + addCommands(new ConeScoreHighAuto(dt, shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger, ledSubsystem, sm), + new Drive(dt, 8.2, 0.0) .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) .raceWith(new WristHoldCommand(wrist, () -> 0.0)) .raceWith(new ArmHoldCommand(arm)) diff --git a/src/main/java/frc/robot/autos/ConeHighAndBalanceAuto.java b/src/main/java/frc/robot/autos/CubeHighAndBalanceAuto.java similarity index 74% rename from src/main/java/frc/robot/autos/ConeHighAndBalanceAuto.java rename to src/main/java/frc/robot/autos/CubeHighAndBalanceAuto.java index cc3593e..b699de8 100644 --- a/src/main/java/frc/robot/autos/ConeHighAndBalanceAuto.java +++ b/src/main/java/frc/robot/autos/CubeHighAndBalanceAuto.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.autos.basic.DriveEncoder; +import frc.robot.autos.procedures.CubeScoreHighAuto; import frc.robot.commands.arm.ArmHoldCommand; import frc.robot.commands.shoulder.ShoulderHoldCommand; import frc.robot.commands.wrist.WristHoldCommand; @@ -9,18 +10,20 @@ import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShoulderSubsystem; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; import frc.robot.subsystems.WristSubsystem; -public class ConeHighAndBalanceAuto extends SequentialCommandGroup { +public class CubeHighAndBalanceAuto extends SequentialCommandGroup { - public ConeHighAndBalanceAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, + public CubeHighAndBalanceAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, - ArmShoulderMessenger armMessenger) { + ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, StateMachine sm) { - addCommands(//new ConeScoreHighAuto(shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger), - new DriveEncoder(dt, 3.5, 0.0) + addCommands(new CubeScoreHighAuto(dt, shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger, ledSubsystem, sm), + new DriveEncoder(dt, 4.0, 0.0) .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) .raceWith(new WristHoldCommand(wrist, () -> 0.0)) .raceWith(new ArmHoldCommand(arm)), diff --git a/src/main/java/frc/robot/autos/Right2CargoBalance.java b/src/main/java/frc/robot/autos/DriveStraightAuto.java similarity index 50% rename from src/main/java/frc/robot/autos/Right2CargoBalance.java rename to src/main/java/frc/robot/autos/DriveStraightAuto.java index cc1f582..31634bd 100644 --- a/src/main/java/frc/robot/autos/Right2CargoBalance.java +++ b/src/main/java/frc/robot/autos/DriveStraightAuto.java @@ -4,9 +4,12 @@ import frc.robot.autos.basic.Drive; import frc.robot.subsystems.DrivetrainSubsystem; -public class Right2CargoBalance extends SequentialCommandGroup { +public class DriveStraightAuto extends SequentialCommandGroup { - public Right2CargoBalance(DrivetrainSubsystem dt) { - //addCommands(new Drive()); + public DriveStraightAuto(DrivetrainSubsystem dt) { + + addCommands( + new Drive(dt, 8.0, 0.0) + ); } } diff --git a/src/main/java/frc/robot/autos/LeftSide2ConeAuto.java b/src/main/java/frc/robot/autos/LeftSide2ConeAuto.java deleted file mode 100644 index fd40c2f..0000000 --- a/src/main/java/frc/robot/autos/LeftSide2ConeAuto.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.autos; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.autos.basic.Direction; -import frc.robot.autos.basic.DriveAndRotate; -//import frc.robot.autos.builderSequences.CenterToScore; -import frc.robot.autos.procedures.GetAndScoreCone; -import frc.robot.subsystems.DrivetrainSubsystem; - -public class LeftSide2ConeAuto extends SequentialCommandGroup { - - public LeftSide2ConeAuto(DrivetrainSubsystem dt) { - // Note: Rotation doesn't work in the simulator! But something to think out in real world - // Relative to starting position - addCommands(/* Command to score preload */ - new DriveAndRotate(dt, 0.0, 0.0, 90.0) - // new GetAndScoreCone(dt, Direction.FRONT_CENTER, Direction.LEFT), - // new CenterToScore(dt, Direction.RIGHT), // To reset - // new GetAndScoreCone(dt, Direction.RIGHT, Direction.RIGHT) - ); - } -} diff --git a/src/main/java/frc/robot/autos/RightSide2ConeAuto.java b/src/main/java/frc/robot/autos/RightSide2ConeAuto.java deleted file mode 100644 index 1d756fb..0000000 --- a/src/main/java/frc/robot/autos/RightSide2ConeAuto.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.autos; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.autos.basic.Direction; -import frc.robot.autos.basic.Drive; -import frc.robot.autos.basic.DriveAndRotate; -import frc.robot.autos.basic.Rotate; -import frc.robot.autos.procedures.GetAndScoreCone; -import frc.robot.subsystems.DrivetrainSubsystem; - -public class RightSide2ConeAuto extends SequentialCommandGroup { - - public RightSide2ConeAuto(DrivetrainSubsystem dt) { - // Note: Rotation doesn't work in the simulator! But something to think out in real world - // Relative to starting position - - addCommands(/* Command to score preload */ - // new GetAndScoreCone(dt, Direction.FRONT_CENTER, Direction.RIGHT), - // new CenterToScore(dt, Direction.LEFT), - // new GetAndScoreCone(dt, Direction.LEFT, Direction.LEFT) - //new Drive(dt, 7.0, 0.0) - new DriveAndRotate(dt, 0.0, 0.0, 179.0) - //new Drive(dt, -7.0, 0.0) - //new Drive(dt, 0.0, -1.0) - ); - - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/autos/basic/DriveSpeed.java b/src/main/java/frc/robot/autos/basic/LockWheels.java similarity index 57% rename from src/main/java/frc/robot/autos/basic/DriveSpeed.java rename to src/main/java/frc/robot/autos/basic/LockWheels.java index 0500fdb..0755d46 100644 --- a/src/main/java/frc/robot/autos/basic/DriveSpeed.java +++ b/src/main/java/frc/robot/autos/basic/LockWheels.java @@ -4,17 +4,13 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DrivetrainSubsystem; -public class DriveSpeed extends CommandBase { +public class LockWheels extends CommandBase { private DrivetrainSubsystem dt; - private double xSpeed; - private double ySpeed; private ChassisSpeeds speeds; - public DriveSpeed(DrivetrainSubsystem dt, double xSpeed, double ySpeed) { + public LockWheels(DrivetrainSubsystem dt) { this.dt = dt; - this.xSpeed = xSpeed; - this.ySpeed = ySpeed; this.speeds = new ChassisSpeeds(0.0, 0.0, 0.0); addRequirements(dt); @@ -22,8 +18,17 @@ public DriveSpeed(DrivetrainSubsystem dt, double xSpeed, double ySpeed) { @Override public void execute() { - speeds.vxMetersPerSecond = this.xSpeed; - speeds.vyMetersPerSecond = this.ySpeed; + this.speeds.omegaRadiansPerSecond = 0.0001; dt.drive(speeds); } + + @Override + public void end(boolean interrupt) { + } + + @Override + public boolean isFinished() { + //return this.speeds.vyMetersPerSecond > 0.0; + return false; + } } diff --git a/src/main/java/frc/robot/autos/ConeScoreHighAuto.java b/src/main/java/frc/robot/autos/procedures/ConeScoreHighAuto.java similarity index 71% rename from src/main/java/frc/robot/autos/ConeScoreHighAuto.java rename to src/main/java/frc/robot/autos/procedures/ConeScoreHighAuto.java index f88fe12..8a5d9a1 100644 --- a/src/main/java/frc/robot/autos/ConeScoreHighAuto.java +++ b/src/main/java/frc/robot/autos/procedures/ConeScoreHighAuto.java @@ -1,4 +1,4 @@ -package frc.robot.autos; +package frc.robot.autos.procedures; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -8,29 +8,26 @@ import frc.robot.commands.shoulder.ShoulderHoldCommand; import frc.robot.commands.wrist.WristHoldCommand; import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShoulderSubsystem; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; import frc.robot.subsystems.WristSubsystem; public class ConeScoreHighAuto extends SequentialCommandGroup { - public ConeScoreHighAuto(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, + public ConeScoreHighAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, - ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem) + ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, StateMachine sm) { addCommands( - new AutoAssemblyConeHighScoreCommand(shoulder, shoulderWristMessenger, wrist, - arm, intake, armMessenger), + new AutoAssemblyConeHighScoreCommand(dt, shoulder, shoulderWristMessenger, wrist, + arm, intake, ledSubsystem, armMessenger, sm), new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, - wrist, arm, armMessenger, ledSubsystem) - /* - new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0) - .raceWith(new WristHoldCommand(wrist, () -> 0.0)) - .raceWith(new ArmHoldCommand(arm)) - */ + wrist, arm, armMessenger, ledSubsystem, sm) ); } } diff --git a/src/main/java/frc/robot/autos/procedures/CubeScoreHighAuto.java b/src/main/java/frc/robot/autos/procedures/CubeScoreHighAuto.java new file mode 100644 index 0000000..5bfafe4 --- /dev/null +++ b/src/main/java/frc/robot/autos/procedures/CubeScoreHighAuto.java @@ -0,0 +1,48 @@ +package frc.robot.autos.procedures; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.autos.basic.LockWheels; +import frc.robot.commands.arm.ArmHoldCommand; +import frc.robot.commands.assembly.AssemblyHighScoreCommand; +import frc.robot.commands.assembly.AssemblyHomePositionCommand; +import frc.robot.commands.intake.AutoIntakeCommand; +import frc.robot.commands.intake.AutoPutDownCommand; +import frc.robot.commands.intake.IntakeHoldCommand; +import frc.robot.commands.shoulder.ShoulderHoldCommand; +import frc.robot.commands.wrist.WristHoldCommand; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ShoulderSubsystem; +import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; +import frc.robot.subsystems.WristSubsystem; + +public class CubeScoreHighAuto extends SequentialCommandGroup { + + public CubeScoreHighAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, + WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, + ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, StateMachine sm) { + + addCommands( + + new AutoIntakeCommand(intake, 0.25) + .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) + .raceWith(new WristHoldCommand(wrist, () -> 0.0)) + .raceWith(new ArmHoldCommand(arm)), + + new AssemblyHighScoreCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, () -> true, ledSubsystem, sm) + .raceWith(new LockWheels(dt)) + .raceWith(new IntakeHoldCommand(intake)), + + new AutoPutDownCommand(intake, 0.8) + .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) + .raceWith(new WristHoldCommand(wrist, () -> 0.0)) + .raceWith(new ArmHoldCommand(arm)), + + new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, ledSubsystem, sm) + ); + } +} diff --git a/src/main/java/frc/robot/autos/procedures/GetAndScoreCone.java b/src/main/java/frc/robot/autos/procedures/GetAndScoreCone.java deleted file mode 100644 index fe2b149..0000000 --- a/src/main/java/frc/robot/autos/procedures/GetAndScoreCone.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.autos.procedures; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.autos.basic.Direction; -import frc.robot.autos.basic.Drive; -import frc.robot.subsystems.DrivetrainSubsystem; - -public class GetAndScoreCone extends SequentialCommandGroup { - - public GetAndScoreCone(DrivetrainSubsystem dt, Direction gameObjectPosition, Direction scorePosition) { - // addCommands(new DriveToPickUp(dt, gameObjectPosition), - // /* Command to pickup */ - // new DriveBackFromPickUp(dt, gameObjectPosition.getOpposite()), - // new CenterToScore(dt, scorePosition) - // /* Command to score */ - // /* Command to lower (?)*/ - // ); - } -} diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyGoToConeIntakeCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyGoToConeIntakeCommand.java index 01d8bef..98a4a19 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyGoToConeIntakeCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyGoToConeIntakeCommand.java @@ -16,14 +16,17 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyGoToConeIntakeCommand extends SequentialCommandGroup { public AssemblyGoToConeIntakeCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, IntakeSubsystem intake, - LEDSubsystem ledSubsystem) { + LEDSubsystem ledSubsystem, + StateMachine sm) { addCommands( + new InstantCommand( () -> sm.setAtHome(false)), new InstantCommand( () -> shoulder.setInIntakePosition(false)), new InstantCommand( () -> intake.setAtSubstationState(false)), new InstantCommand(ledSubsystem::setLEDDisable), diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyGoToCubeIntakeCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyGoToCubeIntakeCommand.java index 09a5690..e90cc9b 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyGoToCubeIntakeCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyGoToCubeIntakeCommand.java @@ -16,15 +16,18 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyGoToCubeIntakeCommand extends SequentialCommandGroup { public AssemblyGoToCubeIntakeCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, IntakeSubsystem intake, - LEDSubsystem ledSubsystem) { + LEDSubsystem ledSubsystem, + StateMachine sm) { addCommands( + new InstantCommand( () -> sm.setAtHome(false)), new InstantCommand( () -> shoulder.setInIntakePosition(false)), new InstantCommand( () -> intake.setAtSubstationState(false)), new InstantCommand(ledSubsystem::setLEDDisable), diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyHighScoreCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyHighScoreCommand.java index c5ce66d..0ee648f 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyHighScoreCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyHighScoreCommand.java @@ -18,14 +18,17 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyHighScoreCommand extends SequentialCommandGroup { public AssemblyHighScoreCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, BooleanSupplier scoreCube, - LEDSubsystem ledSubsystem) { + LEDSubsystem ledSubsystem, + StateMachine sm) { addCommands( + new InstantCommand( () -> sm.setAtHome(false)), new InstantCommand( () -> shoulder.setInIntakePosition(false)), new InstantCommand(ledSubsystem::setLEDDisable), diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyHomePositionCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyHomePositionCommand.java index 0bfb56d..0f0cfe6 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyHomePositionCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyHomePositionCommand.java @@ -15,11 +15,13 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyHomePositionCommand extends SequentialCommandGroup { public AssemblyHomePositionCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, - WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem) { + WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, + StateMachine sm) { addCommands( new InstantCommand( () -> shoulder.setInIntakePosition(false)), new InstantCommand(ledSubsystem::setLEDDisable), @@ -34,7 +36,8 @@ public AssemblyHomePositionCommand(ShoulderSubsystem shoulder, ShoulderWristMess .raceWith(new WristHoldCommand(wrist, () -> 0.0)) .raceWith(new ArmHoldCommand(arm)), - new InstantCommand(ledSubsystem::setLEDEnable) + new InstantCommand(ledSubsystem::setLEDEnable), + new InstantCommand( () -> sm.setAtHome(true)) ); diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyMidScoreCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyMidScoreCommand.java index 10319f4..f303ab7 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyMidScoreCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyMidScoreCommand.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; -import frc.robot.autos.LeftSide2ConeAuto; import frc.robot.commands.arm.ArmGoToPositionCommand; import frc.robot.commands.arm.ArmHoldCommand; import frc.robot.commands.shoulder.ShoulderGoToPositionCommand; @@ -19,14 +18,18 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyMidScoreCommand extends SequentialCommandGroup { public AssemblyMidScoreCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, WristSubsystem wrist, ArmSubsystem arm, ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, - BooleanSupplier scoreCube) { + BooleanSupplier scoreCube, + StateMachine sm) { + addCommands( + new InstantCommand( () -> sm.setAtHome(false)), new InstantCommand(() -> shoulder.setInIntakePosition(false)), new InstantCommand(ledSubsystem::setLEDDisable), diff --git a/src/main/java/frc/robot/commands/assembly/AssemblyPickUpSingleSubstationCommand.java b/src/main/java/frc/robot/commands/assembly/AssemblyPickUpSingleSubstationCommand.java index a62638a..07272c9 100644 --- a/src/main/java/frc/robot/commands/assembly/AssemblyPickUpSingleSubstationCommand.java +++ b/src/main/java/frc/robot/commands/assembly/AssemblyPickUpSingleSubstationCommand.java @@ -16,13 +16,15 @@ import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AssemblyPickUpSingleSubstationCommand extends SequentialCommandGroup { public AssemblyPickUpSingleSubstationCommand(ShoulderSubsystem shoulder, WristSubsystem wrist, ArmSubsystem arm, - ShoulderWristMessenger shoulderWristMessenger, ArmShoulderMessenger armMessenger, IntakeSubsystem intake, LEDSubsystem ledSubsystem) { + ShoulderWristMessenger shoulderWristMessenger, ArmShoulderMessenger armMessenger, IntakeSubsystem intake, LEDSubsystem ledSubsystem, StateMachine sm) { addCommands( + new InstantCommand( () -> sm.setAtHome(false)), new InstantCommand( () -> shoulder.setInIntakePosition(false)), new InstantCommand( () -> intake.setAtSubstationState(true)), new InstantCommand(ledSubsystem::setLEDDisable), diff --git a/src/main/java/frc/robot/commands/assembly/autoAssembly/AutoAssemblyConeHighScoreCommand.java b/src/main/java/frc/robot/commands/assembly/autoAssembly/AutoAssemblyConeHighScoreCommand.java index daf025a..3d5d98e 100644 --- a/src/main/java/frc/robot/commands/assembly/autoAssembly/AutoAssemblyConeHighScoreCommand.java +++ b/src/main/java/frc/robot/commands/assembly/autoAssembly/AutoAssemblyConeHighScoreCommand.java @@ -1,9 +1,13 @@ package frc.robot.commands.assembly.autoAssembly; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; +import frc.robot.autos.basic.LockWheels; import frc.robot.commands.arm.ArmGoToPositionCommand; import frc.robot.commands.arm.ArmHoldCommand; +import frc.robot.commands.assembly.AssemblyHighScoreCommand; import frc.robot.commands.intake.AutoPutDownCommand; import frc.robot.commands.intake.IntakeHoldCommand; import frc.robot.commands.shoulder.ShoulderGoToPositionCommand; @@ -11,34 +15,25 @@ import frc.robot.commands.wrist.WristGoToPositionCommand; import frc.robot.commands.wrist.WristHoldCommand; import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShoulderSubsystem; import frc.robot.subsystems.WristSubsystem; import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger; import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger; +import frc.robot.util.StateMachine; public class AutoAssemblyConeHighScoreCommand extends SequentialCommandGroup { - public AutoAssemblyConeHighScoreCommand(ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, - WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, - ArmShoulderMessenger armMessenger) { + public AutoAssemblyConeHighScoreCommand(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, + WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, + LEDSubsystem ledSubsystem, ArmShoulderMessenger armMessenger, StateMachine sm) { addCommands( - new InstantCommand( () -> shoulder.setInIntakePosition(false)), + new AssemblyHighScoreCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, () -> false, ledSubsystem, sm) + .raceWith(new LockWheels(dt)), - new ShoulderGoToPositionCommand(shoulder, Constants.CONE_SCORE_HIGH_POSITION_SHOULDER) - .raceWith(new WristHoldCommand(wrist, () -> 0.0)) - .raceWith(new ArmHoldCommand(arm)) - .raceWith(new IntakeHoldCommand(intake)), - - new WristGoToPositionCommand(wrist, Constants.CONE_SCORE_HIGH_POSITION_WRIST) - .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) - .raceWith(new ArmHoldCommand(arm)) - .raceWith(new IntakeHoldCommand(intake)), - - new ArmGoToPositionCommand(arm, shoulderWristMessenger, Constants.CONE_SCORE_HIGH_POSITION_ARM) - .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) - .raceWith(new WristHoldCommand(wrist, () -> 0.0)) - .raceWith(new IntakeHoldCommand(intake)), + new AutoPutDownCommand(intake, 0.8) .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0)) diff --git a/src/main/java/frc/robot/commands/intake/AutoIntakeCommand.java b/src/main/java/frc/robot/commands/intake/AutoIntakeCommand.java index f672f45..1882ba3 100644 --- a/src/main/java/frc/robot/commands/intake/AutoIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/AutoIntakeCommand.java @@ -35,7 +35,7 @@ public void end(boolean interruptible) { @Override public boolean isFinished() { - return this.timer.getTimeDeltaSec() > 1.0; + return this.timer.getTimeDeltaSec() > 0.25; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java b/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java index 039dc5f..d3ce597 100644 --- a/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java +++ b/src/main/java/frc/robot/commands/intake/AutoPutDownCommand.java @@ -34,7 +34,7 @@ public void end(boolean interruptible) { @Override public boolean isFinished() { - return this.timer.getTimeDeltaSec() > 1.0; + return this.timer.getTimeDeltaSec() > 0.3; } diff --git a/src/main/java/frc/robot/commands/shoulder/ShoulderHoldCommand.java b/src/main/java/frc/robot/commands/shoulder/ShoulderHoldCommand.java index 8334b59..a05e38e 100644 --- a/src/main/java/frc/robot/commands/shoulder/ShoulderHoldCommand.java +++ b/src/main/java/frc/robot/commands/shoulder/ShoulderHoldCommand.java @@ -43,7 +43,7 @@ public void execute() { double speed = speedOnDistance; - if (this.intakeSpeed.getAsDouble() > 0.1 && this.shoulder.getInIntakePosition()) speed = -0.1; // This is to drive the shoulder into the ground while intaking + if (this.intakeSpeed.getAsDouble() > 0.1 && this.shoulder.getInIntakePosition()) speed = -0.08; // This is to drive the shoulder into the ground while intaking this.shoulder.setShoulderNormalizedVoltage(speed); diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 9288880..eb89398 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -85,6 +85,8 @@ public class DrivetrainSubsystem extends SubsystemBase { private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + public boolean lockWheels; + //private SwerveDriveOdometry2022 odometry = new SwerveDriveOdometry2022(m_kinematics, getGyroscopeRotation(), new Pose2d(2.3, 4.5, Rotation2d.fromDegrees(0))); private SwerveDriveOdometry2022 odometry = new SwerveDriveOdometry2022(m_kinematics, getGyroscopeRotation(), new Pose2d(2.3, 1.5, Rotation2d.fromDegrees(0))); @@ -93,6 +95,8 @@ public class DrivetrainSubsystem extends SubsystemBase { public OrbitPID driveRotPID; public DrivetrainSubsystem() { + lockWheels = false; + ShuffleboardTab tab = Shuffleboard.getTab("Drivetrain"); m_frontLeftModule = Mk4iSwerveModuleHelper.createNeo( @@ -203,6 +207,22 @@ public OrbitPID getDrivePID() { @Override public void periodic() { SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(m_chassisSpeeds); + + //SwerveModuleState[] xstates = new SwerveModuleState[4]; + //xstates[0] = new SwerveModuleState + + if(lockWheels) { + for(int i = 0; i < 4; i++) { + states[i].speedMetersPerSecond = 0.0; + } + + states[0].angle = Rotation2d.fromDegrees(45.0); + states[1].angle = Rotation2d.fromDegrees(-45.0); + states[2].angle = Rotation2d.fromDegrees(-45.0); + states[3].angle = Rotation2d.fromDegrees(45.0); + } + + SwerveDriveKinematics.desaturateWheelSpeeds(states, MAX_VELOCITY_METERS_PER_SECOND); // this was normalizeWheelSpeeds but WPILib doesn't like that m_frontLeftModule.set(states[0].speedMetersPerSecond / MAX_VELOCITY_METERS_PER_SECOND * MAX_VOLTAGE, states[0].angle.getRadians()); diff --git a/src/main/java/frc/robot/util/StateMachine.java b/src/main/java/frc/robot/util/StateMachine.java new file mode 100644 index 0000000..5aafc05 --- /dev/null +++ b/src/main/java/frc/robot/util/StateMachine.java @@ -0,0 +1,23 @@ +package frc.robot.util; + +public class StateMachine { + + boolean atHome; + boolean atMid; + boolean atHigh; + boolean atLow; + boolean atIntake; + + public StateMachine() { + atHome = false; + } + + public void setAtHome(boolean atHome) { + this.atHome = atHome; + } + + public boolean getAtHome() { + return this.atHome; + } + +}