diff --git a/2024Crescendo/src/main/java/frc/robot/Constants.java b/2024Crescendo/src/main/java/frc/robot/Constants.java index 66629e2..096f209 100644 --- a/2024Crescendo/src/main/java/frc/robot/Constants.java +++ b/2024Crescendo/src/main/java/frc/robot/Constants.java @@ -29,6 +29,7 @@ public static class Climb { public static class Feeder { public static final int LEFT_ID = 17, RIGHT_ID = 18; public static final double FEED_TIME = 5.0, FEED_SPEED = -0.8, REVERSE_SPEED = 0.2; + public static final int BEAM_ID = 8; } public static class Swerve { @@ -135,16 +136,18 @@ public static class Shooter { public static final double SPEAKER_TOP_VELOCITY = 100, SPEAKER_TOP_ACCELERATION = 90; public static final double SPEAKER_BOTTOM_VELOCITY = 100, SPEAKER_BOTTOM_ACCELERATION = 90; + + public static final double THRESHOLD_SPEED = 100; } public static class Pivot { public static final int PIVOT_ID = 14, ENCODER_ID = 23; - public static final double GEAR_RATIO = 50/1; + public static final double GEAR_RATIO = 50.0/1.0; public static final double kP = 3, kI = 0, kD = 0, kD_TIME = 0.02; - public static final double CONVERSION_FACTOR = 1.0/360.0; - public static final double INTAKE_SAFE = 110 * Constants.Pivot.CONVERSION_FACTOR; - public static final double INTAKE_DOWN = 6 * Constants.Pivot.CONVERSION_FACTOR; + public static final double DEGREES_TO_REVOLUTIONS = 1.0/360.0; + public static final double INTAKE_SAFE = 110 * Constants.Pivot.DEGREES_TO_REVOLUTIONS; + public static final double INTAKE_DOWN = 6 * Constants.Pivot.DEGREES_TO_REVOLUTIONS; } } \ No newline at end of file diff --git a/2024Crescendo/src/main/java/frc/robot/RobotContainer.java b/2024Crescendo/src/main/java/frc/robot/RobotContainer.java index bc84a9f..4ef8357 100644 --- a/2024Crescendo/src/main/java/frc/robot/RobotContainer.java +++ b/2024Crescendo/src/main/java/frc/robot/RobotContainer.java @@ -122,6 +122,7 @@ private void configureBindings() { rightClimb.setDefaultCommand(new RunCommand(() -> rightClimb.runManual(() -> operatorController.getRightY()), rightClimb)); intake.setDefaultCommand(new RunIntake(intake, 0)); feeder.setDefaultCommand(new RunFeeder(feeder, 0)); + shooter.setDefaultCommand(new InstantCommand(() -> shooter.stopMotors())); driverController.start().onTrue(new InstantCommand(() -> swerve.resetGyro(), swerve)); driverController.x().onTrue(new InstantCommand(() -> swerve.defenseMode(), swerve)); diff --git a/2024Crescendo/src/main/java/frc/robot/commands/HomePivot.java b/2024Crescendo/src/main/java/frc/robot/commands/HomePivot.java index f6fb289..a5c3a6a 100644 --- a/2024Crescendo/src/main/java/frc/robot/commands/HomePivot.java +++ b/2024Crescendo/src/main/java/frc/robot/commands/HomePivot.java @@ -29,15 +29,15 @@ public void execute() { } @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { + public void end(boolean interrupted) { pivot.setpoint.velocity = 0; pivot.setpoint.position = pivot.getAbsPos(); pivot.goal.velocity = 0; pivot.goal.position = pivot.getAbsPos(); + } + @Override + public boolean isFinished() { SmartDashboard.putString("Did we finish", "yes"); return pivot.isReached(); diff --git a/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java b/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java index 884f7e8..0db4dea 100644 --- a/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java +++ b/2024Crescendo/src/main/java/frc/robot/commands/ShootSpeaker.java @@ -13,8 +13,8 @@ public class ShootSpeaker extends Command { private Shooter shooter; private Feeder feeder; - private double topVelocity, topAcceleration, bottomVelocity, bottomAcceleration; - private Timer timer; + private double topVelocity, topAcceleration, bottomVelocity, bottomAcceleration; + private boolean reachedShooterSpeed = false; /** * Creates a new ShootSpeaker @@ -33,15 +33,13 @@ public ShootSpeaker(Shooter shooter, Feeder feeder) { } @Override - public void initialize() { - timer = new Timer(); - timer.start(); - } + public void initialize() {} @Override public void execute() { shooter.runShooter(topVelocity, topAcceleration, bottomVelocity, bottomAcceleration); - if (shooter.getAverageVelocity() >= 90){ + if (reachedShooterSpeed || shooter.getAverageTopVelocity() >= Constants.Shooter.THRESHOLD_SPEED*0.9) { + reachedShooterSpeed = true; shooter.runShooter(topVelocity, topAcceleration, bottomVelocity, bottomAcceleration); feeder.feedFeeder(Constants.Feeder.FEED_SPEED); } diff --git a/2024Crescendo/src/main/java/frc/robot/commands/VisionAlign.java b/2024Crescendo/src/main/java/frc/robot/commands/VisionAlign.java index c0d7af2..c5ff469 100644 --- a/2024Crescendo/src/main/java/frc/robot/commands/VisionAlign.java +++ b/2024Crescendo/src/main/java/frc/robot/commands/VisionAlign.java @@ -22,6 +22,7 @@ public VisionAlign(Swerve swerve, Vision vision) { translateController.setTolerance(0.0001); translateController.enableContinuousInput(0, 1); + addRequirements(swerve, vision); } public void initialize() { diff --git a/2024Crescendo/src/main/java/frc/robot/subsystems/Feeder.java b/2024Crescendo/src/main/java/frc/robot/subsystems/Feeder.java index 3cef108..d4103f1 100644 --- a/2024Crescendo/src/main/java/frc/robot/subsystems/Feeder.java +++ b/2024Crescendo/src/main/java/frc/robot/subsystems/Feeder.java @@ -21,7 +21,7 @@ public class Feeder extends SubsystemBase { public Feeder() { leftMotor = new CANSparkMax (Constants.Feeder.LEFT_ID, MotorType.kBrushless); rightMotor = new CANSparkMax(Constants.Feeder.RIGHT_ID, MotorType.kBrushless); - sensor = new DigitalInput(8); + sensor = new DigitalInput(Constants.Feeder.BEAM_ID); } /** diff --git a/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java b/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java index c321abc..fae5e9f 100644 --- a/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java +++ b/2024Crescendo/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,15 +3,14 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; -import frc.robot.Constants; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class Shooter extends SubsystemBase { private TalonFX topLeftMotor, topRightMotor, bottomLeftMotor, bottomRightMotor; @@ -24,43 +23,25 @@ public Shooter() { bottomRightMotor = new TalonFX(Constants.Shooter.BOTTOM_RIGHT_MOTOR_ID); TalonFXConfiguration config = new TalonFXConfiguration(); - - configMotor(topLeftMotor, config); - configMotor(topRightMotor, config); - configMotor(bottomLeftMotor, config); - configMotor(bottomRightMotor, config); - } - - /** - * Configure motor, add PID - * @param motor - motor to configure - * @param config - configuration to use - */ - private void configMotor(TalonFX motor, TalonFXConfiguration config) { config.Slot0.kP = Constants.Shooter.kP; config.Slot0.kI = Constants.Shooter.kI; config.Slot0.kD = Constants.Shooter.kD; config.Slot0.kV = Constants.Shooter.kV; config.Slot0.kA = Constants.Shooter.kA; - config.Slot0.kI = Constants.Shooter.kI; - - /*config.MotionMagic.MotionMagicCruiseVelocity = 3; //rps (4) - config.MotionMagic.MotionMagicAcceleration = 10; //rps/s - config.MotionMagic.MotionMagicJerk = 0;*/ - - motor.setNeutralMode(NeutralModeValue.Coast); - motor.getConfigurator().apply(config); - config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.StatorCurrentLimit = 60; + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + topLeftMotor.getConfigurator().apply(config); + topRightMotor.getConfigurator().apply(config); + bottomLeftMotor.getConfigurator().apply(config); + bottomRightMotor.getConfigurator().apply(config); } /** * Run all shooter motors at inputted velocities/accelerations - * @param topVelocity - * @param topAcceleration - * @param bottomVelocity - * @param bottomAcceleration + * @param topSpeed - percentage of full power (1.0) + * @param bottomSpeed - percentage of full power (1.0) */ public void runNoPID(double topSpeed, double bottomSpeed) { topLeftMotor.set(-topSpeed); @@ -72,10 +53,8 @@ public void runNoPID(double topSpeed, double bottomSpeed) { public void runShooter(double topVelocity, double topAcceleration, double bottomVelocity, double bottomAcceleration) { topLeftMotor.setControl(new VelocityVoltage(-topVelocity, -topAcceleration, false, 0.0, 0, false, false, false)); topRightMotor.setControl(new VelocityVoltage(topVelocity, topAcceleration, false, 0.0, 0, false, false, false)); - bottomLeftMotor - .setControl(new VelocityVoltage(-bottomVelocity, -bottomAcceleration, false, 0.0, 0, false, false, false)); - bottomRightMotor - .setControl(new VelocityVoltage(bottomVelocity, bottomAcceleration, false, 0.0, 0, false, false, false)); + bottomLeftMotor.setControl(new VelocityVoltage(-bottomVelocity, -bottomAcceleration, false, 0.0, 0, false, false, false)); + bottomRightMotor.setControl(new VelocityVoltage(bottomVelocity, bottomAcceleration, false, 0.0, 0, false, false, false)); } /*public void runTopPID(double topVelocity, double topAcceleration, double bottomSpeed) { @@ -89,15 +68,10 @@ public void runShooter(double topVelocity, double topAcceleration, double bottom * Stops all shooter motors */ public void stopMotors() { - /*topLeftMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false)); + topLeftMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false)); topRightMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false)); bottomLeftMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false)); - bottomRightMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false));*/ - - topLeftMotor.stopMotor(); - topRightMotor.stopMotor(); - bottomLeftMotor.stopMotor(); - bottomRightMotor.stopMotor(); + bottomRightMotor.setControl(new VelocityVoltage(0, 0, false, 0.0, 0, false, false, false)); } public void configDashboard(ShuffleboardTab tab) { @@ -109,8 +83,11 @@ public void configDashboard(ShuffleboardTab tab) { } public double getAverageVelocity(){ - double averageVelocity = (Math.abs(topLeftMotor.getVelocity().getValueAsDouble())+ Math.abs(topRightMotor.getVelocity().getValueAsDouble()) + Math.abs(bottomLeftMotor.getVelocity().getValueAsDouble()) + Math.abs(bottomRightMotor.getVelocity().getValueAsDouble()))/4.0; - return averageVelocity; + return (getMotorVelocity(topLeftMotor) + getMotorVelocity(topRightMotor) + getMotorVelocity(bottomLeftMotor) + getMotorVelocity(bottomRightMotor))/4.0; + } + + public double getAverageTopVelocity(){ + return (getMotorVelocity(topLeftMotor) + getMotorVelocity(topRightMotor))/2.0; } public double getMotorVelocity(TalonFX motor){ @@ -118,6 +95,5 @@ public double getMotorVelocity(TalonFX motor){ } @Override - public void periodic() { - } + public void periodic() {} } \ No newline at end of file