From 509ef2d00aacb1b4995d7af544113ae2f0f64cf6 Mon Sep 17 00:00:00 2001 From: Jacob M Date: Thu, 5 Mar 2026 20:28:59 -0500 Subject: [PATCH 1/3] the robot now has fully functional teleop code; indexer not very mechanically consistent --- src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/Robot.java | 24 ++++- .../commands/driveStates/TeleopDrive.java | 2 +- .../robot/commands/states/OnSideState.java | 90 +++++++++++++++++++ .../subsystems/shooter/ShooterStack.java | 8 +- .../flywheel/FlywheelIOCompetition.java | 3 +- 6 files changed, 125 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/states/OnSideState.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5c78f6e..19a7a02 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -158,13 +158,14 @@ public static class Flywheel{ public static final double simD = 0.0; public static final double simS = 8; - public static double motionMagicCruise = 100; - public static double motionMagicAccel = 50; + public static double motionMagicCruise = 400; + public static double motionMagicAccel = 600; - public static final double compP = 13; + public static final double compP = 4.5; public static final double compI = 0; public static final double compD = 0; - public static final double compS = 0; + public static final double compS = 6; + public static final double compV = 0.1; public static final double gearRatio = 0.75; public static double currentLimit = 60; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68de313..1ed1b8b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -188,7 +188,7 @@ public Robot() { )); indexer = new Indexer(new IndexerIOCompetition()); intake = new Intake(new IntakeIOCompetition()); - shooterLimelight = new LimelightCamera("limelight-turret", new Pose3d(Units.inchesToMeters(-12.140), Units.inchesToMeters(-12.745), Units.inchesToMeters(19.296), new Rotation3d(0, 0, Units.degreesToRadians(145))), LimelightCamera.limelightPipeline.APRIL_TAG); +// shooterLimelight = new LimelightCamera("limelight-turret", new Pose3d(Units.inchesToMeters(-12.140), Units.inchesToMeters(-12.745), Units.inchesToMeters(19.296), new Rotation3d(0, 0, Units.degreesToRadians(145))), LimelightCamera.limelightPipeline.APRIL_TAG); // swerveLimelight = new LimelightCamera("limelight-swerve", new Pose3d(Units.inchesToMeters(6.833), Units.inchesToMeters(11.255), Units.inchesToMeters(8.691), new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(-35))), LimelightCamera.limelightPipeline.APRIL_TAG); break; @@ -269,7 +269,7 @@ public Robot() { // initialize default state and drive commands RobotControl.setDriveModeCommand(DriveModes.teleopDrive); - RobotControl.setCurrentMode(RobotTransitions.shooterStacksInit); +// RobotControl.setCurrentMode(RobotStates.onSideState); // drivetrain.setPose(new Pose2d(0, 2, Rotation2d.fromDegrees(32))); } @@ -289,6 +289,26 @@ public void robotPeriodic() { // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); + if (Robot.driverController.getLeftTriggerAxis() > 0.5) { + // Intake mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); + Robot.intake.setPivotPosition(0); + Robot.shooterArray.setFeederVelocity(-10); + } else if (Robot.driverController.getRightTriggerAxis() > 0.5) { + // Shoot mode + Robot.shooterArray.enableShooting(true); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); +// Robot.intake.setPivotPosition(0.15); + } else { + // Neutral mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(0); + Robot.indexer.setIndexVelocity(0); + Robot.shooterArray.setFeederVelocity(0); + } } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/commands/driveStates/TeleopDrive.java b/src/main/java/frc/robot/commands/driveStates/TeleopDrive.java index 8e19fdd..9fdb41a 100644 --- a/src/main/java/frc/robot/commands/driveStates/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/driveStates/TeleopDrive.java @@ -16,8 +16,8 @@ public class TeleopDrive extends Command { public TeleopDrive() { DrivetrainPublisher.setSuppliers( - () -> -Robot.driverController.getLeftX(), Robot.driverController::getLeftY, + Robot.driverController::getLeftX, Robot.driverController::getRightX, () -> true ); diff --git a/src/main/java/frc/robot/commands/states/OnSideState.java b/src/main/java/frc/robot/commands/states/OnSideState.java new file mode 100644 index 0000000..9790eab --- /dev/null +++ b/src/main/java/frc/robot/commands/states/OnSideState.java @@ -0,0 +1,90 @@ +package frc.robot.commands.states; + +import ControlAnnotations.State; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.TriggerBoard; +import frc.robot.commands.basicCommands.intake.IntakeRollerDriveVelocityMatch; +import frc.robot.commands.transitions.OnSideEnterTransition; +import frc.robot.subsystems.robotControl.RobotControl; +import frc.robot.util.HubState; +import frc.robot.utils.RobotTransitions; + +import static edu.wpi.first.units.Units.Seconds; + +@State +public class OnSideState extends Command { + + public OnSideState() { + + } + + @Override + public void initialize() { +// Robot.indexer.setIndexVelocity(15); +// Robot.intake.setPivotPosition(0); +// Robot.intake.setRollerVelocitySupplier(() -> MathUtil.clamp(Math.sqrt(((Robot.drivetrain.getChassisSpeeds().vxMetersPerSecond * Robot.drivetrain.getChassisSpeeds().vxMetersPerSecond) + (Robot.drivetrain.getChassisSpeeds().vyMetersPerSecond * Robot.drivetrain.getChassisSpeeds().vyMetersPerSecond)) / Constants.Intake.rollerRadius) / (2 * Math.PI), 40.0, 100)); + } + + @Override + public void execute() { +// if (TriggerBoard.isRobotInNoShootingZone()) { +// inactive(); +// } else { +// if (HubState.isActive()) { +// active(); +// } else { +// inactive(); +// } +// } +// +// if (HubState.timeRemainingInCurrentShift().get().in(Seconds) <= Constants.shiftOffset && !HubState.isActiveNext()) { +// RobotControl.setCurrentMode(RobotTransitions.hubInactiveTransition); +// return; +// } else if (HubState.timeRemainingInCurrentShift().get().in(Seconds) <= Constants.shiftOffset && HubState.isActiveNext()) { +// RobotControl.setCurrentMode(RobotTransitions.hubActiveTransition); +// return; +// } + +// if (TriggerBoard.isInNeutralZone()) { +// RobotControl.setCurrentMode(RobotTransitions.neutralZoneEnterTransition); +// return; +// } + active(); + System.out.println("Ran execute from OnSideState"); + } + + public void active() { + if (Robot.driverController.getLeftTriggerAxis() > 0.5) { + // Intake mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); + Robot.intake.setPivotPosition(0); + } else if (Robot.driverController.getRightTriggerAxis() > 0.5) { + // Shoot mode + Robot.shooterArray.enableShooting(true); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); +// Robot.intake.setPivotPosition(0.15); + } else { + // Neutral mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(0); + Robot.indexer.setIndexVelocity(0); + } + System.out.println("Ran Active function from OnSideState"); + } + + public void inactive() { + Robot.shooterArray.enableShooting(false); + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java b/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java index fb0196d..e2adad3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java @@ -61,6 +61,7 @@ public void periodic() { Logger.recordOutput(name + " Distance To Target", distanceToTarget); double targetTurretRotation = calculateTurretRotation(); double angleToTarget = targetTurretRotation - drivetrain.getRotation().getRadians(); + Logger.recordOutput(name + "shooting enabled", this.shootingEnabled); if (pointToTarget) { @@ -74,7 +75,7 @@ public void periodic() { if (shootingEnabled) { // System.out.println("shooting enabled"); - flywheel.setVelocity(50); + flywheel.setVelocity(60); // flywheel.setVelocity( // flywheelMap.get(distanceToTarget) != null ? // flywheelMap.get(distanceToTarget) + Units.radiansToRotations(calculateFlywheelVelocityCorrection((angleToTarget))) @@ -90,12 +91,13 @@ public void periodic() { return; } + if (!(flywheel.getVelocity() >= flywheel.getTargetVelocity() - 10) || !shootingEnabled) { -// feeder.setFeedVelocity(-10); + feeder.setFeedVelocity(-10); } else if ((flywheel.getVelocity() >= flywheel.getTargetVelocity() - 10) && shootingEnabled) { feeder.setFeedVelocity(50); } else { -// feeder.setFeedVelocity(-10); + feeder.setFeedVelocity(-10); } if (Constants.currentMode == Constants.Mode.SIM) { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOCompetition.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOCompetition.java index 7583e89..a772426 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOCompetition.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOCompetition.java @@ -39,7 +39,7 @@ public FlywheelIOCompetition(int CANID) { flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; flywheelConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - flywheelConfig.Feedback.RotorToSensorRatio = Constants.Shooter.Flywheel.gearRatio; + flywheelConfig.Feedback.SensorToMechanismRatio = Constants.Shooter.Flywheel.gearRatio; flywheelConfig.CurrentLimits.StatorCurrentLimitEnable = true; flywheelConfig.CurrentLimits.StatorCurrentLimit = Constants.Shooter.Flywheel.currentLimit; @@ -47,6 +47,7 @@ public FlywheelIOCompetition(int CANID) { flywheelSlot0Config.kI = Constants.Shooter.Flywheel.compI; flywheelSlot0Config.kD = Constants.Shooter.Flywheel.compD; flywheelSlot0Config.kS = Constants.Shooter.Flywheel.compS; + flywheelSlot0Config.kV = Constants.Shooter.Flywheel.compV; flywheelMotionMagicConfig.MotionMagicCruiseVelocity = Constants.Shooter.Flywheel.motionMagicCruise; flywheelMotionMagicConfig.MotionMagicAcceleration = Constants.Shooter.Flywheel.motionMagicAccel; From dfc0620cc1c58c02dfc06fe8c1aaa7fe2204795f Mon Sep 17 00:00:00 2001 From: Jacob M Date: Thu, 5 Mar 2026 21:25:46 -0500 Subject: [PATCH 2/3] added auton and better shooter tuning --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 45 ++++++++++--------- src/main/java/frc/robot/RobotContainer.java | 3 ++ .../java/frc/robot/commands/BasicAuton.java | 24 ++++++++++ .../robot/commands/states/OnSideState.java | 12 ++--- .../subsystems/shooter/ShooterStack.java | 4 +- 6 files changed, 59 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc/robot/commands/BasicAuton.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 19a7a02..3fce07d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -235,7 +235,7 @@ public static class Intake{ public static final double PivotGearRatio = 11.555; public static final double PivotMotionMagicCruise = 100; public static final double PivotMotionMagicAccel = 200; - public static final double PivotkP = 10.0; + public static final double PivotkP = 200.0; public static final double PivotkI = 0.0; public static final double PivotkD = 0.0; public static final double PivotkS = 0.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ed1b8b..3734b06 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -269,7 +269,7 @@ public Robot() { // initialize default state and drive commands RobotControl.setDriveModeCommand(DriveModes.teleopDrive); -// RobotControl.setCurrentMode(RobotStates.onSideState); + RobotControl.setCurrentMode(RobotStates.onSideState); // drivetrain.setPose(new Pose2d(0, 2, Rotation2d.fromDegrees(32))); } @@ -289,26 +289,6 @@ public void robotPeriodic() { // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); - if (Robot.driverController.getLeftTriggerAxis() > 0.5) { - // Intake mode - Robot.shooterArray.enableShooting(false); - Robot.intake.setRollerVelocity(50); - Robot.indexer.setIndexVelocity(15); - Robot.intake.setPivotPosition(0); - Robot.shooterArray.setFeederVelocity(-10); - } else if (Robot.driverController.getRightTriggerAxis() > 0.5) { - // Shoot mode - Robot.shooterArray.enableShooting(true); - Robot.intake.setRollerVelocity(50); - Robot.indexer.setIndexVelocity(15); -// Robot.intake.setPivotPosition(0.15); - } else { - // Neutral mode - Robot.shooterArray.enableShooting(false); - Robot.intake.setRollerVelocity(0); - Robot.indexer.setIndexVelocity(0); - Robot.shooterArray.setFeederVelocity(0); - } } /** This function is called once when the robot is disabled. */ @@ -352,7 +332,28 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + if (Robot.driverController.getLeftTriggerAxis() > 0.5) { + // Intake mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); + Robot.intake.setPivotPosition(0); + Robot.shooterArray.setFeederVelocity(-10); + } else if (Robot.driverController.getRightTriggerAxis() > 0.5) { + // Shoot mode + Robot.shooterArray.enableShooting(true); + Robot.intake.setRollerVelocity(50); + Robot.indexer.setIndexVelocity(15); +// Robot.intake.setPivotPosition(0.15); + } else { + // Neutral mode + Robot.shooterArray.enableShooting(false); + Robot.intake.setRollerVelocity(0); + Robot.indexer.setIndexVelocity(0); + Robot.shooterArray.setFeederVelocity(0); + } + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61622fa..a3258e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.BasicAuton; import frc.robot.commands.PanicCommand; import frc.robot.commands.SetMode; import frc.robot.commands.basicCommands.exampleCommands.ExampleCommand; @@ -52,6 +53,8 @@ public RobotContainer() { // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); + autoChooser.addDefaultOption("Basic Auto", new BasicAuton()); + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/BasicAuton.java b/src/main/java/frc/robot/commands/BasicAuton.java new file mode 100644 index 0000000..a144a45 --- /dev/null +++ b/src/main/java/frc/robot/commands/BasicAuton.java @@ -0,0 +1,24 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Robot; + +public class BasicAuton extends SequentialCommandGroup { + + public BasicAuton() { + addCommands( + new InstantCommand(() -> Robot.drivetrain.setPose(new Pose2d(new Translation2d(), new Rotation2d(Units.degreesToRadians(180))))), + new InstantCommand(() -> { + Robot.intake.setPivotPosition(0); + Robot.intake.setRollerVelocity(20); + Robot.indexer.setIndexVelocity(20); + Robot.shooterArray.enableShooting(true); + }) + ); + } +} diff --git a/src/main/java/frc/robot/commands/states/OnSideState.java b/src/main/java/frc/robot/commands/states/OnSideState.java index 9790eab..49e231a 100644 --- a/src/main/java/frc/robot/commands/states/OnSideState.java +++ b/src/main/java/frc/robot/commands/states/OnSideState.java @@ -53,11 +53,6 @@ public void execute() { // RobotControl.setCurrentMode(RobotTransitions.neutralZoneEnterTransition); // return; // } - active(); - System.out.println("Ran execute from OnSideState"); - } - - public void active() { if (Robot.driverController.getLeftTriggerAxis() > 0.5) { // Intake mode Robot.shooterArray.enableShooting(false); @@ -76,7 +71,12 @@ public void active() { Robot.intake.setRollerVelocity(0); Robot.indexer.setIndexVelocity(0); } - System.out.println("Ran Active function from OnSideState"); +// System.out.println("Ran Active function from OnSideState"); +// active(); +// System.out.println("Ran execute from OnSideState"); + } + + public void active() { } public void inactive() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java b/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java index e2adad3..0d6594a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterStack.java @@ -75,7 +75,7 @@ public void periodic() { if (shootingEnabled) { // System.out.println("shooting enabled"); - flywheel.setVelocity(60); + flywheel.setVelocity(62); // flywheel.setVelocity( // flywheelMap.get(distanceToTarget) != null ? // flywheelMap.get(distanceToTarget) + Units.radiansToRotations(calculateFlywheelVelocityCorrection((angleToTarget))) @@ -95,7 +95,7 @@ public void periodic() { if (!(flywheel.getVelocity() >= flywheel.getTargetVelocity() - 10) || !shootingEnabled) { feeder.setFeedVelocity(-10); } else if ((flywheel.getVelocity() >= flywheel.getTargetVelocity() - 10) && shootingEnabled) { - feeder.setFeedVelocity(50); + feeder.setFeedVelocity(35); } else { feeder.setFeedVelocity(-10); } From 0f38a4b15c197dfe232f8e13ef3071bffc00f0c8 Mon Sep 17 00:00:00 2001 From: Jacob M Date: Mon, 9 Mar 2026 18:06:53 -0400 Subject: [PATCH 3/3] week 1 complete code --- src/main/java/frc/robot/Constants.java | 15 +++++------ src/main/java/frc/robot/Robot.java | 25 ++++++++++++------- .../java/frc/robot/commands/BasicAuton.java | 4 +-- .../frc/robot/generated/TunerConstants.java | 2 +- .../robot/subsystems/drive/Drivetrain.java | 20 +++++++++++---- .../subsystems/drive/ModuleIOTalonFX.java | 8 +++--- .../intake/IntakeIOCompetition.java | 9 ++++++- .../subsystems/shooter/ShooterArray.java | 5 ++++ .../shooter/hood/HoodIOCompetition.java | 20 ++++++++++----- 9 files changed, 73 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3fce07d..903f130 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -203,10 +203,10 @@ public static class Indexer { public static final double simS = 0.0; // Competition bot - public static final double compP = 20; + public static final double compP = 15; public static final double compI = 0.0; public static final double compD = 0.0; - public static final double compS = 10; + public static final double compS = 4; public static final int indexerCanID = 25; @@ -227,19 +227,20 @@ public static class Intake{ public static final double RollerkP = 0.6; public static final double RollerkI = 0.0; public static final double RollerkD = 0.0; - public static final double RollerkS = 2.1; + public static final double RollerkS = .6; public static final double RollerkG = 0.0; public static final int pivotMotorCANID = 40; - public static final double PivotCurrentLimit = 40; + public static final double PivotCurrentLimit = 60; public static final double PivotGearRatio = 11.555; public static final double PivotMotionMagicCruise = 100; public static final double PivotMotionMagicAccel = 200; - public static final double PivotkP = 200.0; + public static final double PivotkP = 3.8; public static final double PivotkI = 0.0; public static final double PivotkD = 0.0; - public static final double PivotkS = 0.0; - public static final double PivotkG = 0.0; + public static final double PivotkS = 0.9; + public static final double PivotkG = 0.5; + public static final double PivotkV = 0.6; public static final double FrequencyUpdateRate = 20; public static double rollerRadius = Units.inchesToMeters(1); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3734b06..327b6e8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -269,7 +269,7 @@ public Robot() { // initialize default state and drive commands RobotControl.setDriveModeCommand(DriveModes.teleopDrive); - RobotControl.setCurrentMode(RobotStates.onSideState); +// RobotControl.setCurrentMode(RobotStates.onSideState); // drivetrain.setPose(new Pose2d(0, 2, Rotation2d.fromDegrees(32))); } @@ -336,22 +336,29 @@ public void teleopPeriodic() { if (Robot.driverController.getLeftTriggerAxis() > 0.5) { // Intake mode Robot.shooterArray.enableShooting(false); - Robot.intake.setRollerVelocity(50); - Robot.indexer.setIndexVelocity(15); - Robot.intake.setPivotPosition(0); + Robot.intake.setRollerVelocity(60); +// Robot.indexer.setIndexVelocity(25); + Robot.intake.setPivotPosition(-0.5); Robot.shooterArray.setFeederVelocity(-10); } else if (Robot.driverController.getRightTriggerAxis() > 0.5) { // Shoot mode Robot.shooterArray.enableShooting(true); - Robot.intake.setRollerVelocity(50); - Robot.indexer.setIndexVelocity(15); -// Robot.intake.setPivotPosition(0.15); + Robot.shooterArray.setHoodAngle(1100); + Robot.intake.setRollerVelocity(45); + Robot.indexer.setIndexVelocity(25); + Robot.intake.setPivotPosition(0.25); + } else if (Robot.driverController.getLeftBumperButton()) { + Robot.intake.setRollerVelocity(-30); + Robot.indexer.setIndexVelocity(-25); + Robot.shooterArray.setFeederVelocity(-20); } else { // Neutral mode Robot.shooterArray.enableShooting(false); - Robot.intake.setRollerVelocity(0); - Robot.indexer.setIndexVelocity(0); + Robot.intake.setRollerSpeed(0); + Robot.indexer.setIndexSpeed(0); + Robot.shooterArray.setHoodAngle(1000); Robot.shooterArray.setFeederVelocity(0); + Robot.intake.setPivotSpeed(0); } } diff --git a/src/main/java/frc/robot/commands/BasicAuton.java b/src/main/java/frc/robot/commands/BasicAuton.java index a144a45..b9d94e4 100644 --- a/src/main/java/frc/robot/commands/BasicAuton.java +++ b/src/main/java/frc/robot/commands/BasicAuton.java @@ -15,8 +15,8 @@ public BasicAuton() { new InstantCommand(() -> Robot.drivetrain.setPose(new Pose2d(new Translation2d(), new Rotation2d(Units.degreesToRadians(180))))), new InstantCommand(() -> { Robot.intake.setPivotPosition(0); - Robot.intake.setRollerVelocity(20); - Robot.indexer.setIndexVelocity(20); + Robot.intake.setRollerSpeed(0.2); + Robot.indexer.setIndexSpeed(0.20); Robot.shooterArray.enableShooting(true); }) ); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 22f08b5..000d5fb 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -45,7 +45,7 @@ public class TunerConstants { // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 589550e..34cc895 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -288,12 +288,22 @@ public void joystickDrive( // Square rotation value for more precise control omega = Math.copySign(omega * omega, omega); + ChassisSpeeds speeds; + // Convert to field relative speeds & send command - ChassisSpeeds speeds = - new ChassisSpeeds( - linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(), - omega * getMaxAngularSpeedRadPerSec()); + if (Robot.getAlliance() == Alliance.Red) { + speeds = + new ChassisSpeeds( + linearVelocity.getX() * getMaxLinearSpeedMetersPerSec() * 0.5, + linearVelocity.getY() * getMaxLinearSpeedMetersPerSec() * 0.5, + omega * getMaxAngularSpeedRadPerSec() * 0.5); + } else { + speeds = + new ChassisSpeeds( + -linearVelocity.getX() * getMaxLinearSpeedMetersPerSec() * 0.5, + -linearVelocity.getY() * getMaxLinearSpeedMetersPerSec() * 0.5, + omega * getMaxAngularSpeedRadPerSec() * 0.5); + } boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index c770925..dc70a8a 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -183,7 +183,7 @@ public void updateInputs(ModuleIOInputs inputs) { public void setDriveOpenLoop(double output) { driveTalon.setControl( switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); + case Voltage -> voltageRequest.withOutput(output).withEnableFOC(false); case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); }); } @@ -192,7 +192,7 @@ public void setDriveOpenLoop(double output) { public void setTurnOpenLoop(double output) { turnTalon.setControl( switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> voltageRequest.withOutput(output); + case Voltage -> voltageRequest.withOutput(output).withEnableFOC(false); case TorqueCurrentFOC -> torqueCurrentRequest.withOutput(output); }); } @@ -203,7 +203,7 @@ public void setDriveVelocity(double wheelVelocityRadPerSec) { Units.radiansToRotations(wheelVelocityRadPerSec) * constants.DriveMotorGearRatio; driveTalon.setControl( switch (constants.DriveMotorClosedLoopOutput) { - case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec); + case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec).withEnableFOC(false); case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(motorVelocityRotPerSec); }); } @@ -212,7 +212,7 @@ public void setDriveVelocity(double wheelVelocityRadPerSec) { public void setTurnPosition(Rotation2d rotation) { turnTalon.setControl( switch (constants.SteerMotorClosedLoopOutput) { - case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()); + case Voltage -> positionVoltageRequest.withPosition(rotation.getRotations()).withEnableFOC(false); case TorqueCurrentFOC -> positionTorqueCurrentRequest.withPosition(rotation.getRotations()); }); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOCompetition.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOCompetition.java index e085dba..7304dfb 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOCompetition.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOCompetition.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -92,6 +93,7 @@ public IntakeIOCompetition() { pivotMotorSlot0Config.kD = Constants.Intake.PivotkD; pivotMotorSlot0Config.kS = Constants.Intake.PivotkS; pivotMotorSlot0Config.kG = Constants.Intake.PivotkG; + pivotMotorSlot0Config.kV = Constants.Intake.PivotkV; pivotMotor.getConfigurator().apply(pivotMotorConfiguration); pivotMotor.getConfigurator().apply(pivotMotorMotionMagicConfig); @@ -139,6 +141,11 @@ public void updateInputs(IntakeIOInputs inputs) { } case POSITION -> { pivotMotor.setControl(new MotionMagicVoltage(pivotTargetPosition)); +// if (pivotMotorPosition.getValueAsDouble() < 0.18) { +// pivotMotor.setControl(new MotionMagicExpoVoltage(pivotTargetPosition).withFeedForward(50)); +// } else { +// pivotMotor.setControl(new MotionMagicVoltage(pivotTargetPosition)); +// } } case VELOCITY -> { break; @@ -170,7 +177,7 @@ public void setPivotPosition(double position) { @Override public void setRollerSpeed(double velocity) { - this.rollerControlMode = ControlMode.VELOCITY; + this.rollerControlMode = ControlMode.POWER; rollerTargetPower = velocity; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterArray.java b/src/main/java/frc/robot/subsystems/shooter/ShooterArray.java index d987df9..1650e74 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterArray.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterArray.java @@ -104,4 +104,9 @@ public void setFeederVelocity(double velocity) { }); } + public void setHoodAngle(int angle) { + shooterStacks.forEach((shooterStackName, shooterStack) -> { + shooterStack.setHoodAngle(angle); + }); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOCompetition.java b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOCompetition.java index c156b6b..89aa031 100644 --- a/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOCompetition.java +++ b/src/main/java/frc/robot/subsystems/shooter/hood/HoodIOCompetition.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.shooter.hood; +import com.revrobotics.ResetMode; import com.revrobotics.servohub.ServoChannel; +import com.revrobotics.servohub.ServoHub; import com.revrobotics.servohub.config.ServoChannelConfig; -import edu.wpi.first.math.MathUtil; +import com.revrobotics.servohub.config.ServoHubConfig; import frc.robot.Constants; import frc.robot.Robot; @@ -13,7 +15,7 @@ public class HoodIOCompetition implements HoodIO{ private final ServoChannel servo; private double angleTarget = 0; - private int servoPulseWidth = 1000; + private int targetPulseWidth = 1000; public HoodIOCompetition(int servoChannel) { this.servoChannel = servoChannel; @@ -23,6 +25,12 @@ public HoodIOCompetition(int servoChannel) { channelConfig.disableBehavior(ServoChannelConfig.BehaviorWhenDisabled.kDoNotSupplyPower); channelConfig.pulseRange(Constants.Shooter.Hood.minimumPulseWidth, 1500, Constants.Shooter.Hood.maximumPulseWidth); + ServoHubConfig servoHubConfig = new ServoHubConfig(); + + servoHubConfig.apply(ServoChannel.ChannelId.fromInt(servoChannel), channelConfig); + + Robot.servoHub.configure(servoHubConfig, ResetMode.kNoResetSafeParameters); + this.servo.setPowered(true); this.servo.setEnabled(true); @@ -30,10 +38,10 @@ public HoodIOCompetition(int servoChannel) { @Override public void updateInputs(HoodIOInputs inputs) { - servo.setPulseWidth(MathUtil.clamp(servoPulseWidth, Constants.Shooter.Hood.minimumPulseWidth, Constants.Shooter.Hood.maximumPulseWidth)); + this.servo.setPulseWidth(this.targetPulseWidth); inputs.targetAngle = this.angleTarget; - inputs.commandedPulseWidth = this.servoPulseWidth; + inputs.commandedPulseWidth = this.targetPulseWidth; inputs.currentAngle = this.servo.getPulseWidth(); } @@ -41,8 +49,8 @@ public void updateInputs(HoodIOInputs inputs) { public void setAngle(double angle) { // this.angleTarget = angle; // this.servoPulseWidth = angleToPulseWidth(angle); - this.servoPulseWidth = (int) angle; - } + this.targetPulseWidth = (int) angle; + } // TODO: give this function an actual output that does what it should private int angleToPulseWidth(double angle) {