diff --git a/build.gradle b/build.gradle index 3b15497..19ecb03 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.3.2" + id "edu.wpi.first.GradleRIO" version "2023.4.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ddd64cf..b41a35f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,9 +26,6 @@ public static class OperatorConstants { public static final int LEFT_INTAKE = 21; public static final int XBOX_CONTROLLER_PORT = 0; public static final int LINE_BREAKER_PORT = 0; -public static final int BOX_GRABBER = 32; -public static final int CLAW_MOTOR = 30; -public static final int ARM_MOTOR = 31; public static final int XBOX_CONTROLLER_PORT_2 = 1; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 17a5f18..ab1a66c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,19 +15,21 @@ * project. */ public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - private Command autoCommand; - + + private RobotContainer m_robotContainer = null; + private Command autoCommand = null; + private Command m_autonomousCommand = null; + /** * This function is run when the robot is first started up and should be used for any * initialization code. */ + @Override public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); } @@ -48,6 +50,7 @@ public void robotPeriodic() { } /** This function is called once each time the robot enters Disabled mode. */ + @Override public void disabledInit() {} @@ -55,9 +58,9 @@ public void disabledInit() {} public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override public void autonomousInit() { - autoCommand = m_robotContainer.getAutonomousCommand(); System.out.println("auto init"); // schedule the autonomous command (example) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 877db0e..52f3e30 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,13 +3,10 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; - - -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -17,22 +14,21 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AutonomousCommand; -import frc.robot.commands.AutonomousCommandThree; -import frc.robot.commands.AutonomousCommandTwo; -import frc.robot.commands.ClawGrab; +import frc.robot.commands.CubeAdjust; +import frc.robot.commands.CubeAdjustHigh; +import frc.robot.commands.CubeAdjustMid; +import frc.robot.commands.CubeShoot; +import frc.robot.commands.CubeShootFull; +import frc.robot.commands.CubeScoreMid; import frc.robot.commands.DriveArcadeCustomized; -import frc.robot.commands.DriveFast; -import frc.robot.commands.GrabberOne; -import frc.robot.commands.MoveArm; -import frc.robot.commands.MoveClaw; +import frc.robot.commands.IntakeIn; +import frc.robot.commands.IntakeOut; +import frc.robot.commands.RunLeftIntake; +import frc.robot.commands.RunRightIntake; +import frc.robot.commands.Shoot; import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.BoxGrabber; -import frc.robot.subsystems.Claw; -// import frc.robot.commands.GrabberIntake; -// import frc.robot.commands.GrabberOne; -// import frc.robot.subsystems.BoxGrabber; import frc.robot.subsystems.Drivetrain; +import frc.robot.commands.CubeScoreHigh; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -40,39 +36,25 @@ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including * subsystems, commands, and trigger mappings) should be declared here. */ + public class RobotContainer { // The robot's subsystems and commands are defined here... + Intake boxGrabber = new Intake(); - BoxGrabber gBox = new BoxGrabber(); + Drivetrain driveTrain = new Drivetrain(); - Claw claw = new Claw(); - Arm arm = new Arm(); - DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); // add port number in constants file + DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); - XboxController xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); XboxController xboxController2 = new XboxController(Constants.XBOX_CONTROLLER_PORT_2); SendableChooser chooser = new SendableChooser<>(); - // Replace with CommandPS4Controller or CommandJoystick if needed - //AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0.5, -0.5, boxGrabber, 0.75); - AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, gBox, .6); - AutonomousCommandTwo autoCommandTwo= new AutonomousCommandTwo(driveTrain); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, 0.5, boxGrabber, -1, digitalInput, xboxController2); + public RobotContainer() { - // Configure the trigger bindings configureBindings(); - // driveTrain.setDefaultCommand(new DriveArcade(driveTrain, xboxController::getRightX, xboxController::getLeftY)); - //driveTrain.setDefaultCommand(new DriveTank(driveTrain, xboxController::getRightY, xboxController::getLeftY)); - //driveTrain.setDefaultCommand(new DriveCurvature(driveTrain, xboxController::getRightX, xboxController::getLeftY)); - //driveTrain.setDefaultCommand(new DriveTank(driveTrain, xboxController::getRightY, xboxController::getLeftY)); - driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.8, xboxController)); - - driveTrain.putNumbers(); - + driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.3, 0.2, 0.8, xboxController)); chooser.setDefaultOption("Default Auto Command", autoCommand); - chooser.addOption("Second Auto Command", autoCommandTwo); - SmartDashboard.putData(chooser); } @@ -85,23 +67,17 @@ public RobotContainer() { * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - //new JoystickButton(xboxController, Button.kRightBumper.value).whileTrue(new DriveFast(driveTrain, -1)); - //new JoystickButton(xboxController, Button.kLeftBumper.value).whileTrue(new DriveFast(driveTrain, 1)); - new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new GrabberOne(boxGrabber, 1, digitalInput, xboxController2)); - new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new GrabberOne(boxGrabber, -1, digitalInput, xboxController2)); - new JoystickButton(xboxController2, Button.kA.value).whileTrue(new MoveClaw(claw, 0.1)); - new JoystickButton(xboxController2, Button.kB.value).whileTrue(new MoveClaw(claw, -0.1)); - new JoystickButton(xboxController2, Button.kX.value).whileTrue(new ClawGrab(gBox, 0.6)); - new JoystickButton(xboxController2, Button.kY.value).whileTrue(new ClawGrab(gBox, -0.1)); - //new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new MoveArm(arm, 0.1)); - //new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new MoveArm(arm, -0.1)); - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - - } + private void configureBindings() { + new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new IntakeOut(boxGrabber, 0.40)); + new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.25, digitalInput, xboxController2)); + new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -1, -1)); + //new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeAdjustFullMid(digitalInput, boxGrabber, xboxController2)); + new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeScoreHigh(digitalInput, boxGrabber, xboxController2, boxGrabber, -1, -1)); + new JoystickButton(xboxController2, Button.kY.value).whileTrue(new CubeScoreMid(digitalInput, boxGrabber, xboxController2, boxGrabber, -1, -1)); + new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 1)); + + } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index 951f9b4..87ccad5 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -4,26 +4,25 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.BoxGrabber; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutonomousCommand extends SequentialCommandGroup { /** Creates a new AutonomousCommand. */ - public AutonomousCommand(Drivetrain driveTrain, double power, double rotation, BoxGrabber boxGrabber, double intakePower) { + DigitalInput digitalInput; + XboxController xboxController2; + public AutonomousCommand(Drivetrain driveTrain, double power, double rotation, Intake boxGrabber, double intakePower, DigitalInput digitalInput, XboxController xboxController2) { // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new ClawGrab(boxGrabber, intakePower).withTimeout(1), - //new Drive(driveTrain, 0, -rotation).withTimeout(0.75), - new Drive(driveTrain, power, 0).withTimeout(0), - new Drive(driveTrain, 0, rotation).withTimeout(3)); - - + addCommands( + new Drive(driveTrain, power, 0).withTimeout(0.75), + new CubeShoot(boxGrabber, intakePower, -1).withTimeout(1.55), + new Drive(driveTrain, 0,-rotation).withTimeout(1.25)); } } diff --git a/src/main/java/frc/robot/commands/AutonomousCommandTwo.java b/src/main/java/frc/robot/commands/CubeAdjust.java similarity index 69% rename from src/main/java/frc/robot/commands/AutonomousCommandTwo.java rename to src/main/java/frc/robot/commands/CubeAdjust.java index 0df85cb..9bc7cd6 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommandTwo.java +++ b/src/main/java/frc/robot/commands/CubeAdjust.java @@ -5,19 +5,18 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutonomousCommandTwo extends SequentialCommandGroup { - /** Creates a new AutonomousCommandTwo. */ - Drivetrain driveTrain; - public AutonomousCommandTwo(Drivetrain driveTrain) { +public class CubeAdjust extends SequentialCommandGroup { + /** Creates a new CubeAdjust. */ + public CubeAdjust(Intake intake, double power) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new DriveStraight(driveTrain).withTimeout(20) + new RunLeftIntake(intake, power).withTimeout(0.35) ); } } diff --git a/src/main/java/frc/robot/commands/CubeAdjustHigh.java b/src/main/java/frc/robot/commands/CubeAdjustHigh.java new file mode 100644 index 0000000..3d6c5f4 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeAdjustHigh.java @@ -0,0 +1,59 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Intake; + + +public class CubeAdjustHigh extends CommandBase { + DigitalInput digitalInput = null; + boolean done = false; + Intake boxGrabber = null; + XboxController xboxController = null; + + + + + public CubeAdjustHigh(DigitalInput digitalInput, Intake boxGrabber, XboxController xboxController) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(boxGrabber); + this.digitalInput = digitalInput; + this.boxGrabber = boxGrabber; + this.xboxController = xboxController; + } + @Override + public void initialize() { + boxGrabber.stop(); + } + + + @Override + public void execute() { + if (xboxController.getAButton() && !digitalInput.get()) { + boxGrabber.runLeftIntake(0.37); + } + + if(!digitalInput.get()){ + boxGrabber.runLeftIntake(0.37); + } else { + boxGrabber.stop(); + } + /* else if (xboxController.getAButton()) { + boxGrabber.runLeftIntake(-0.5); + } */ + } + + @Override + public void end(boolean interrupted) { + boxGrabber.stop(); + done=true; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + + return false; + } +} diff --git a/src/main/java/frc/robot/commands/CubeAdjustMid.java b/src/main/java/frc/robot/commands/CubeAdjustMid.java new file mode 100644 index 0000000..12a68e7 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeAdjustMid.java @@ -0,0 +1,57 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Intake; + +public class CubeAdjustMid extends CommandBase { + DigitalInput digitalInput = null; + boolean done = false; + Intake boxGrabber = null; + XboxController xboxController = null; + + + + + public CubeAdjustMid(DigitalInput digitalInput, Intake boxGrabber, XboxController xboxController) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(boxGrabber); + this.digitalInput = digitalInput; + this.boxGrabber = boxGrabber; + this.xboxController = xboxController; + } + @Override + public void initialize() { + boxGrabber.stop(); + } + + + @Override + public void execute() { + if (xboxController.getAButton() && !digitalInput.get()) { + boxGrabber.runLeftIntake(0.09); + } + + if(!digitalInput.get()){ + boxGrabber.runLeftIntake(0.09); + } else { + boxGrabber.stop(); + } + /* else if (xboxController.getAButton()) { + boxGrabber.runLeftIntake(-0.5); + } */ + } + + @Override + public void end(boolean interrupted) { + boxGrabber.stop(); + done=true; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/CubeScoreHigh.java b/src/main/java/frc/robot/commands/CubeScoreHigh.java new file mode 100644 index 0000000..cfb95b2 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeScoreHigh.java @@ -0,0 +1,25 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.Intake; + +public class CubeScoreHigh extends SequentialCommandGroup { + /** Creates a new CubeShoot. */ + public CubeScoreHigh(DigitalInput digitalInput, Intake boxGrabber, XboxController xboxController2, Intake intake, double intakePower, double shooterPower) { + + double power = 0.1; + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + + //new CubeAdjustFullHigh(digitalInput, boxGrabber, xboxController2), + new CubeAdjustHigh(digitalInput, boxGrabber, xboxController2).withTimeout(0.30), + new RunLeftIntake(intake, power).withTimeout(0.5), + new CubeShoot( boxGrabber, -1, -1) + ); + + } + } diff --git a/src/main/java/frc/robot/commands/CubeScoreMid.java b/src/main/java/frc/robot/commands/CubeScoreMid.java new file mode 100644 index 0000000..65e12e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeScoreMid.java @@ -0,0 +1,21 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.Intake; + +public class CubeScoreMid extends SequentialCommandGroup { + /** Creates a new CubeShoot. */ + public CubeScoreMid(DigitalInput digitalInput, Intake boxGrabber, XboxController xboxController2, Intake intake, double intakePower, double shooterPower) { + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + + new CubeAdjustMid(digitalInput, boxGrabber, xboxController2).withTimeout(0.8), + new CubeShoot( boxGrabber, -1, -1)); + + } + } diff --git a/src/main/java/frc/robot/commands/AutonomousCommandThree.java b/src/main/java/frc/robot/commands/CubeShoot.java similarity index 64% rename from src/main/java/frc/robot/commands/AutonomousCommandThree.java rename to src/main/java/frc/robot/commands/CubeShoot.java index 247ec83..29de3be 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommandThree.java +++ b/src/main/java/frc/robot/commands/CubeShoot.java @@ -5,20 +5,23 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutonomousCommandThree extends SequentialCommandGroup { - /** Creates a new AutonomousCommandThree. */ - Drivetrain drivetrain; - double distance; - public AutonomousCommandThree(Drivetrain driveTrain, double distance) { +public class CubeShoot extends SequentialCommandGroup { + /** Creates a new CubeShoot. */ + public CubeShoot(Intake intake, double intakePower, double shooterPower) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new DriveToDistance(driveTrain, distance).withTimeout(5) - ); + + new RunRightIntake(intake, intakePower).withTimeout(0.5), + new Shoot(intake, shooterPower).withTimeout(1.5)); + } } + + + diff --git a/src/main/java/frc/robot/commands/CubeShootFull.java b/src/main/java/frc/robot/commands/CubeShootFull.java new file mode 100644 index 0000000..b6e6d7c --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeShootFull.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.Intake; + +public class CubeShootFull extends ParallelCommandGroup { + + // DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); + + public CubeShootFull(Intake intake, double intakePower, double shooterPower, DigitalInput digitalInput) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + + new CubeShoot(intake, intakePower, shooterPower)); + // new CubeAdjustFull(digitalInput)); + + } +} diff --git a/src/main/java/frc/robot/commands/CubeShootPlusAdjustHigh.java b/src/main/java/frc/robot/commands/CubeShootPlusAdjustHigh.java new file mode 100644 index 0000000..5afa86f --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeShootPlusAdjustHigh.java @@ -0,0 +1,24 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.Intake; + +public class CubeShootPlusAdjustHigh extends SequentialCommandGroup { + /** Creates a new CubeShoot. */ + public CubeShootPlusAdjustHigh(DigitalInput digitalInput, Intake boxGrabber, XboxController xboxController2, Intake intake, double intakePower, double shooterPower) { + + double power = 0.5; + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + + //new CubeAdjustFullHigh(digitalInput, boxGrabber, xboxController2), + new CubeAdjust(boxGrabber, 0.15).withTimeout(0.35), + new CubeShoot( boxGrabber, -1, -1) + ); + + } + } diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 4289fff..5e4d497 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -10,22 +10,22 @@ public class Drive extends CommandBase { /** Creates a new Drive. */ - Drivetrain driveTrain; - double power; - double rotation; + Drivetrain driveTrain = null; + double power = 0.0; + double rotation = 0.0; + public Drive(Drivetrain driveTrain, double power, double rotation) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); this.driveTrain = driveTrain; this.power = power; this.rotation = rotation; - } // Called when the command is initially scheduled. @Override public void initialize() { - driveTrain.arcadeDrive(power, rotation); + driveTrain.arcadeDrive(0, 0); System.out.println("init"); } @@ -33,13 +33,14 @@ public void initialize() { @Override public void execute() { driveTrain.arcadeDrive(power, rotation); - System.out.println("exec"); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + driveTrain.arcadeDrive(0, 0); + System.out.println("end"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/DriveArcade.java b/src/main/java/frc/robot/commands/DriveArcade.java deleted file mode 100644 index 0444f50..0000000 --- a/src/main/java/frc/robot/commands/DriveArcade.java +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; - -public class DriveArcade extends CommandBase { - /** Creates a new DriveArcade. */ - Drivetrain driveTrain; - DoubleSupplier speed; - DoubleSupplier rotation; - SlewRateLimiter filter; - SlewRateLimiter filter1; - public DriveArcade(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation) { - this.driveTrain = driveTrain; - this.speed = speed; - this.rotation = rotation; - filter = new SlewRateLimiter(0.9); //0.9 best results - filter1 = new SlewRateLimiter(0.2); - addRequirements(driveTrain); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - driveTrain.arcadeDrive(speed.getAsDouble()*0.75, filter.calculate(rotation.getAsDouble())*0.75); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java index f489189..c38a8a6 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -5,59 +5,75 @@ package frc.robot.commands; import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Drivetrain; public class DriveArcadeCustomized extends CommandBase { /** Creates a new DriveArcadeCustomized. */ - Drivetrain driveTrain; - DoubleSupplier speed; - DoubleSupplier rotation; - SlewRateLimiter filter; - SlewRateLimiter filter1; - XboxController xboxController; - double limit; - public DriveArcadeCustomized(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation, double limit, XboxController xboxController) { + Drivetrain driveTrain = null; + DoubleSupplier speed = null; + DoubleSupplier rotation = null; + XboxController xboxController = null; + double creepRotationLimit = 0.0; + double creepLimit = 0.0; + double fastLimit =0.0; + + + public DriveArcadeCustomized(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation, double creepLimit, double creepRotationLimit, double fastLimit, XboxController xboxController) { // Use addRequirements() here to declare subsystem dependencies. this.driveTrain = driveTrain; this.speed = speed; this.rotation = rotation; - this.limit = limit; + this.creepLimit = creepLimit; + this.creepRotationLimit = creepRotationLimit; + this.fastLimit = fastLimit; this.xboxController = xboxController; - filter = new SlewRateLimiter(0.9); - filter1 = new SlewRateLimiter(0.9); addRequirements(driveTrain); } // Called when the command is initially scheduled. @Override public void initialize() { - - driveTrain.zeroEncoders(); - + driveTrain.arcadeDriveCustomized(0, 0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double speeds = speed.getAsDouble(); + double rotations = rotation.getAsDouble(); + double currentLimit = 0.6; + double currentRotationLimit = 0.3; + double speedInterval = 0.08; + if(xboxController.getRightBumper()){ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*limit, rotation.getAsDouble()*0.3); + currentLimit = fastLimit; + currentRotationLimit = 0.3; } - else if(xboxController.getLeftBumper()){ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*limit, rotation.getAsDouble()*0.3); + else if(xboxController.getLeftBumper()) { + currentLimit = creepLimit; + currentRotationLimit = creepRotationLimit; } - else{ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*0.6, rotation.getAsDouble()*0.3); + + if((speeds >= -speedInterval) && (speeds <= speedInterval)) { + speeds= 0.0; } - driveTrain.putNumbers(); + + if((rotations >= -speedInterval) && (rotations <= speedInterval)) { + rotations = 0.0; + } + + driveTrain.arcadeDriveCustomized(-speeds*currentLimit, rotations*currentRotationLimit); + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + driveTrain.arcadeDriveCustomized(0, 0); + System.out.println("arcade drive ended"); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/DriveCurvature.java b/src/main/java/frc/robot/commands/DriveCurvature.java deleted file mode 100644 index 7209f5f..0000000 --- a/src/main/java/frc/robot/commands/DriveCurvature.java +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; - - -public class DriveCurvature extends CommandBase { - /** Creates a new DriveCurvature. */ - Drivetrain driveTrain; - DoubleSupplier speed; - DoubleSupplier rotation; - public DriveCurvature(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation){ - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - this.speed = speed; - this.rotation = rotation; - addRequirements(driveTrain); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - driveTrain.curvatureDrive(speed.getAsDouble()*0.75, rotation.getAsDouble()*0.75); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DriveStraight.java b/src/main/java/frc/robot/commands/DriveStraight.java deleted file mode 100644 index 740f8d0..0000000 --- a/src/main/java/frc/robot/commands/DriveStraight.java +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; - -public class DriveStraight extends CommandBase { - /** Creates a new DriveStraight. */ - Drivetrain driveTrain; - public DriveStraight(Drivetrain driveTrain) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - addRequirements(driveTrain); - - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.resetGyros(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - driveTrain.putNumbers(); - if(driveTrain.getAngle() > -5 && driveTrain.getAngle() <5){ - driveTrain.arcadeDriveCustomized(0.1, 0); - } - - else if(driveTrain.getAngle() < -5){ - driveTrain.arcadeDriveCustomized(0, 0.1); - } - else{ - driveTrain.arcadeDriveCustomized(0, -0.1); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - driveTrain.stopMotors(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DriveTank.java b/src/main/java/frc/robot/commands/DriveTank.java deleted file mode 100644 index 2c9f0a3..0000000 --- a/src/main/java/frc/robot/commands/DriveTank.java +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; - -public class DriveTank extends CommandBase { - /** Creates a new DriveTank. */ - Drivetrain driveTrain; - DoubleSupplier right; - DoubleSupplier left; - SlewRateLimiter filter; - SlewRateLimiter filter1; - public DriveTank(Drivetrain driveTrain, DoubleSupplier right, DoubleSupplier left) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - this.right = right; - this.left = left; - filter = new SlewRateLimiter(0.9); - filter1 = new SlewRateLimiter(0.9); - addRequirements(driveTrain); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - driveTrain.tankDrive(filter.calculate(right.getAsDouble())*0.5, filter1.calculate(left.getAsDouble())*0.5); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DriveToDistance.java b/src/main/java/frc/robot/commands/DriveToDistance.java deleted file mode 100644 index 2c17e65..0000000 --- a/src/main/java/frc/robot/commands/DriveToDistance.java +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; - -public class DriveToDistance extends CommandBase { - /** Creates a new DriveToDistance. */ - Drivetrain driveTrain; - double distance; - public DriveToDistance(Drivetrain driveTrain, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - this.distance = distance; - addRequirements(driveTrain); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.zeroEncoders(); - driveTrain.resetGyros(); - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - while(driveTrain.getLeftPosition()< distance){ - // if(driveTrain.getLeftPosition() < distance){ - // driveTrain.arcadeDriveCustomized(0.1, 0); - // } - // else{ - // driveTrain.stopMotors(); - // } - double currentPos = distance - driveTrain.getLeftPosition(); - - driveTrain.putNumbers(); - - if(driveTrain.getAngle() > -5 && driveTrain.getAngle() <5){ - driveTrain.arcadeDriveCustomized(0.1, 0); - } - - else if(driveTrain.getAngle() < -5){ - - driveTrain.arcadeDriveCustomized(0, 0.1); - } - else{ - driveTrain.arcadeDriveCustomized(0, -0.1); - } - - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - driveTrain.stopMotors(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/GrabberOne.java b/src/main/java/frc/robot/commands/IntakeIn.java similarity index 64% rename from src/main/java/frc/robot/commands/GrabberOne.java rename to src/main/java/frc/robot/commands/IntakeIn.java index d68069c..b4a1ef0 100644 --- a/src/main/java/frc/robot/commands/GrabberOne.java +++ b/src/main/java/frc/robot/commands/IntakeIn.java @@ -4,43 +4,47 @@ package frc.robot.commands; + import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Intake; -public class GrabberOne extends CommandBase { +public class IntakeIn extends CommandBase { /** Creates a new GrabberOne. */ - Intake boxGrabber; - DigitalInput digitalInput; - double power; - XboxController xboxController; - public GrabberOne(Intake boxGrabber, double power, DigitalInput digitalInput, XboxController xboxController) { + Intake boxGrabber = null; + DigitalInput digitalInput = null; + double power = 0.0; + XboxController xboxController = null; + boolean done = false; + + public IntakeIn(Intake boxGrabber, double power, DigitalInput digitalInput, XboxController xboxController) { // Use addRequirements() here to declare subsystem dependencies. this.boxGrabber = boxGrabber; this.power=power; this.xboxController = xboxController; this.digitalInput = digitalInput; - addRequirements(boxGrabber); } // Called when the command is initially scheduled. @Override public void initialize() { + boxGrabber.stop(); } - + // runRightIntake is the shooter motor, and runLeftIntake is the Intake // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(digitalInput.get()){ - boxGrabber.intakeRun(power*0.75); - } - else if(xboxController.getRightBumper()){ - boxGrabber.intakeRun(0.75); + if(!digitalInput.get()){ + done = true; + System.out.println(done); } - else{ + if(done) { boxGrabber.stop(); + } else if (xboxController.getLeftBumper()) { + boxGrabber.runLeftIntake(power); + boxGrabber.runRightIntake(0.5); } } @@ -48,6 +52,7 @@ else if(xboxController.getRightBumper()){ @Override public void end(boolean interrupted) { boxGrabber.stop(); + done = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ClawGrab.java b/src/main/java/frc/robot/commands/IntakeOut.java similarity index 69% rename from src/main/java/frc/robot/commands/ClawGrab.java rename to src/main/java/frc/robot/commands/IntakeOut.java index 88c84c4..477f1c9 100644 --- a/src/main/java/frc/robot/commands/ClawGrab.java +++ b/src/main/java/frc/robot/commands/IntakeOut.java @@ -4,18 +4,20 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.BoxGrabber; +import frc.robot.subsystems.Intake; -public class ClawGrab extends CommandBase { - /** Creates a new ClawGrab. */ - BoxGrabber boxGrabber; - double power; - public ClawGrab(BoxGrabber boxGrabber, double power) { - addRequirements(boxGrabber); - this.boxGrabber = boxGrabber; - this.power = power; +public class IntakeOut extends CommandBase { + /** Creates a new IntakeOut. */ + Intake intake = null; + double power = 0.0; + + public IntakeOut(Intake intake, double power) { // Use addRequirements() here to declare subsystem dependencies. + this.power = power; + this.intake = intake; + addRequirements(intake); } // Called when the command is initially scheduled. @@ -25,13 +27,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - boxGrabber.setBoxGrabber(power); + intake.intakeRun(power); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - boxGrabber.boxGrabberStop(); + intake.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index d413d02..8dac695 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -11,34 +11,34 @@ public class RunIntake extends CommandBase { /** Creates a new GrabberOne. */ - Intake boxGrabber; - DigitalInput digitalInput; - double power; - XboxController xboxController; + Intake boxGrabber = null; + DigitalInput digitalInput = null; + double power = 0.0; + XboxController xboxController = null; + public RunIntake(Intake boxGrabber, double power) { - // Use addRequirements() here to declare subsystem dependencies. this.boxGrabber = boxGrabber; - this.power=power; - //this.digitalInput = digitalInput; - + this.power = power; addRequirements(boxGrabber); } // Called when the command is initially scheduled. @Override public void initialize() { + boxGrabber.stop(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - boxGrabber.intakeRun(0.75); + boxGrabber.intakeRun(power); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { boxGrabber.stop(); + System.out.println("run intake method stopped"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/MoveArm.java b/src/main/java/frc/robot/commands/RunLeftIntake.java similarity index 67% rename from src/main/java/frc/robot/commands/MoveArm.java rename to src/main/java/frc/robot/commands/RunLeftIntake.java index 5169709..2d8a473 100644 --- a/src/main/java/frc/robot/commands/MoveArm.java +++ b/src/main/java/frc/robot/commands/RunLeftIntake.java @@ -5,33 +5,35 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Intake; -public class MoveArm extends CommandBase { - /** Creates a new MoveArm. */ - Arm arm; - double power; - public MoveArm(Arm arm, double power) { - addRequirements(arm); - this.arm = arm; - this.power = power; +public class RunLeftIntake extends CommandBase { + /** Creates a new RunRightIntake. */ + Intake intake = null; + double power = 0.0; + public RunLeftIntake(Intake intake, double power) { // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intake); + this.power = power; + this.intake = intake; } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + intake.stop(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.moveArm(power); + intake.runLeftIntake(power); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - arm.stopArm(); + intake.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/DriveFast.java b/src/main/java/frc/robot/commands/RunRightIntake.java similarity index 65% rename from src/main/java/frc/robot/commands/DriveFast.java rename to src/main/java/frc/robot/commands/RunRightIntake.java index 8e4ea6e..4a2846d 100644 --- a/src/main/java/frc/robot/commands/DriveFast.java +++ b/src/main/java/frc/robot/commands/RunRightIntake.java @@ -5,33 +5,35 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; -public class DriveFast extends CommandBase { - /** Creates a new DriveFast. */ - Drivetrain driveTrain; - double speed; - public DriveFast(Drivetrain driveTrain, double speed) { +public class RunRightIntake extends CommandBase { + /** Creates a new RunRightIntake. */ + Intake intake = null; + double power = 0.0; + public RunRightIntake(Intake intake, double power) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveTrain); - this.driveTrain = driveTrain; - this.speed=speed; + addRequirements(intake); + this.power = power; + this.intake = intake; } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + intake.stop(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - driveTrain.arcadeDrive(0, speed); + intake.runRightIntake(power); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - driveTrain.stopMotors(); + intake.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/Shoot.java similarity index 71% rename from src/main/java/frc/robot/commands/MoveClaw.java rename to src/main/java/frc/robot/commands/Shoot.java index d3f7028..9d60da8 100644 --- a/src/main/java/frc/robot/commands/MoveClaw.java +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -5,34 +5,35 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Intake; -public class MoveClaw extends CommandBase { - /** Creates a new MoveClaw. */ - Claw claw; - double power; - public MoveClaw(Claw claw, double power) { - addRequirements(claw); - this.claw = claw; - this.power = power; +public class Shoot extends CommandBase { + /** Creates a new Shoot. */ + Intake intake = null; + double power = 0.0; + public Shoot(Intake intake, double power) { // Use addRequirements() here to declare subsystem dependencies. + this.intake = intake; + this.power = power; + addRequirements(intake); } // Called when the command is initially scheduled. @Override public void initialize() { + intake.stop(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - claw.rotateClawUp(power); + intake.intakeRun(power); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - claw.stop(); + intake.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java deleted file mode 100644 index 29ef21e..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class Arm extends SubsystemBase { - /** Creates a new Arm. */ - CANSparkMax arm; - public Arm() { - arm = new CANSparkMax(Constants.ARM_MOTOR, MotorType.kBrushless); - arm.enableVoltageCompensation(12); - arm.setIdleMode(IdleMode.kBrake); - } - public void moveArm(double power){ - arm.set(power); - } - public void stopArm(){ - arm.set(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/BoxGrabber.java b/src/main/java/frc/robot/subsystems/BoxGrabber.java deleted file mode 100644 index 4938c26..0000000 --- a/src/main/java/frc/robot/subsystems/BoxGrabber.java +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class BoxGrabber extends SubsystemBase { - /** Creates a new BoxClaw. */ - CANSparkMax boxGrabber; - public BoxGrabber(){; - boxGrabber = new CANSparkMax(Constants.BOX_GRABBER, MotorType.kBrushless); - } - public void setBoxGrabber(double power){ - boxGrabber.set(power); - } - public void boxGrabberStop(){ - boxGrabber.set(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java deleted file mode 100644 index 98c80ae..0000000 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - - - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class Claw extends SubsystemBase { - /** Creates a new Claw. */ - CANSparkMax clawMotor; - public Claw() { - - clawMotor = new CANSparkMax(Constants.CLAW_MOTOR, MotorType.kBrushless); - clawMotor.enableVoltageCompensation(12); - clawMotor.setIdleMode(IdleMode.kBrake); - } - - public void rotateClawUp(double power){ - clawMotor.set(power); - } - public void stop(){ - clawMotor.set(0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6ba6ad2..248d3ed 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -3,22 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - - - -import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import frc.robot.Constants; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drivetrain extends SubsystemBase { @@ -32,16 +23,10 @@ public class Drivetrain extends SubsystemBase { RelativeEncoder rightBackEncoder; RelativeEncoder leftFrontEncoder; RelativeEncoder leftBackEncoder; - - AHRS ahrs; - - + MotorControllerGroup rightMotors; MotorControllerGroup leftMotors; DifferentialDrive dDrive; - - DifferentialDriveOdometry odometry; - Pose2d pose; public Drivetrain() { rightFrontSpark = new CANSparkMax(Constants.RIGHT_FRONT_SPARK, MotorType.kBrushless); @@ -59,183 +44,45 @@ public Drivetrain() { leftFrontSpark.restoreFactoryDefaults(); leftBackSpark.restoreFactoryDefaults(); - rightFrontEncoder = rightFrontSpark.getEncoder(); rightBackEncoder = rightBackSpark.getEncoder(); leftFrontEncoder = leftFrontSpark.getEncoder(); leftBackEncoder = leftBackSpark.getEncoder(); - rightFrontSpark.enableVoltageCompensation(12); rightBackSpark.enableVoltageCompensation(12); leftFrontSpark.enableVoltageCompensation(12); leftBackSpark.enableVoltageCompensation(12); - + rightMotors = new MotorControllerGroup(rightFrontSpark, rightBackSpark); leftMotors = new MotorControllerGroup(leftFrontSpark, leftBackSpark); dDrive = new DifferentialDrive(leftMotors, rightMotors); - //odometry = new DifferentialDriveOdometry(ahrs.getRotation2d(), rightFrontEncoder.getPosition(), leftFrontEncoder.getPosition()); - - ahrs = new AHRS(SPI.Port.kMXP); - - + arcadeDrive(0, 0); + arcadeDriveCustomized(0,0); } + public void arcadeDrive(double speed, double rotation){ - dDrive.arcadeDrive(speed, rotation); } - public void curvatureDrive(double speed, double rotation){ - dDrive.curvatureDrive(speed,rotation, false); - } - public void setMotorSpeed(){ - rightFrontSpark.set(-0.5); - rightBackSpark.set(-0.5); - leftFrontSpark.set(0.5); - leftBackSpark.set(0.5); - } - public void tankDrive(double right, double left){ - rightFrontSpark.set(right); - rightBackSpark.set(right); - leftFrontSpark.set(-left); - leftBackSpark.set(-left); - } + public void arcadeDriveCustomized(double speed, double rotation){ rightFrontSpark.set(rotation-speed); rightBackSpark.set(rotation-speed); leftFrontSpark.set(speed+rotation); leftBackSpark.set(speed+rotation); } - public void zeroEncoders(){ - rightFrontEncoder.setPosition(0); - rightBackEncoder.setPosition(0); - leftFrontEncoder.setPosition(0); - leftBackEncoder.setPosition(0); - } - public void resetPosition(){ - odometry.resetPosition(ahrs.getRotation2d(), rightFrontEncoder.getPosition(), leftFrontEncoder.getPosition(), pose); - } - public void updateOdometry(){ - odometry.update(ahrs.getRotation2d(), rightFrontEncoder.getPosition(), leftFrontEncoder.getPosition()); - } + public void stopMotors(){ rightFrontSpark.set(0); rightBackSpark.set(0); leftFrontSpark.set(0); leftBackSpark.set(0); } - public void resetGyros(){ - ahrs.reset(); - } - public double getAngle(){ - return ahrs.getAngle(); - } - public double getLeftPosition(){ - return leftFrontEncoder.getPosition(); - } - - public void putNumbers(){ - SmartDashboard.putNumber("Right Front Position", rightFrontEncoder.getPosition()); - SmartDashboard.putNumber("Right Back Position", rightBackEncoder.getPosition()); - SmartDashboard.putNumber("Left Front Position", leftFrontEncoder.getPosition()); - SmartDashboard.putNumber("Left Back Position", leftBackEncoder.getPosition()); - SmartDashboard.putNumber("Right Front Velocity", rightFrontEncoder.getVelocity()); - SmartDashboard.putNumber("Right Back Velocity", rightBackEncoder.getVelocity()); - SmartDashboard.putNumber("Left Front Velocity", leftFrontEncoder.getVelocity()); - SmartDashboard.putNumber("Left Back Velocity", leftBackEncoder.getVelocity()); - - //---------------------------------Odometry-------------------------------------- - - - // --------------------------------NAV X Below----------------------------------- - - SmartDashboard.putBoolean( "IMU_Connected", ahrs.isConnected()); - SmartDashboard.putBoolean( "IMU_IsCalibrating", ahrs.isCalibrating()); - SmartDashboard.putNumber( "IMU_Yaw", ahrs.getYaw()); - SmartDashboard.putNumber( "IMU_Pitch", ahrs.getPitch()); - SmartDashboard.putNumber( "IMU_Roll", ahrs.getRoll()); - - /* Display tilt-corrected, Magnetometer-based heading (requires */ - /* magnetometer calibration to be useful) */ - - SmartDashboard.putNumber( "IMU_CompassHeading", ahrs.getCompassHeading()); - - /* Display 9-axis Heading (requires magnetometer calibration to be useful) */ - SmartDashboard.putNumber( "IMU_FusedHeading", ahrs.getFusedHeading()); - - /* These functions are compatible w/the WPI Gyro Class, providing a simple */ - /* path for upgrading from the Kit-of-Parts gyro to the navx MXP */ - - SmartDashboard.putNumber( "IMU_TotalYaw", ahrs.getAngle()); - SmartDashboard.putNumber( "IMU_YawRateDPS", ahrs.getRate()); - - /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */ - SmartDashboard.putNumber( "IMU_Accel_X", ahrs.getWorldLinearAccelX()); - SmartDashboard.putNumber( "IMU_Accel_Y", ahrs.getWorldLinearAccelY()); - SmartDashboard.putBoolean( "IMU_IsMoving", ahrs.isMoving()); - SmartDashboard.putBoolean( "IMU_IsRotating", ahrs.isRotating()); - - /* Display estimates of velocity/displacement. Note that these values are */ - /* not expected to be accurate enough for estimating robot position on a */ - /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */ - /* of these errors due to single (velocity) integration and especially */ - /* double (displacement) integration. */ - - SmartDashboard.putNumber( "Velocity_X", ahrs.getVelocityX()); - SmartDashboard.putNumber( "Velocity_Y", ahrs.getVelocityY()); - SmartDashboard.putNumber( "Displacement_X", ahrs.getDisplacementX()); - SmartDashboard.putNumber( "Displacement_Y", ahrs.getDisplacementY()); - - /* Display Raw Gyro/Accelerometer/Magnetometer Values */ - /* NOTE: These values are not normally necessary, but are made available */ - /* for advanced users. Before using this data, please consider whether */ - /* the processed data (see above) will suit your needs. */ - - SmartDashboard.putNumber( "RawGyro_X", ahrs.getRawGyroX()); - SmartDashboard.putNumber( "RawGyro_Y", ahrs.getRawGyroY()); - SmartDashboard.putNumber( "RawGyro_Z", ahrs.getRawGyroZ()); - SmartDashboard.putNumber( "RawAccel_X", ahrs.getRawAccelX()); - SmartDashboard.putNumber( "RawAccel_Y", ahrs.getRawAccelY()); - SmartDashboard.putNumber( "RawAccel_Z", ahrs.getRawAccelZ()); - SmartDashboard.putNumber( "RawMag_X", ahrs.getRawMagX()); - SmartDashboard.putNumber( "RawMag_Y", ahrs.getRawMagY()); - SmartDashboard.putNumber( "RawMag_Z", ahrs.getRawMagZ()); - SmartDashboard.putNumber( "IMU_Temp_C", ahrs.getTempC()); - - /* Omnimount Yaw Axis Information */ - /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount */ - AHRS.BoardYawAxis yaw_axis = ahrs.getBoardYawAxis(); - SmartDashboard.putString( "YawAxisDirection", yaw_axis.up ? "Up" : "Down" ); - SmartDashboard.putNumber( "YawAxis", yaw_axis.board_axis.getValue() ); - - /* Sensor Board Information */ - SmartDashboard.putString( "FirmwareVersion", ahrs.getFirmwareVersion()); - - /* Quaternion Data */ - /* Quaternions are fascinating, and are the most compact representation of */ - /* orientation data. All of the Yaw, Pitch and Roll Values can be derived */ - /* from the Quaternions. If interested in motion processing, knowledge of */ - /* Quaternions is highly recommended. */ - SmartDashboard.putNumber( "QuaternionW", ahrs.getQuaternionW()); - SmartDashboard.putNumber( "QuaternionX", ahrs.getQuaternionX()); - SmartDashboard.putNumber( "QuaternionY", ahrs.getQuaternionY()); - SmartDashboard.putNumber( "QuaternionZ", ahrs.getQuaternionZ()); - - /* Sensor Data Timestamp */ - SmartDashboard.putNumber( "SensorTimestamp", ahrs.getLastSensorTimestamp()); - - /* Connectivity Debugging Support */ - SmartDashboard.putNumber( "IMU_Byte_Count", ahrs.getByteCount()); - SmartDashboard.putNumber( "IMU_Update_Count", ahrs.getUpdateCount()); - - - -} @Override - public void periodic() { - // This method will be called once per scheduler run + public void periodic() { } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1fceb2e..c8b2d5b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,52 +3,41 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; - - - - import com.revrobotics.CANSparkMax; - import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class Intake extends SubsystemBase { - /** Creates a new BoxGrabber. */ public CANSparkMax rightIntake; public CANSparkMax leftIntake; public Intake() { - - rightIntake = new CANSparkMax(Constants.RIGHT_INTAKE, MotorType.kBrushless); leftIntake = new CANSparkMax(Constants.LEFT_INTAKE, MotorType.kBrushless); - - leftIntake.follow(rightIntake); + rightIntake.enableVoltageCompensation(12); leftIntake.enableVoltageCompensation(12); - - + intakeRun(0); } - public void intakeRun(double power){ rightIntake.set(power); leftIntake.set(power); } + + public void runRightIntake(double power){ + rightIntake.set(power); + } + + public void runLeftIntake(double power){ + leftIntake.set(power); + } + public void stop(){ rightIntake.set(0); leftIntake.set(0); } -} - - - -// @Override -// public void periodic() { -// // This method will be called once per scheduler run -// } -// } \ No newline at end of file +} \ No newline at end of file