diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3c76dac..9c84de8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,7 +19,6 @@ import static edu.wpi.first.units.Units.KilogramSquareMeters; import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; @@ -40,6 +39,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.RGBWColor; import com.ctre.phoenix6.signals.StatusLedWhenActiveValue; @@ -67,6 +67,7 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.lib.W8.io.motor.MotorIOTalonFX.TalonFXFollower; import frc.lib.W8.mechanisms.linear.LinearMechanism.LinearMechCharacteristics; import frc.lib.W8.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics; import frc.lib.W8.util.Device; @@ -292,7 +293,7 @@ public final class ShooterConstants { public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); - public static final double HARD_STOP_CURRENT_LIMIT = 2.5; + public static final double HARD_STOP_CURRENT_LIMIT = 3.0; } public class ShooterFlywheelConstants { @@ -302,16 +303,19 @@ public class ShooterFlywheelConstants { public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); - private static final double GEARING = (2.0 / 1.0); + private static final double GEARING = (1.0 / 1.0); public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + public static final TalonFXFollower FOLLOWER_1 = + new TalonFXFollower(Ports.LeftFlywheel, MotorAlignmentValue.Opposed); + // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(18.0).withKI(0.0).withKD(0.01).withKV(0.05).withKS(8.0); + new Slot0Configs().withKP(3.0).withKI(0.15).withKD(0.0).withKV(0.1).withKS(8.5); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -381,7 +385,7 @@ public class ShooterRotaryConstants { // Positional PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(250.0).withKI(0.0).withKD(0.0).withKS(10.0).withKV(0.0); + new Slot0Configs().withKP(600.0).withKI(0.0).withKD(0.0).withKS(10.0).withKV(0.0); // Velocity PID private static Slot1Configs SLOT1CONFIG = new Slot1Configs().withKP(30.0).withKI(0.0).withKD(0.0).withKS(40.0).withKV(0.0); @@ -423,7 +427,7 @@ public static TalonFXConfiguration getFXConfig() { config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Degrees); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Degrees); config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; @@ -486,7 +490,7 @@ public class IntakeFlywheelConstants { public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); - public static final double GEARING = (5.0 / 1.0); + public static final double GEARING = (22.0 / 18.0); public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -513,53 +517,9 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) { return config; } - public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - - // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); - public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 0 - 1.0 // Camera 1 - }; - - /** Tags used for reef alignment */ - public static List alignmentTags = - Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22); - - public static VisionSystemSim getSystemSim() { - var system = new VisionSystemSim("main"); - system.addAprilTags(aprilTagLayout); - return system; - } - } - // config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(30).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0); + new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0); public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -600,25 +560,68 @@ public static TalonFXConfiguration getFXConfig() { } } + public class VisionConstants { + // AprilTag layout + public static AprilTagFieldLayout aprilTagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + + // Camera names, must match names configured on coprocessor + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + // Robot to camera transforms + // (Not used by Limelight, configure in web UI instead) + public static Transform3d robotToCamera0 = + new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + public static Transform3d robotToCamera1 = + new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + + // Basic filtering thresholds + public static double maxAmbiguity = 0.3; + public static double maxZError = 0.75; + + // Standard deviation baselines, for 1 meter distance and 1 tag + // (Adjusted automatically based on distance and # of tags) + public static double linearStdDevBaseline = 0.02; // Meters + public static double angularStdDevBaseline = 0.06; // Radians + + // Standard deviation multipliers for each camera + // (Adjust to trust some cameras more than others) + public static double[] cameraStdDevFactors = + new double[] { + 1.0, // Camera 0 + 1.0 // Camera 1 + }; + + /** Tags used for reef alignment */ + public static List alignmentTags = + Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22); + + public static VisionSystemSim getSystemSim() { + var system = new VisionSystemSim("main"); + system.addAprilTags(aprilTagLayout); + return system; + } + } + public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Degrees.of(120.0); + public static final Angle PICKUP_ANGLE = Degrees.of(106.0); public static final Angle STOW_ANGLE = Degrees.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); - public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); - public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100); - public static final Velocity JERK = - RadiansPerSecondPerSecond.per(Second).of(0.1); + public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(100); + public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(200); + public static final Velocity JERK = ACCELERATION.per(Second); - private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); - private static final double SENSOR_TO_MECHANISM = 1.0; + private static final double ROTOR_TO_SENSOR = (1.0 / 1.0); + private static final double SENSOR_TO_MECHANISM = 17.35; public static final Angle MIN_ANGLE = Degrees.of(0.0); public static final Angle MAX_ANGLE = Degrees.of(130.0); - public static final Angle STARTING_ANGLE = Radians.zero(); + public static final Angle STARTING_ANGLE = Degrees.of(0.0); public static final Distance ARM_LENGTH = Foot.one(); public static final RotaryMechCharacteristics CONSTANTS = @@ -635,9 +638,10 @@ public class IntakePivotConstants { // Positional PID public static final Slot0Configs SLOT_0_CONFIG = - new Slot0Configs().withKP(.0).withKI(0.0).withKD(0).withKS(0.00).withKV(0.0); + new Slot0Configs().withKP(300.0).withKI(1000).withKD(80).withKS(8.0).withKV(0.0); - // ^^^ CHANGE + // .withKG(15) + // .withGravityType(GravityTypeValue.Arm_Cosine); public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -656,10 +660,10 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Rotations); config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR; @@ -685,7 +689,7 @@ public class FeederConstants { public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); - private static final double GEARING = (15.0 / 30.0); + private static final double GEARING = (15.0 / 7.0); public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); @@ -738,7 +742,7 @@ public class ClimberConstants { public static final String MOTOR_NAME = "Climber motor"; public static final String NAME = "Climber"; public static final Distance TOLERANCE = Inches.of(0.1); - public static final double GEARING = 1; + public static final double GEARING = 140.8; public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -746,11 +750,11 @@ public class ClimberConstants { public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity CRUISE_VELOCITY = - RadiansPerSecond.of(2 * Math.PI).times(8000.0); + RadiansPerSecond.of(2 * Math.PI).times(800.0 / GEARING); public static final AngularVelocity LOWER_VELOCITY = - RadiansPerSecond.of(-2 * Math.PI).times(8000.0); + RadiansPerSecond.of(-2 * Math.PI).times(800.0 / GEARING); public static final AngularVelocity CALIBRATE_VELOCITY = - RadiansPerSecond.of(-2 * Math.PI).times(1000.0); + RadiansPerSecond.of(-2 * Math.PI).times(100.0 / GEARING); public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); @@ -797,6 +801,7 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.SensorToMechanismRatio = GEARING; config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0); + config.Slot1 = new Slot1Configs().withKP(1).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba23689..7afa381 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,9 @@ package frc.robot; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; @@ -21,9 +23,11 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.W8.io.motor.*; +import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; @@ -37,11 +41,11 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; -import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Climber; @@ -109,16 +113,12 @@ public RobotContainer() { Ports.Spindexer))); shooter = new Shooter( - new FlywheelMechanismReal( - new MotorIOTalonFX( - "ShooterLeftFlywheel", - ShooterFlywheelConstants.getFXConfig(true), - Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( "ShooterRightFlywheel", ShooterFlywheelConstants.getFXConfig(false), - Ports.RightFlywheel)), + Ports.RightFlywheel, + ShooterFlywheelConstants.FOLLOWER_1)), new FlywheelMechanismReal( new MotorIOTalonFX( "ShooterTower", FeederConstants.getFXConfig(false), Ports.TowerRoller)), @@ -181,19 +181,12 @@ public RobotContainer() { HopperConstants.TOLERANCE)); shooter = new Shooter( - new FlywheelMechanismSim( - new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), - Ports.LeftFlywheel), - ShooterFlywheelConstants.DCMOTOR, - ShooterFlywheelConstants.MOI, - ShooterFlywheelConstants.TOLERANCE), new FlywheelMechanismSim( new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, ShooterFlywheelConstants.getFXConfig(true), - Ports.RightFlywheel), + Ports.RightFlywheel, + ShooterFlywheelConstants.FOLLOWER_1), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, ShooterFlywheelConstants.TOLERANCE), @@ -272,7 +265,6 @@ public RobotContainer() { shooter = new Shooter( - new FlywheelMechanism() {}, new FlywheelMechanism() {}, new FlywheelMechanism() {}, new RotaryMechanism(null, null) {}); @@ -371,7 +363,7 @@ private void configureButtonBindings() { // controller.x().onTrue(intake.stowAndStopRollers()); // Flywheel - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(25))); + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(20))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); // Intake + Spindexer + Tower @@ -391,8 +383,11 @@ private void configureButtonBindings() { shooter.runTower(RotationsPerSecond.of(0)))); // Intake Rollers 11 Motor: 9 Intake - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); - controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); + + controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); + controller + .a() + .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); // Spindexer 1:1 // controller.x().whileTrue(hopper.runSpindexer(18)); @@ -400,21 +395,40 @@ private void configureButtonBindings() { // Tower - 15 Motor:7 Tower - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); - controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); + // controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); + // controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude())); - controller.b().whileTrue(shooter.setHoodAngle(5)); + // controller.povUp().onTrue(shooter.calibrateHood()); + // controller.povLeft().onTrue(shooter.setHoodAngle(10)); + // controller.povDown().onTrue(shooter.setHoodAngle(15)); + // controller.povRight().onTrue(shooter.setHoodAngle(20)); // controller.x().onFalse(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); controller.x().whileTrue(intake.setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + controller.y().whileTrue(intake.setPivotAngle(IntakePivotConstants.STOW_ANGLE)); + + controller.b().whileTrue(intake.zeroEncoder()); - controller.povRight().onTrue(shooter.calibrateHood()); + // controller.povRight().onTrue(shooter.calibrateHood()); - // controller.povUp().whileTrue(climber.raiseClimber()); - // controller.povUp().onFalse(climber.stopClimber()); - // controller.povDown().whileTrue(climber.lowerClimber()); - // controller.povDown().onFalse(climber.stopClimber()); + controller + .povLeft() + .whileTrue( + new RunCommand( + () -> + climber._io.runPosition( + Rotations.of(0.25), + ClimberConstants.CRUISE_VELOCITY, + ClimberConstants.ACCELERATION, + ClimberConstants.JERK, + PIDSlot.SLOT_1), + climber)); + controller.povLeft().onFalse(climber.stopClimber()); + controller.povUp().whileTrue(climber.raiseClimber()); + controller.povUp().onFalse(climber.stopClimber()); + controller.povDown().whileTrue(climber.lowerClimber()); + controller.povDown().onFalse(climber.stopClimber()); } /** diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c891c3c..845e8c3 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -25,24 +25,26 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100) + .withKP(2000) .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(2.66) + .withKD(20) + .withKS(0.0) + .withKV(0.0) .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + new Slot0Configs().withKP(60.0).withKI(0).withKD(0).withKS(7.262990).withKV(0.0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType kSteerClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + private static final ClosedLoopOutputType kDriveClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The type of motor used for the drive motor private static final DriveMotorArrangement kDriveMotorType = diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 5b091a0..848b38e 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -12,7 +12,7 @@ import frc.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { - private LinearMechanism _io; + public LinearMechanism _io; Distance goalDistance; public Climber(LinearMechanism io) { @@ -91,8 +91,6 @@ public Command stopClimber() { } public Command raiseClimber() { - System.out.println(ClimberConstants.CRUISE_VELOCITY); - System.out.println(ClimberConstants.ACCELERATION); return this.run( () -> _io.runVelocity( diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e5476ce..0ff58c8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -18,11 +19,10 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; -import frc.robot.Robot; import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { - private FlywheelMechanism _rollerIO; + public FlywheelMechanism _rollerIO; private RotaryMechanism _pivotIO; double velocity; double pivotAngle; @@ -34,7 +34,7 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; - if (Robot.isReal()) tunePivotPosition(); + // if (Robot.isReal()) tunePivotPosition(); simBalls = 0; } @@ -52,10 +52,14 @@ public Command setPivotAngle(Angle pivotAngle) { () -> _pivotIO.runPosition( pivotAngle, - IntakeFlywheelConstants.CRUISE_VELOCITY, - IntakeFlywheelConstants.ACCELERATION, - IntakeFlywheelConstants.JERK, - PIDSlot.SLOT_0)); + IntakePivotConstants.CRUISE_VELOCITY, + IntakePivotConstants.ACCELERATION, + IntakePivotConstants.JERK, + PIDSlot.SLOT_0) + // () -> + // _pivotIO.runVelocity(IntakePivotConstants.CRUISE_VELOCITY, + // IntakePivotConstants.ACCELERATION, PIDSlot.SLOT_0) + ); // .withName("Go To " + setpoint.toString() + " Setpoint"); } @@ -116,6 +120,10 @@ public void tunePivotPosition() { _pivotIO.setEncoderPosition(Rotations.of(IntakePivotConstants.ENCODER1.get())); } + public Command zeroEncoder() { + return Commands.runOnce(() -> _pivotIO.setEncoderPosition(Degrees.of(0))); + } + @Override public void periodic() { // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index af75f1b..b85a226 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -33,8 +33,7 @@ public class Shooter extends SubsystemBase { - private final FlywheelMechanism _rflywheel; - private final FlywheelMechanism _lflywheel; + private final FlywheelMechanism _flywheel; private final FlywheelMechanism _feeder; private final RotaryMechanism _hood; @@ -49,13 +48,8 @@ public class Shooter extends SubsystemBase { private Debouncer homeDebouncer = new Debouncer(0.1, DebounceType.kRising); private Trigger homedTrigger; - public Shooter( - FlywheelMechanism lflywheel, - FlywheelMechanism rflywheel, - FlywheelMechanism feeder, - RotaryMechanism hood) { - _lflywheel = lflywheel; - _rflywheel = rflywheel; + public Shooter(FlywheelMechanism rflywheel, FlywheelMechanism feeder, RotaryMechanism hood) { + _flywheel = rflywheel; _feeder = feeder; _hood = hood; homedTrigger = @@ -79,8 +73,7 @@ public void setFlywheelVelocity(AngularVelocity velocity) { // this.desiredVelo = velocity; // AngularVelocity angVelo = RotationsPerSecond.of(velocity); // AngularVelocity negangVelo = RotationsPerSecond.of(velocity); - _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _flywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); targetVelocity = velocity; } @@ -101,9 +94,7 @@ public enum State { // Checks if the flywheel is at speed and returns a boolean public boolean flyAtVelocity() { - return ((Math.abs(desiredVelo - _lflywheel.getVelocity().in(RotationsPerSecond)) - + Math.abs(desiredVelo - _rflywheel.getVelocity().in(RotationsPerSecond))) - / 2) + return (Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond))) <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE; } @@ -242,8 +233,7 @@ public void simShoot() { public void periodic() { _hood.periodic(); - _lflywheel.periodic(); - _rflywheel.periodic(); + _flywheel.periodic(); _feeder.periodic(); Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); Logger.recordOutput("Feeder/TargetVelocity", feederTargetVelocity);