From 486393332748c189fe3e83995fb600d57c422a3a Mon Sep 17 00:00:00 2001 From: aditya917 Date: Sat, 25 Mar 2023 13:29:51 -0700 Subject: [PATCH 01/16] competition changes 2 --- src/main/java/frc/robot/Robot.java | 7 +++++++ src/main/java/frc/robot/RobotContainer.java | 8 ++++---- src/main/java/frc/robot/commands/MoveClaw.java | 1 + src/main/java/frc/robot/subsystems/Arm.java | 1 + src/main/java/frc/robot/subsystems/Claw.java | 13 ++++++++++++- src/main/java/frc/robot/subsystems/Drivetrain.java | 1 + 6 files changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 17a5f18..a4917af 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -19,6 +21,8 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; private Command autoCommand; + private UsbCamera usbCamera; + private UsbCamera usbCamera2; /** * This function is run when the robot is first started up and should be used for any @@ -29,6 +33,9 @@ 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(); + CameraServer.startAutomaticCapture(); + CameraServer.startAutomaticCapture(); + } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 877db0e..a3a692a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { 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); + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, gBox, .5); AutonomousCommandTwo autoCommandTwo= new AutonomousCommandTwo(driveTrain); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -66,7 +66,7 @@ public RobotContainer() { //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.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.08, xboxController)); driveTrain.putNumbers(); @@ -91,8 +91,8 @@ private void configureBindings() { //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.kA.value).whileTrue(new MoveClaw(claw, 0.2)); + new JoystickButton(xboxController2, Button.kB.value).whileTrue(new MoveClaw(claw, -0.2)); 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)); diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/MoveClaw.java index d3f7028..8f9da47 100644 --- a/src/main/java/frc/robot/commands/MoveClaw.java +++ b/src/main/java/frc/robot/commands/MoveClaw.java @@ -21,6 +21,7 @@ public MoveClaw(Claw claw, double power) { // Called when the command is initially scheduled. @Override public void initialize() { + claw.zeroEncoder(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 29ef21e..a567985 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -19,6 +19,7 @@ public Arm() { arm.enableVoltageCompensation(12); arm.setIdleMode(IdleMode.kBrake); } + public void moveArm(double power){ arm.set(power); } diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 98c80ae..3d5460f 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -7,6 +7,7 @@ import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -16,19 +17,29 @@ public class Claw extends SubsystemBase { /** Creates a new Claw. */ CANSparkMax clawMotor; + private RelativeEncoder clawEncoder; public Claw() { - clawMotor = new CANSparkMax(Constants.CLAW_MOTOR, MotorType.kBrushless); + clawEncoder = clawMotor.getEncoder(); clawMotor.enableVoltageCompensation(12); clawMotor.setIdleMode(IdleMode.kBrake); } + public double getEncoderValue(){ + return (clawEncoder.getPosition()*(18/42)*(1/100)); + } + public void zeroEncoder(){ + clawEncoder.setPosition(0); + } public void rotateClawUp(double power){ clawMotor.set(power); } public void stop(){ clawMotor.set(0); } + public void putNumbers(){ + System.out.println("Encoder value: " + this.getEncoderValue()); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6ba6ad2..2e21392 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -135,6 +135,7 @@ public double getAngle(){ public double getLeftPosition(){ return leftFrontEncoder.getPosition(); } + public void putNumbers(){ SmartDashboard.putNumber("Right Front Position", rightFrontEncoder.getPosition()); From 246b3143e5cc250802d27be6d2cb1b73b94a0bb6 Mon Sep 17 00:00:00 2001 From: aditya917 Date: Sat, 25 Mar 2023 13:36:10 -0700 Subject: [PATCH 02/16] stash --- src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/commands/DriveTank.java | 49 ------------------- .../{DriveFast.java => IntakeOverride.java} | 23 +++------ 3 files changed, 8 insertions(+), 65 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/DriveTank.java rename src/main/java/frc/robot/commands/{DriveFast.java => IntakeOverride.java} (61%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a3a692a..853b3db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import frc.robot.commands.AutonomousCommandTwo; import frc.robot.commands.ClawGrab; 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; 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/DriveFast.java b/src/main/java/frc/robot/commands/IntakeOverride.java similarity index 61% rename from src/main/java/frc/robot/commands/DriveFast.java rename to src/main/java/frc/robot/commands/IntakeOverride.java index 8e4ea6e..5f8c616 100644 --- a/src/main/java/frc/robot/commands/DriveFast.java +++ b/src/main/java/frc/robot/commands/IntakeOverride.java @@ -5,17 +5,14 @@ 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 IntakeOverride extends CommandBase { + /** Creates a new IntakeOverride. */ + Intake intake; + double power; + public IntakeOverride() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveTrain); - this.driveTrain = driveTrain; - this.speed=speed; } // Called when the command is initially scheduled. @@ -24,15 +21,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - driveTrain.arcadeDrive(0, speed); - } + public void execute() {} // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) { - driveTrain.stopMotors(); - } + public void end(boolean interrupted) {} // Returns true when the command should end. @Override From 4bba6709d4cbd8663637e9059b042d1bd93e6a12 Mon Sep 17 00:00:00 2001 From: aditya917 Date: Sat, 25 Mar 2023 17:10:42 -0700 Subject: [PATCH 03/16] code cleanup --- src/main/java/frc/robot/RobotContainer.java | 47 +---- .../frc/robot/commands/AutonomousCommand.java | 12 +- .../commands/AutonomousCommandThree.java | 24 --- .../robot/commands/AutonomousCommandTwo.java | 23 --- .../java/frc/robot/commands/ClawGrab.java | 42 ----- .../java/frc/robot/commands/DriveArcade.java | 51 ------ .../robot/commands/DriveArcadeCustomized.java | 32 ++-- .../frc/robot/commands/DriveCurvature.java | 45 ----- .../frc/robot/commands/DriveStraight.java | 54 ------ .../frc/robot/commands/DriveToDistance.java | 69 ------- .../frc/robot/commands/IntakeOverride.java | 35 ---- src/main/java/frc/robot/commands/MoveArm.java | 42 ----- .../java/frc/robot/commands/MoveClaw.java | 44 ----- src/main/java/frc/robot/subsystems/Arm.java | 34 ---- .../java/frc/robot/subsystems/BoxGrabber.java | 30 --- src/main/java/frc/robot/subsystems/Claw.java | 48 ----- .../java/frc/robot/subsystems/Drivetrain.java | 171 +----------------- .../java/frc/robot/subsystems/Intake.java | 25 +-- 18 files changed, 29 insertions(+), 799 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AutonomousCommandThree.java delete mode 100644 src/main/java/frc/robot/commands/AutonomousCommandTwo.java delete mode 100644 src/main/java/frc/robot/commands/ClawGrab.java delete mode 100644 src/main/java/frc/robot/commands/DriveArcade.java delete mode 100644 src/main/java/frc/robot/commands/DriveCurvature.java delete mode 100644 src/main/java/frc/robot/commands/DriveStraight.java delete mode 100644 src/main/java/frc/robot/commands/DriveToDistance.java delete mode 100644 src/main/java/frc/robot/commands/IntakeOverride.java delete mode 100644 src/main/java/frc/robot/commands/MoveArm.java delete mode 100644 src/main/java/frc/robot/commands/MoveClaw.java delete mode 100644 src/main/java/frc/robot/subsystems/Arm.java delete mode 100644 src/main/java/frc/robot/subsystems/BoxGrabber.java delete mode 100644 src/main/java/frc/robot/subsystems/Claw.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 853b3db..96352be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,11 +5,9 @@ package frc.robot; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DigitalInput; 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,20 +15,9 @@ 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.DriveArcadeCustomized; import frc.robot.commands.GrabberOne; -import frc.robot.commands.MoveArm; -import frc.robot.commands.MoveClaw; 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; /** @@ -42,36 +29,20 @@ 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, .5); - 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, .5); + 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.08, xboxController)); - - driveTrain.putNumbers(); - chooser.setDefaultOption("Default Auto Command", autoCommand); - chooser.addOption("Second Auto Command", autoCommandTwo); - SmartDashboard.putData(chooser); } @@ -85,20 +56,8 @@ public RobotContainer() { * 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.2)); - new JoystickButton(xboxController2, Button.kB.value).whileTrue(new MoveClaw(claw, -0.2)); - 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. } diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index 951f9b4..fe1d212 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -5,25 +5,19 @@ package frc.robot.commands; 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) { + public AutonomousCommand(Drivetrain driveTrain, double power, double rotation, Intake boxGrabber, double intakePower) { // 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 RunIntake(boxGrabber, intakePower).withTimeout(1), new Drive(driveTrain, power, 0).withTimeout(0), new Drive(driveTrain, 0, rotation).withTimeout(3)); - - - } } diff --git a/src/main/java/frc/robot/commands/AutonomousCommandThree.java b/src/main/java/frc/robot/commands/AutonomousCommandThree.java deleted file mode 100644 index 247ec83..0000000 --- a/src/main/java/frc/robot/commands/AutonomousCommandThree.java +++ /dev/null @@ -1,24 +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.SequentialCommandGroup; -import frc.robot.subsystems.Drivetrain; - -// 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) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new DriveToDistance(driveTrain, distance).withTimeout(5) - ); - } -} diff --git a/src/main/java/frc/robot/commands/AutonomousCommandTwo.java b/src/main/java/frc/robot/commands/AutonomousCommandTwo.java deleted file mode 100644 index 0df85cb..0000000 --- a/src/main/java/frc/robot/commands/AutonomousCommandTwo.java +++ /dev/null @@ -1,23 +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.SequentialCommandGroup; -import frc.robot.subsystems.Drivetrain; - -// 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) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new DriveStraight(driveTrain).withTimeout(20) - ); - } -} diff --git a/src/main/java/frc/robot/commands/ClawGrab.java b/src/main/java/frc/robot/commands/ClawGrab.java deleted file mode 100644 index 88c84c4..0000000 --- a/src/main/java/frc/robot/commands/ClawGrab.java +++ /dev/null @@ -1,42 +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.BoxGrabber; - -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; - // Use addRequirements() here to declare subsystem dependencies. - } - - // 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() { - boxGrabber.setBoxGrabber(power); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - boxGrabber.boxGrabberStop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} 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..e411119 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -5,8 +5,6 @@ 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; @@ -16,8 +14,7 @@ public class DriveArcadeCustomized extends CommandBase { 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) { @@ -27,37 +24,34 @@ public DriveArcadeCustomized(Drivetrain driveTrain, DoubleSupplier speed, Double this.rotation = rotation; this.limit = limit; 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() { - if(xboxController.getRightBumper()){ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*limit, rotation.getAsDouble()*0.3); - } - else if(xboxController.getLeftBumper()){ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*limit, rotation.getAsDouble()*0.3); - } - else{ - driveTrain.arcadeDriveCustomized(-speed.getAsDouble()*0.6, rotation.getAsDouble()*0.3); + double speeds = speed.getAsDouble()*(limit); + double rotations = rotation.getAsDouble() *0.3; + + if(xboxController.getRightBumper() || xboxController.getLeftBumper() ){ + driveTrain.arcadeDriveCustomized(-speeds*limit, rotations); + } else{ + driveTrain.arcadeDriveCustomized(-speeds*0.6, rotations); } - driveTrain.putNumbers(); + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + 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/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/IntakeOverride.java b/src/main/java/frc/robot/commands/IntakeOverride.java deleted file mode 100644 index 5f8c616..0000000 --- a/src/main/java/frc/robot/commands/IntakeOverride.java +++ /dev/null @@ -1,35 +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.Intake; - -public class IntakeOverride extends CommandBase { - /** Creates a new IntakeOverride. */ - Intake intake; - double power; - public IntakeOverride() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // 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() {} - - // 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/MoveArm.java b/src/main/java/frc/robot/commands/MoveArm.java deleted file mode 100644 index 5169709..0000000 --- a/src/main/java/frc/robot/commands/MoveArm.java +++ /dev/null @@ -1,42 +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.Arm; - -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; - // Use addRequirements() here to declare subsystem dependencies. - } - - // 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() { - arm.moveArm(power); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - arm.stopArm(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/MoveClaw.java deleted file mode 100644 index 8f9da47..0000000 --- a/src/main/java/frc/robot/commands/MoveClaw.java +++ /dev/null @@ -1,44 +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.Claw; - -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; - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - claw.zeroEncoder(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - claw.rotateClawUp(power); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - claw.stop(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} 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 a567985..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,34 +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 3d5460f..0000000 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ /dev/null @@ -1,48 +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.RelativeEncoder; -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; - private RelativeEncoder clawEncoder; - public Claw() { - clawMotor = new CANSparkMax(Constants.CLAW_MOTOR, MotorType.kBrushless); - clawEncoder = clawMotor.getEncoder(); - clawMotor.enableVoltageCompensation(12); - clawMotor.setIdleMode(IdleMode.kBrake); - } - - public double getEncoderValue(){ - return (clawEncoder.getPosition()*(18/42)*(1/100)); - } - public void zeroEncoder(){ - clawEncoder.setPosition(0); - } - public void rotateClawUp(double power){ - clawMotor.set(power); - } - public void stop(){ - clawMotor.set(0); - } - public void putNumbers(){ - System.out.println("Encoder value: " + this.getEncoderValue()); - } - - @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 2e21392..dfca346 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -3,22 +3,12 @@ // 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 +22,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,184 +43,43 @@ 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..fac4b7d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,52 +3,33 @@ // 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 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 From 566d06be651a2169c20bc3812925ba2a204fec32 Mon Sep 17 00:00:00 2001 From: aditya917 Date: Sat, 25 Mar 2023 17:30:52 -0700 Subject: [PATCH 04/16] code cleanup 2 --- src/main/java/frc/robot/Constants.java | 3 --- src/main/java/frc/robot/Robot.java | 19 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 11 +++++------ src/main/java/frc/robot/commands/Drive.java | 14 +++++++------- .../robot/commands/DriveArcadeCustomized.java | 14 +++++++------- .../java/frc/robot/commands/GrabberOne.java | 12 +++++++----- .../java/frc/robot/commands/RunIntake.java | 16 ++++++++-------- 7 files changed, 43 insertions(+), 46 deletions(-) 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 a4917af..53dbbe2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,7 +5,6 @@ package frc.robot; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -17,25 +16,24 @@ * project. */ public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - private Command autoCommand; - private UsbCamera usbCamera; - private UsbCamera usbCamera2; - + + 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(); CameraServer.startAutomaticCapture(); CameraServer.startAutomaticCapture(); - } /** @@ -55,6 +53,7 @@ public void robotPeriodic() { } /** This function is called once each time the robot enters Disabled mode. */ + @Override public void disabledInit() {} @@ -62,9 +61,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 96352be..fa19fd2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,8 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; - - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; @@ -26,18 +24,19 @@ * 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(); Drivetrain driveTrain = new Drivetrain(); 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<>(); - AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, boxGrabber, .5); + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, boxGrabber, .5); public RobotContainer() { configureBindings(); @@ -55,11 +54,11 @@ public RobotContainer() { * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight * joysticks}. */ + private void configureBindings() { 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)); - - } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 4289fff..66d3361 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,13 @@ 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/DriveArcadeCustomized.java b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java index e411119..1d6f051 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -11,12 +11,12 @@ public class DriveArcadeCustomized extends CommandBase { /** Creates a new DriveArcadeCustomized. */ - Drivetrain driveTrain; - DoubleSupplier speed; - DoubleSupplier rotation; - - XboxController xboxController; - double limit; + Drivetrain driveTrain = null; + DoubleSupplier speed = null; + DoubleSupplier rotation = null; + XboxController xboxController = null; + double limit = 0.0; + public DriveArcadeCustomized(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation, double limit, XboxController xboxController) { // Use addRequirements() here to declare subsystem dependencies. this.driveTrain = driveTrain; @@ -44,12 +44,12 @@ public void execute() { } else{ driveTrain.arcadeDriveCustomized(-speeds*0.6, rotations); } - } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + driveTrain.arcadeDriveCustomized(0, 0); System.out.println("arcade drive ended"); } diff --git a/src/main/java/frc/robot/commands/GrabberOne.java b/src/main/java/frc/robot/commands/GrabberOne.java index d68069c..ff489d2 100644 --- a/src/main/java/frc/robot/commands/GrabberOne.java +++ b/src/main/java/frc/robot/commands/GrabberOne.java @@ -11,23 +11,24 @@ public class GrabberOne 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 GrabberOne(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(); } // Called every time the scheduler runs while the command is scheduled. @@ -48,6 +49,7 @@ else if(xboxController.getRightBumper()){ @Override public void end(boolean interrupted) { boxGrabber.stop(); + System.out.println("intake ended"); } // 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..e09b50a 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -11,22 +11,21 @@ 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. @@ -39,6 +38,7 @@ public void execute() { @Override public void end(boolean interrupted) { boxGrabber.stop(); + System.out.println("run intake method stopped"); } // Returns true when the command should end. From 10fa9be3e6483a0e1d62d2c133388e3a61641092 Mon Sep 17 00:00:00 2001 From: aditya917 Date: Sat, 25 Mar 2023 21:31:32 -0700 Subject: [PATCH 05/16] competition --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa19fd2..fb48f71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { public RobotContainer() { configureBindings(); - driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.08, xboxController)); + driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.8, xboxController)); chooser.setDefaultOption("Default Auto Command", autoCommand); SmartDashboard.putData(chooser); } From 5913cb0b0760ee22d5143d71e2ee8a6cf43d0724 Mon Sep 17 00:00:00 2001 From: "LAPTOP-MAXIM\\maxim" Date: Sun, 26 Mar 2023 09:34:04 -0700 Subject: [PATCH 06/16] added creep and fast limits --- src/main/java/frc/robot/RobotContainer.java | 5 ++-- .../frc/robot/commands/AutonomousCommand.java | 6 ++--- .../robot/commands/DriveArcadeCustomized.java | 23 +++++++++++++------ 3 files changed, 22 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb48f71..9aa2d9c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,11 +36,11 @@ public class RobotContainer { XboxController xboxController2 = new XboxController(Constants.XBOX_CONTROLLER_PORT_2); SendableChooser chooser = new SendableChooser<>(); - AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, boxGrabber, .5); + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, boxGrabber, .75); public RobotContainer() { configureBindings(); - driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.8, xboxController)); + driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.08, 0.1, 0.8, xboxController)); chooser.setDefaultOption("Default Auto Command", autoCommand); SmartDashboard.putData(chooser); } @@ -58,6 +58,7 @@ public RobotContainer() { private void configureBindings() { 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)); + } /** diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index fe1d212..d7d8474 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -16,8 +16,8 @@ public class AutonomousCommand extends SequentialCommandGroup { public AutonomousCommand(Drivetrain driveTrain, double power, double rotation, Intake boxGrabber, double intakePower) { // Add your commands in the addCommands() call, e.g. addCommands( - new RunIntake(boxGrabber, intakePower).withTimeout(1), - new Drive(driveTrain, power, 0).withTimeout(0), - new Drive(driveTrain, 0, rotation).withTimeout(3)); + new Drive(driveTrain, power, 0).withTimeout(0.75), + new RunIntake(boxGrabber, intakePower).withTimeout(2), + new Drive(driveTrain, 0,-rotation).withTimeout(3)); } } diff --git a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java index 1d6f051..f7cf8ba 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -15,14 +15,19 @@ public class DriveArcadeCustomized extends CommandBase { DoubleSupplier speed = null; DoubleSupplier rotation = null; XboxController xboxController = null; - double limit = 0.0; + double creepRotationLimit = 0.0; + double creepLimit = 0.0; + double fastLimit =0.0; + - public DriveArcadeCustomized(Drivetrain driveTrain, DoubleSupplier speed, DoubleSupplier rotation, double limit, XboxController xboxController) { + 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; addRequirements(driveTrain); } @@ -36,12 +41,16 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double speeds = speed.getAsDouble()*(limit); + double speeds = speed.getAsDouble(); double rotations = rotation.getAsDouble() *0.3; - if(xboxController.getRightBumper() || xboxController.getLeftBumper() ){ - driveTrain.arcadeDriveCustomized(-speeds*limit, rotations); - } else{ + if(xboxController.getLeftBumper() ){ + driveTrain.arcadeDriveCustomized(-speeds*creepLimit, rotations*creepRotationLimit); + } + else if(xboxController.getRightBumper()){ + driveTrain.arcadeDriveCustomized(-speeds*fastLimit, rotations); + } + else{ driveTrain.arcadeDriveCustomized(-speeds*0.6, rotations); } } From 1ff4543e991bcccdddec7a1fc6de85d72adadce7 Mon Sep 17 00:00:00 2001 From: "LAPTOP-MAXIM\\maxim" Date: Sun, 26 Mar 2023 10:00:38 -0700 Subject: [PATCH 07/16] changed creep multipliers --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9aa2d9c..c7af996 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { public RobotContainer() { configureBindings(); - driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.08, 0.1, 0.8, xboxController)); + driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.3, 0.15, 0.8, xboxController)); chooser.setDefaultOption("Default Auto Command", autoCommand); SmartDashboard.putData(chooser); } From 728b2e97ece965acee1beb47b791a368b704bbd4 Mon Sep 17 00:00:00 2001 From: "LAPTOP-MAXIM\\maxim" Date: Sun, 26 Mar 2023 10:08:11 -0700 Subject: [PATCH 08/16] readable changes to arcade drive method --- .../robot/commands/DriveArcadeCustomized.java | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java index f7cf8ba..c9d2a41 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -42,17 +42,19 @@ public void initialize() { @Override public void execute() { double speeds = speed.getAsDouble(); - double rotations = rotation.getAsDouble() *0.3; - - if(xboxController.getLeftBumper() ){ - driveTrain.arcadeDriveCustomized(-speeds*creepLimit, rotations*creepRotationLimit); - } - else if(xboxController.getRightBumper()){ - driveTrain.arcadeDriveCustomized(-speeds*fastLimit, rotations); + double rotations = rotation.getAsDouble(); + double currentLimit = 0.6; + double currentRotationLimit = 0.3; + if(xboxController.getRightBumper()){ + currentLimit = fastLimit; + currentRotationLimit = 0.3; } - else{ - driveTrain.arcadeDriveCustomized(-speeds*0.6, rotations); + else if(xboxController.getLeftBumper()) { + currentLimit = creepLimit; + currentRotationLimit = creepRotationLimit; } + + driveTrain.arcadeDriveCustomized(-speeds*currentLimit, rotations*currentRotationLimit); } // Called once the command ends or is interrupted. From 50e96e008216f9731d4eb1acb8b419117c9ce77b Mon Sep 17 00:00:00 2001 From: "LAPTOP-MAXIM\\maxim" Date: Thu, 6 Apr 2023 11:30:15 -0700 Subject: [PATCH 09/16] added speed interval to reduce joystick drift --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/Drive.java | 1 + .../frc/robot/commands/DriveArcadeCustomized.java | 11 +++++++++++ src/main/java/frc/robot/subsystems/Drivetrain.java | 3 +++ 5 files changed, 17 insertions(+), 2 deletions(-) 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7af996..aebf8be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,7 @@ public class RobotContainer { public RobotContainer() { configureBindings(); - driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.3, 0.15, 0.8, xboxController)); + driveTrain.setDefaultCommand(new DriveArcadeCustomized(driveTrain, xboxController::getLeftY, xboxController::getRightX, 0.3, 0.2, 0.8, xboxController)); chooser.setDefaultOption("Default Auto Command", autoCommand); SmartDashboard.putData(chooser); } diff --git a/src/main/java/frc/robot/commands/Drive.java b/src/main/java/frc/robot/commands/Drive.java index 66d3361..5e4d497 100644 --- a/src/main/java/frc/robot/commands/Drive.java +++ b/src/main/java/frc/robot/commands/Drive.java @@ -33,6 +33,7 @@ public void initialize() { @Override public void execute() { driveTrain.arcadeDrive(power, rotation); + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java index c9d2a41..c38a8a6 100644 --- a/src/main/java/frc/robot/commands/DriveArcadeCustomized.java +++ b/src/main/java/frc/robot/commands/DriveArcadeCustomized.java @@ -45,6 +45,8 @@ public void execute() { double rotations = rotation.getAsDouble(); double currentLimit = 0.6; double currentRotationLimit = 0.3; + double speedInterval = 0.08; + if(xboxController.getRightBumper()){ currentLimit = fastLimit; currentRotationLimit = 0.3; @@ -54,7 +56,16 @@ else if(xboxController.getLeftBumper()) { currentRotationLimit = creepRotationLimit; } + if((speeds >= -speedInterval) && (speeds <= speedInterval)) { + speeds= 0.0; + } + + if((rotations >= -speedInterval) && (rotations <= speedInterval)) { + rotations = 0.0; + } + driveTrain.arcadeDriveCustomized(-speeds*currentLimit, rotations*currentRotationLimit); + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index dfca346..248d3ed 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; 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.wpilibj.drive.DifferentialDrive; @@ -53,6 +54,8 @@ public Drivetrain() { leftFrontSpark.enableVoltageCompensation(12); leftBackSpark.enableVoltageCompensation(12); + + rightMotors = new MotorControllerGroup(rightFrontSpark, rightBackSpark); leftMotors = new MotorControllerGroup(leftFrontSpark, leftBackSpark); dDrive = new DifferentialDrive(leftMotors, rightMotors); From 0112a886dcda4513789273aa7f5d2e9836db93c2 Mon Sep 17 00:00:00 2001 From: "LAPTOP-MAXIM\\maxim" Date: Thu, 6 Apr 2023 23:36:31 -0700 Subject: [PATCH 10/16] stash --- src/main/java/frc/robot/Robot.java | 3 -- src/main/java/frc/robot/RobotContainer.java | 20 ++++++--- .../frc/robot/commands/AutonomousCommand.java | 4 +- .../java/frc/robot/commands/CubeAdjust.java | 22 ++++++++++ .../java/frc/robot/commands/CubeShoot.java | 22 ++++++++++ .../{GrabberOne.java => IntakeIn.java} | 28 +++++++----- .../java/frc/robot/commands/IntakeOut.java | 44 +++++++++++++++++++ .../java/frc/robot/commands/RunIntake.java | 2 +- .../frc/robot/commands/RunLeftIntake.java | 44 +++++++++++++++++++ .../frc/robot/commands/RunRightIntake.java | 44 +++++++++++++++++++ src/main/java/frc/robot/commands/Shoot.java | 44 +++++++++++++++++++ .../java/frc/robot/subsystems/Intake.java | 10 ++++- 12 files changed, 265 insertions(+), 22 deletions(-) create mode 100644 src/main/java/frc/robot/commands/CubeAdjust.java create mode 100644 src/main/java/frc/robot/commands/CubeShoot.java rename src/main/java/frc/robot/commands/{GrabberOne.java => IntakeIn.java} (71%) create mode 100644 src/main/java/frc/robot/commands/IntakeOut.java create mode 100644 src/main/java/frc/robot/commands/RunLeftIntake.java create mode 100644 src/main/java/frc/robot/commands/RunRightIntake.java create mode 100644 src/main/java/frc/robot/commands/Shoot.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 53dbbe2..ab1a66c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -32,8 +31,6 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - CameraServer.startAutomaticCapture(); - CameraServer.startAutomaticCapture(); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aebf8be..8db7dc1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,15 @@ 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.CubeAdjust; +import frc.robot.commands.CubeShoot; import frc.robot.commands.DriveArcadeCustomized; -import frc.robot.commands.GrabberOne; +import frc.robot.commands.IntakeIn; +import frc.robot.commands.IntakeOut; +import frc.robot.commands.RunIntake; +import frc.robot.commands.RunLeftIntake; +import frc.robot.commands.RunRightIntake; +import frc.robot.commands.Shoot; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Drivetrain; @@ -36,7 +43,7 @@ public class RobotContainer { XboxController xboxController2 = new XboxController(Constants.XBOX_CONTROLLER_PORT_2); SendableChooser chooser = new SendableChooser<>(); - AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, -0.5, boxGrabber, .75); + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, 0.5, boxGrabber, -1); public RobotContainer() { configureBindings(); @@ -56,9 +63,12 @@ public RobotContainer() { */ private void configureBindings() { - 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.kRightBumper.value).whileTrue(new IntakeOut(boxGrabber, 0.35)); + new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.35, digitalInput, xboxController2)); + new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeShoot(boxGrabber, -1)); + new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 0.15)); + + } /** diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index d7d8474..e5dc16e 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -17,7 +17,7 @@ public AutonomousCommand(Drivetrain driveTrain, double power, double rotation, I // Add your commands in the addCommands() call, e.g. addCommands( new Drive(driveTrain, power, 0).withTimeout(0.75), - new RunIntake(boxGrabber, intakePower).withTimeout(2), - new Drive(driveTrain, 0,-rotation).withTimeout(3)); + new RunIntake(boxGrabber, intakePower).withTimeout(1), + new Drive(driveTrain, 0,-rotation).withTimeout(1.25)); } } diff --git a/src/main/java/frc/robot/commands/CubeAdjust.java b/src/main/java/frc/robot/commands/CubeAdjust.java new file mode 100644 index 0000000..508219f --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeAdjust.java @@ -0,0 +1,22 @@ +// 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.SequentialCommandGroup; +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 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 RunLeftIntake(intake, power).withTimeout(0.15) + ); + } +} diff --git a/src/main/java/frc/robot/commands/CubeShoot.java b/src/main/java/frc/robot/commands/CubeShoot.java new file mode 100644 index 0000000..4b47d66 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeShoot.java @@ -0,0 +1,22 @@ +// 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.SequentialCommandGroup; +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 CubeShoot extends SequentialCommandGroup { + /** Creates a new CubeShoot. */ + public CubeShoot(Intake intake, double power) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new RunRightIntake(intake, power).withTimeout(0.75), + new Shoot(intake, power)); + } +} diff --git a/src/main/java/frc/robot/commands/GrabberOne.java b/src/main/java/frc/robot/commands/IntakeIn.java similarity index 71% rename from src/main/java/frc/robot/commands/GrabberOne.java rename to src/main/java/frc/robot/commands/IntakeIn.java index ff489d2..e46a4ad 100644 --- a/src/main/java/frc/robot/commands/GrabberOne.java +++ b/src/main/java/frc/robot/commands/IntakeIn.java @@ -4,19 +4,21 @@ 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 = null; DigitalInput digitalInput = null; double power = 0.0; XboxController xboxController = null; + boolean done = false; - public GrabberOne(Intake boxGrabber, double power, DigitalInput digitalInput, XboxController xboxController) { + public IntakeIn(Intake boxGrabber, double power, DigitalInput digitalInput, XboxController xboxController) { // Use addRequirements() here to declare subsystem dependencies. this.boxGrabber = boxGrabber; this.power=power; @@ -34,14 +36,19 @@ public void initialize() { // 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); - } - else{ - boxGrabber.stop(); + if(xboxController.getBButton()){ + boxGrabber.runLeftIntake(power); + boxGrabber.runRightIntake(0.5); + } else{ + if(!digitalInput.get()){ + done = true; + } + if(done) { + boxGrabber.stop(); + } else { + boxGrabber.runLeftIntake(power); + boxGrabber.runRightIntake(0.5); + } } } @@ -49,6 +56,7 @@ else if(xboxController.getRightBumper()){ @Override public void end(boolean interrupted) { boxGrabber.stop(); + done = false; System.out.println("intake ended"); } diff --git a/src/main/java/frc/robot/commands/IntakeOut.java b/src/main/java/frc/robot/commands/IntakeOut.java new file mode 100644 index 0000000..477f1c9 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeOut.java @@ -0,0 +1,44 @@ +// 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.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Intake; + +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. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intake.intakeRun(power); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index e09b50a..8dac695 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -31,7 +31,7 @@ public void initialize() { // 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. diff --git a/src/main/java/frc/robot/commands/RunLeftIntake.java b/src/main/java/frc/robot/commands/RunLeftIntake.java new file mode 100644 index 0000000..2d8a473 --- /dev/null +++ b/src/main/java/frc/robot/commands/RunLeftIntake.java @@ -0,0 +1,44 @@ +// 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.Intake; + +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() { + intake.stop(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intake.runLeftIntake(power); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RunRightIntake.java b/src/main/java/frc/robot/commands/RunRightIntake.java new file mode 100644 index 0000000..4a2846d --- /dev/null +++ b/src/main/java/frc/robot/commands/RunRightIntake.java @@ -0,0 +1,44 @@ +// 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.Intake; + +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(intake); + this.power = power; + this.intake = 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() { + intake.runRightIntake(power); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Shoot.java b/src/main/java/frc/robot/commands/Shoot.java new file mode 100644 index 0000000..9d60da8 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shoot.java @@ -0,0 +1,44 @@ +// 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.Intake; + +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() { + intake.intakeRun(power); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index fac4b7d..c8b2d5b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -15,7 +15,7 @@ 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); @@ -28,6 +28,14 @@ public void intakeRun(double 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); From d98008a0adea08deadc1edc73878c3986400a59a Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Fri, 4 Aug 2023 20:36:42 -0700 Subject: [PATCH 11/16] First commit --- src/main/java/frc/robot/RobotContainer.java | 8 +++++-- .../java/frc/robot/commands/CubeShoot.java | 11 +++++++--- .../java/frc/robot/commands/IntakeIn.java | 21 +++++++------------ 3 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8db7dc1..f72bd89 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AutonomousCommand; import frc.robot.commands.CubeAdjust; +import frc.robot.commands.CubeAdjustFull; import frc.robot.commands.CubeShoot; +import frc.robot.commands.CubeShootFull; import frc.robot.commands.DriveArcadeCustomized; import frc.robot.commands.IntakeIn; import frc.robot.commands.IntakeOut; @@ -36,6 +38,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... Intake boxGrabber = new Intake(); + Drivetrain driveTrain = new Drivetrain(); DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); @@ -65,9 +68,10 @@ public RobotContainer() { private void configureBindings() { new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new IntakeOut(boxGrabber, 0.35)); new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.35, digitalInput, xboxController2)); - new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeShoot(boxGrabber, -1)); + new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -1, -1)); + new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeAdjustFull(digitalInput, boxGrabber, xboxController2)); new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 0.15)); - + // new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -0.5, -1)); } diff --git a/src/main/java/frc/robot/commands/CubeShoot.java b/src/main/java/frc/robot/commands/CubeShoot.java index 4b47d66..dc8537b 100644 --- a/src/main/java/frc/robot/commands/CubeShoot.java +++ b/src/main/java/frc/robot/commands/CubeShoot.java @@ -12,11 +12,16 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class CubeShoot extends SequentialCommandGroup { /** Creates a new CubeShoot. */ - public CubeShoot(Intake intake, double power) { + public CubeShoot(Intake intake, double intakePower, double shooterPower) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new RunRightIntake(intake, power).withTimeout(0.75), - new Shoot(intake, power)); + + new RunRightIntake(intake, intakePower).withTimeout(1.5), + new Shoot(intake, shooterPower).withTimeout(1.5)); + } } + + + diff --git a/src/main/java/frc/robot/commands/IntakeIn.java b/src/main/java/frc/robot/commands/IntakeIn.java index e46a4ad..b4a1ef0 100644 --- a/src/main/java/frc/robot/commands/IntakeIn.java +++ b/src/main/java/frc/robot/commands/IntakeIn.java @@ -32,23 +32,19 @@ public IntakeIn(Intake boxGrabber, double power, DigitalInput digitalInput, Xbox 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(xboxController.getBButton()){ + if(!digitalInput.get()){ + done = true; + System.out.println(done); + } + if(done) { + boxGrabber.stop(); + } else if (xboxController.getLeftBumper()) { boxGrabber.runLeftIntake(power); boxGrabber.runRightIntake(0.5); - } else{ - if(!digitalInput.get()){ - done = true; - } - if(done) { - boxGrabber.stop(); - } else { - boxGrabber.runLeftIntake(power); - boxGrabber.runRightIntake(0.5); - } } } @@ -57,7 +53,6 @@ public void execute() { public void end(boolean interrupted) { boxGrabber.stop(); done = false; - System.out.println("intake ended"); } // Returns true when the command should end. From 80787ab3181266dcc3238d8d6cf5a9dd30a0de87 Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Sun, 27 Aug 2023 21:28:09 -0700 Subject: [PATCH 12/16] Updated Code --- .../frc/robot/commands/CubeAdjustFull.java | 57 +++++++++++++++++++ .../frc/robot/commands/CubeShootFull.java | 20 +++++++ 2 files changed, 77 insertions(+) create mode 100644 src/main/java/frc/robot/commands/CubeAdjustFull.java create mode 100644 src/main/java/frc/robot/commands/CubeShootFull.java diff --git a/src/main/java/frc/robot/commands/CubeAdjustFull.java b/src/main/java/frc/robot/commands/CubeAdjustFull.java new file mode 100644 index 0000000..075bbd6 --- /dev/null +++ b/src/main/java/frc/robot/commands/CubeAdjustFull.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 CubeAdjustFull extends CommandBase { + DigitalInput digitalInput = null; + boolean done = false; + Intake boxGrabber = null; + XboxController xboxController = null; + + + + + public CubeAdjustFull(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/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)); + + } +} From 06df000e31e95aa08b5f11edaaf5827c5af1ff8e Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Mon, 11 Sep 2023 22:13:23 -0700 Subject: [PATCH 13/16] updated code --- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/commands/CubeShoot.java | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f72bd89..e87bd24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.commands.CubeAdjustFull; import frc.robot.commands.CubeShoot; import frc.robot.commands.CubeShootFull; +import frc.robot.commands.CubeShootPlusAdjust; import frc.robot.commands.DriveArcadeCustomized; import frc.robot.commands.IntakeIn; import frc.robot.commands.IntakeOut; @@ -67,10 +68,11 @@ public RobotContainer() { private void configureBindings() { new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new IntakeOut(boxGrabber, 0.35)); - new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.35, digitalInput, xboxController2)); + new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.40, digitalInput, xboxController2)); new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -1, -1)); new JoystickButton(xboxController2, Button.kA.value).whileTrue(new CubeAdjustFull(digitalInput, boxGrabber, xboxController2)); new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 0.15)); + new JoystickButton(xboxController2, Button.kY.value).whileTrue(new CubeShootPlusAdjust(digitalInput, boxGrabber, xboxController2, boxGrabber, -1, -1)); // new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -0.5, -1)); } diff --git a/src/main/java/frc/robot/commands/CubeShoot.java b/src/main/java/frc/robot/commands/CubeShoot.java index dc8537b..29de3be 100644 --- a/src/main/java/frc/robot/commands/CubeShoot.java +++ b/src/main/java/frc/robot/commands/CubeShoot.java @@ -17,7 +17,7 @@ public CubeShoot(Intake intake, double intakePower, double shooterPower) { // addCommands(new FooCommand(), new BarCommand()); addCommands( - new RunRightIntake(intake, intakePower).withTimeout(1.5), + new RunRightIntake(intake, intakePower).withTimeout(0.5), new Shoot(intake, shooterPower).withTimeout(1.5)); } From 2a2fbfb05efe5702f1d794ef35736dfd414662db Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Tue, 19 Sep 2023 21:23:18 -0700 Subject: [PATCH 14/16] updated code with high scoring --- src/main/java/frc/robot/RobotContainer.java | 18 +++--- .../java/frc/robot/commands/CubeAdjust.java | 2 +- .../frc/robot/commands/CubeAdjustFull.java | 57 ------------------- 3 files changed, 11 insertions(+), 66 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/CubeAdjustFull.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e87bd24..a56f666 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,19 +14,20 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AutonomousCommand; import frc.robot.commands.CubeAdjust; -import frc.robot.commands.CubeAdjustFull; +import frc.robot.commands.CubeAdjustHigh; +import frc.robot.commands.CubeAdjustMid; import frc.robot.commands.CubeShoot; import frc.robot.commands.CubeShootFull; -import frc.robot.commands.CubeShootPlusAdjust; +import frc.robot.commands.CubeScoreMid; import frc.robot.commands.DriveArcadeCustomized; import frc.robot.commands.IntakeIn; import frc.robot.commands.IntakeOut; -import frc.robot.commands.RunIntake; import frc.robot.commands.RunLeftIntake; import frc.robot.commands.RunRightIntake; import frc.robot.commands.Shoot; import frc.robot.subsystems.Intake; 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 @@ -67,12 +68,13 @@ public RobotContainer() { */ private void configureBindings() { - new JoystickButton(xboxController2, Button.kRightBumper.value).whileTrue(new IntakeOut(boxGrabber, 0.35)); - new JoystickButton(xboxController2, Button.kLeftBumper.value).whileTrue(new IntakeIn(boxGrabber, -0.40, digitalInput, xboxController2)); + 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 CubeAdjustFull(digitalInput, boxGrabber, xboxController2)); - new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 0.15)); - new JoystickButton(xboxController2, Button.kY.value).whileTrue(new CubeShootPlusAdjust(digitalInput, boxGrabber, xboxController2, 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.kA.value).whileTrue(new CubeShootPlusAdjustHigh(digitalInput, boxGrabber, xboxController2, boxGrabber, -1, -1)); // new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -0.5, -1)); } diff --git a/src/main/java/frc/robot/commands/CubeAdjust.java b/src/main/java/frc/robot/commands/CubeAdjust.java index 508219f..9bc7cd6 100644 --- a/src/main/java/frc/robot/commands/CubeAdjust.java +++ b/src/main/java/frc/robot/commands/CubeAdjust.java @@ -16,7 +16,7 @@ public CubeAdjust(Intake intake, double power) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - new RunLeftIntake(intake, power).withTimeout(0.15) + new RunLeftIntake(intake, power).withTimeout(0.35) ); } } diff --git a/src/main/java/frc/robot/commands/CubeAdjustFull.java b/src/main/java/frc/robot/commands/CubeAdjustFull.java deleted file mode 100644 index 075bbd6..0000000 --- a/src/main/java/frc/robot/commands/CubeAdjustFull.java +++ /dev/null @@ -1,57 +0,0 @@ -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 CubeAdjustFull extends CommandBase { - DigitalInput digitalInput = null; - boolean done = false; - Intake boxGrabber = null; - XboxController xboxController = null; - - - - - public CubeAdjustFull(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; - } -} From fcc8631601953a6a7dd2900cc1123532a789edfd Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Tue, 19 Sep 2023 21:24:07 -0700 Subject: [PATCH 15/16] updated high scoring --- .../frc/robot/commands/CubeAdjustHigh.java | 59 +++++++++++++++++++ .../frc/robot/commands/CubeAdjustMid.java | 57 ++++++++++++++++++ .../frc/robot/commands/CubeScoreHigh.java | 25 ++++++++ .../java/frc/robot/commands/CubeScoreMid.java | 21 +++++++ .../commands/CubeShootPlusAdjustHigh.java | 24 ++++++++ 5 files changed, 186 insertions(+) create mode 100644 src/main/java/frc/robot/commands/CubeAdjustHigh.java create mode 100644 src/main/java/frc/robot/commands/CubeAdjustMid.java create mode 100644 src/main/java/frc/robot/commands/CubeScoreHigh.java create mode 100644 src/main/java/frc/robot/commands/CubeScoreMid.java create mode 100644 src/main/java/frc/robot/commands/CubeShootPlusAdjustHigh.java 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/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) + ); + + } + } From 89d2e390a6d61f5f90c3ac507e89b0b70c81fead Mon Sep 17 00:00:00 2001 From: Sharva Parmar Date: Thu, 5 Oct 2023 19:56:07 -0700 Subject: [PATCH 16/16] Final Code before Calgames --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/commands/AutonomousCommand.java | 9 +++++++-- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a56f666..52f3e30 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; 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.SendableChooser; @@ -48,7 +49,7 @@ public class RobotContainer { XboxController xboxController2 = new XboxController(Constants.XBOX_CONTROLLER_PORT_2); SendableChooser chooser = new SendableChooser<>(); - AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, 0.5, boxGrabber, -1); + AutonomousCommand autoCommand = new AutonomousCommand(driveTrain, 0, 0.5, boxGrabber, -1, digitalInput, xboxController2); public RobotContainer() { configureBindings(); @@ -74,8 +75,7 @@ private void configureBindings() { //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.kA.value).whileTrue(new CubeShootPlusAdjustHigh(digitalInput, boxGrabber, xboxController2, boxGrabber, -1, -1)); - // new JoystickButton(xboxController2, Button.kB.value).whileTrue(new CubeShoot(boxGrabber, -0.5, -1)); + new JoystickButton(xboxController2, Button.kX.value).whileTrue(new CubeAdjust(boxGrabber, 1)); } diff --git a/src/main/java/frc/robot/commands/AutonomousCommand.java b/src/main/java/frc/robot/commands/AutonomousCommand.java index e5dc16e..87ccad5 100644 --- a/src/main/java/frc/robot/commands/AutonomousCommand.java +++ b/src/main/java/frc/robot/commands/AutonomousCommand.java @@ -4,6 +4,8 @@ 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.Drivetrain; import frc.robot.subsystems.Intake; @@ -13,11 +15,14 @@ // 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, Intake 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 Drive(driveTrain, power, 0).withTimeout(0.75), - new RunIntake(boxGrabber, intakePower).withTimeout(1), + new CubeShoot(boxGrabber, intakePower, -1).withTimeout(1.55), new Drive(driveTrain, 0,-rotation).withTimeout(1.25)); } }