diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index af39c6a..3ba8130 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,4 +175,4 @@ public final class Constants { public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a63c659..0111703 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -84,7 +84,6 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { - m_robotContainer.m_robotSwerveDrive.resetGyro(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -103,7 +102,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.m_robotSwerveDrive.resetGyro(); + // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to @@ -112,7 +111,7 @@ public class Robot extends TimedRobot { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } - m_robotContainer.m_robotSwerveDrive.resetGyro(); + m_robotTime.startMatchTime(); m_robotContainer.m_robotMap.restart_motor_tests(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cd728ce..9aacf61 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.util.function.Consumer; import edu.wpi.first.math.geometry.Translation2d; import com.revrobotics.CANSparkMax; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.revrobotics.REVLibError; import com.revrobotics.CANSparkMax.FaultID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -24,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.MotorTest; import frc4388.robot.subsystems.Claw; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; @@ -50,11 +52,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, - m_robotMap.rightFront, - m_robotMap.leftBack, - m_robotMap.rightBack, - m_robotMap.gyro); + public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); @@ -68,23 +66,13 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - - // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); - - // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"); - // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1); - // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - - // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); - + // TODO: PUT PARAMETERS FOR MOTORS + MotorTest m_motortest = new MotorTest(new WPI_TalonFX(1), new WPI_TalonFX(1)); private PlaybackChooser playbackChooser; /* Commands */ private Command emptyCommand = new InstantCommand(); - private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight); - + private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135)); private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw); @@ -92,78 +80,8 @@ public class RobotContainer { private boolean readyForPlacement = false; private Boolean isPole = null; - private SequentialCommandGroup alignToPole = - new SequentialCommandGroup( - new RotateToAngle(m_robotSwerveDrive, 0.0), - new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), - new RotateToAngle(m_robotSwerveDrive, 0.0), - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); - // private SequentialCommandGroup alignToShelf = - // new SequentialCommandGroup( - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), - // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - // new RotateToAngle(m_robotSwerveDrive, 0.0), - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); - - private SequentialCommandGroup alignToShelf = - new SequentialCommandGroup( - new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), - new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), - new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), - new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) - ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); - public SequentialCommandGroup place = null; - - private Consumer queuePlacement = (scg) -> { - place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null)); - }; - - // TODO: find actual values - private SequentialCommandGroup placeCubeHigh = - new SequentialCommandGroup( - // new InstantCommand(() -> System.out.println("Placing cone high")), - new PivotCommand(m_robotArm, 64 + 135), - new InstantCommand(() -> m_robotArm.setRotVel(0)), - new TeleCommand(m_robotArm, 95642), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 70 + 135), - new TeleCommand(m_robotArm, 32866), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); - - private SequentialCommandGroup placeLow = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), - toggleClaw.asProxy(), - armToHome.asProxy() - ); /** @@ -172,30 +90,14 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); - // * Default Commands - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - m_robotArm.setDefaultCommand(new RunCommand(() -> { - m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); - m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); + m_motortest.setDefaultCommand(new RunCommand(() -> { + m_motortest.motorSetSpeed(getDeadbandedDriverController().getLeftX()); }, m_robotArm) .withName("Arm DefaultCommand")); // * Auto Commands - chooser.setDefaultOption("NoAuto", emptyCommand); - chooser.addOption("alignToPole", alignToPole); - chooser.addOption("alignToShelf", alignToShelf); - chooser.addOption("placeConeHigh", placeConeHigh); - chooser.addOption("placeConeMid", placeConeMid); - chooser.addOption("placeCubeHigh", placeCubeHigh); - chooser.addOption("placeCubeMid", placeCubeMid); - chooser.addOption("placeLow", placeLow); // chooser.addOption("Blue1Path", blue1Path); // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); @@ -203,9 +105,7 @@ public class RobotContainer { // chooser.addOption("Red1Path", red1Path); // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); - playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)) - .buildDisplay(); + } // here be dragons - enter if you dare @@ -222,14 +122,8 @@ public class RobotContainer { */ private void configureButtonBindings() { // * Driver Buttons - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final - - // because closure reasons - ask Daniel Thomas - // final TapState tap = new TapState(); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // * Driver Buttons + // .onTrue(new InstantCommand(() -> { // tap.gearTapped = true; // tap.gearTime = System.currentTimeMillis(); @@ -247,85 +141,8 @@ public class RobotContainer { // m_robotSwerveDrive.setToSlow(); // })); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final - - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(interruptCommand.asProxy()); // final - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - // () -> getDeadbandedDriverController().getLeftX(), - // () -> getDeadbandedDriverController().getLeftY(), - // () -> getDeadbandedDriverController().getRightX(), - // () -> getDeadbandedDriverController().getRightY(), - // "Blue1Path.txt")) - // .onFalse(new InstantCommand()); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - // .onFalse(new InstantCommand()); - - // * Operator Buttons - - // align (pole) - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final - .onTrue(alignToPole) - .onFalse(interruptCommand.asProxy()); - - // align (shelf) - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final - .onTrue(alignToShelf) - .onFalse(interruptCommand.asProxy()); - - // toggle claw - new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final - .onTrue(toggleClaw.asProxy()); - - // kill soft limits - new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - - // SmartDashboard.putBoolean("isn't SparkMAX", ); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin())) - .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); - - //Arm to Home - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(armToHome.asProxy()); - - //Interupt Button - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(interruptCommand.asProxy()); - - - // place high - new POVButton(getDeadbandedOperatorController(), 0) - .onTrue(new ConditionalCommand( - new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), - emptyCommand.asProxy(), - () -> readyForPlacement == true) - ); - - // place mid - new POVButton(getDeadbandedOperatorController(), 270) - .onTrue(new ConditionalCommand( - new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), - emptyCommand.asProxy(), - () -> readyForPlacement == true) - ); - - // place low - new POVButton(getDeadbandedOperatorController(), 180) - .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true)); - - // confirm - new POVButton(getDeadbandedOperatorController(), 90) - .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null)); } diff --git a/src/main/java/frc4388/robot/subsystems/MotorTest.java b/src/main/java/frc4388/robot/subsystems/MotorTest.java new file mode 100644 index 0000000..e1d1d19 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/MotorTest.java @@ -0,0 +1,27 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +public class MotorTest extends SubsystemBase { + /** Creates a new MotorTest. */ + WPI_TalonFX motor1; + WPI_TalonFX motor2; + public MotorTest(WPI_TalonFX motor1, WPI_TalonFX motor2) { + this.motor1 = motor1; + this.motor2 = motor2; + } + + + public double motorSetSpeed(double speed) { + motor1.set(speed); + motor2.set(speed); + return speed; + } + public void stop() { + motor1.set(0); + motor2.set(0); + } + + +} \ No newline at end of file