From 82c8e1dcb00a7dde02d682fe2340a8947135b0eb Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Tue, 27 Feb 2024 15:48:59 -0500 Subject: [PATCH] robot testing --- .../roborioteamnumbersetter.json | 1 + .../java/frc/robot/Commands/SpeakerShot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 66 +++++++++++++++---- .../robot/constants/DrivetrainConstants.java | 2 +- .../frc/robot/constants/ShooterConstants.java | 2 +- .../java/frc/robot/subsystems/Climber.java | 12 ++-- .../java/frc/robot/subsystems/Indexer.java | 4 +- .../java/frc/robot/subsystems/Intake.java | 6 +- .../java/frc/robot/subsystems/Shooter.java | 56 +++++++++------- 9 files changed, 101 insertions(+), 50 deletions(-) create mode 100644 .roboRIOTeamNumberSetter/roborioteamnumbersetter.json diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/Commands/SpeakerShot.java b/src/main/java/frc/robot/Commands/SpeakerShot.java index 222de7c..2b0a1e4 100644 --- a/src/main/java/frc/robot/Commands/SpeakerShot.java +++ b/src/main/java/frc/robot/Commands/SpeakerShot.java @@ -8,6 +8,6 @@ import frc.robot.subsystems.Shooter; public class SpeakerShot extends ParallelCommandGroup{ SpeakerShot(Indexer indexer, Shooter shooter){ - addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0)); + //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0)); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29ce5fc..aea12d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,6 +61,8 @@ public class RobotContainer { // TODO There's more than one speaker tag, how do we want to handle this? private int speakerTag; + private boolean setAmpAngle; + public RobotContainer() { /*try { @@ -83,9 +85,9 @@ public class RobotContainer { indexer = new Indexer(); - shooter = new Shooter(indexer.getBeamBreak()); + shooter = new Shooter(indexer::getBeamBreak); - climber = new Climber(shooter.getShooterAngle()); + climber = new Climber(shooter::getShooterAngle); @@ -113,6 +115,8 @@ public class RobotContainer { speakerTag = 8; } + setAmpAngle = false; + configureBindings(); shuffleboardSetup(); } @@ -130,20 +134,47 @@ public class RobotContainer { drivetrain.teleopCommand( driver::getLeftY, driver::getLeftX, - driver::getRightX, + () -> { return -driver.getRightX(); }, OIConstants.kTeleopDriveDeadband ) ); //intake.setDefaultCommand(intake.intakeUpCommand()); - intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, driver::getLeftTriggerAxis)); + intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0)); - // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0)); - shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis)); + shooter.setDefaultCommand( + shooter.angleSpeedsSetpoints( + () -> { + if (setAmpAngle) { + return ShooterConstants.kShooterAmpAngle; + } else { + return ShooterConstants.kShooterLoadAngle; + } + }, + () -> { + if (driver.getLeftTriggerAxis() > .25) { + return -1.0; + }else if(driver.getRightTriggerAxis() > 0.25){ + return 1.0; + } else { + return 0; + } + } + ) + ); + //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis)); climber.setDefaultCommand(climber.stop()); - indexer.setDefaultCommand(indexer.shootNote(operator::getRightTriggerAxis)); + indexer.setDefaultCommand(indexer.shootNote( + () -> { + if (driver.getLeftTriggerAxis() > .25) { + return -1.0; + }else { + return 0.0; + } + } + )); driver.povCenter().onFalse( drivetrain.driveCardinal( @@ -208,9 +239,18 @@ public class RobotContainer { */ driver.start().onTrue(drivetrain.toggleFieldRelativeControl()); - driver.leftBumper().toggleOnTrue(intake.intakeDownCommand()); + - operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis)); + //driver.leftBumper().toggleOnTrue(intake.intakeDownCommand()); + + operator.a().onTrue(new InstantCommand(() -> { setAmpAngle = true; })); + operator.a().onFalse(new InstantCommand(() -> { setAmpAngle = false; })); + + //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0)); + + operator.y().whileTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis)); + + driver.a().whileTrue(indexer.shootNote(() -> 1.0)); } @@ -229,10 +269,10 @@ public class RobotContainer { .withPosition(0, 0) .withSize(2, 1) .withWidget(BuiltInWidgets.kBooleanBox); - teleopTab.addBoolean("indexer beam break", indexer.getBeamBreak()); - teleopTab.addBoolean("shooter beam break", shooter.getBeamBreak()); - teleopTab.addDouble("shooter angle", shooter.getShooterAngle()); - teleopTab.addDouble("intake angle", intake.getIntakeAngle()); + teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak); + teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak); + teleopTab.addDouble("shooter angle", shooter::getShooterAngle); + teleopTab.addDouble("intake angle", intake::getIntakeAngle); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 28d067a..4486262 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -47,5 +47,5 @@ public final class DrivetrainConstants { public static final boolean kGyroReversed = true; - public static final double kRobotStartOffset = 90; + public static final double kRobotStartOffset = 0.0; } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index d9a1eb6..96af214 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -12,7 +12,7 @@ public class ShooterConstants { public static final double kShooterFF = 0.0; - public static final double kShooterPivotP = 0.0; + public static final double kShooterPivotP = 3.0; public static final double kShooterPivotI = 0.0; public static final double kShooterPivotD = 0.0; diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 45c1f25..4c577d0 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -16,12 +16,14 @@ public class Climber extends SubsystemBase { private DoubleSupplier shooterAngle; public Climber(DoubleSupplier shooterAngle) { - if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){ - motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); - motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); + motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); + motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); + - motor2.follow(motor1); - } + motor2.follow(motor1); + motor1.setInverted(true); + motor2.setInverted(true); + this.shooterAngle = shooterAngle; } public Command setSpeedWithSupplier(DoubleSupplier speed) { diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 095c0e6..a3a9424 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -55,7 +55,7 @@ public class Indexer extends SubsystemBase{ }); } - public BooleanSupplier getBeamBreak(){ - return indexerBeamBreak::get; + public boolean getBeamBreak(){ + return indexerBeamBreak.get(); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2e8b59f..17f2649 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -27,7 +27,7 @@ public class Intake extends SubsystemBase{ intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); intakeRoller.setSmartCurrentLimit(60); - intakeRoller.setIdleMode(IdleMode.kCoast); + intakeRoller.setIdleMode(IdleMode.kBrake); intakeRoller.burnFlash(); intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless); @@ -109,7 +109,7 @@ public class Intake extends SubsystemBase{ }); } - public DoubleSupplier getIntakeAngle(){ - return intakeEncoder::getPosition; + public double getIntakeAngle(){ + return intakeEncoder.getPosition(); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index b992a88..1f23184 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -28,9 +28,6 @@ public class Shooter extends SubsystemBase{ private Encoder pivotEncoder; - private SparkPIDController topPID; - private SparkPIDController bottomPID; - private PIDController shooterPivotPID; private ArmFeedforward shooterPivotFF; @@ -41,15 +38,19 @@ public class Shooter extends SubsystemBase{ public Shooter(BooleanSupplier indexerBeamBreak){ topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); + shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); - + shooterPivot.setInverted(true); + topEncoder = topShooter.getEncoder(); bottomEncoder = bottomShooter.getEncoder(); + /* topPID = topShooter.getPIDController(); topPID.setFeedbackDevice(topEncoder); bottomPID = bottomShooter.getPIDController(); bottomPID.setFeedbackDevice(bottomEncoder); + */ shooterPivot.setSmartCurrentLimit(50); topShooter.setSmartCurrentLimit(40); @@ -60,46 +61,52 @@ public class Shooter extends SubsystemBase{ shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel); - topShooter.setIdleMode(IdleMode.kBrake); + topShooter.setIdleMode(IdleMode.kCoast); bottomShooter.setIdleMode(IdleMode.kCoast); bottomShooter.burnFlash(); shooterPivot.burnFlash(); topShooter.burnFlash(); - topPID.setP(ShooterConstants.kShooterP); - topPID.setI(ShooterConstants.kShooterI); - topPID.setD(ShooterConstants.kShooterD); - topPID.setFF(ShooterConstants.kShooterFF); - - bottomPID.setP(ShooterConstants.kShooterP); - bottomPID.setI(ShooterConstants.kShooterI); - bottomPID.setI(ShooterConstants.kShooterD); - bottomPID.setFF(ShooterConstants.kShooterFF); - shooterPivotPID = new PIDController( ShooterConstants.kShooterPivotP, ShooterConstants.kShooterPivotI, ShooterConstants.kShooterPivotD ); + shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF); } + /* public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ return run(()-> { angleAndSpeedControl(setpointAngle, topRPM, bottomRPM); }); + }*/ + + public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){ + return run(()-> { + angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble()); + }); + } + + public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){ + return run(()-> { + angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM); + }); } private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){ shooterPivot.setIdleMode(IdleMode.kBrake); shooterPivot.setVoltage( - shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + + shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + shooterPivotFF.calculate(setpointAngle, 0.0)); - topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity); - bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity); + //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity); + //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity); + topShooter.set(topRPM); + bottomShooter.set(bottomRPM); } public Command ampHandoff(){ @@ -107,11 +114,11 @@ public class Shooter extends SubsystemBase{ shooterPivot.setIdleMode(IdleMode.kBrake); shooterPivot.setVoltage( - shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + + shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0)); if(shooterBeamBreak.get() || indexerBeamBreak){ - angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0); + angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.5, 0.5); }else{ angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0); } @@ -125,12 +132,12 @@ public class Shooter extends SubsystemBase{ }); } - public DoubleSupplier getShooterAngle(){ - return () -> {return pivotEncoder.getDistance();}; + public double getShooterAngle(){ + return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle; } - public BooleanSupplier getBeamBreak(){ - return shooterBeamBreak::get; + public Boolean getBeamBreak(){ + return shooterBeamBreak.get(); } public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){ @@ -141,4 +148,5 @@ public class Shooter extends SubsystemBase{ }); } + }