diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8336718e..7e93a84d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -115,6 +115,8 @@ public class RobotContainer { BooleanSupplier ManualShootSup; BooleanSupplier ForceVisionSup; BooleanSupplier GroundIntakeSup; + BooleanSupplier FarStageAngleSup; + BooleanSupplier OperatorPreRevSup; // Replace with CommandPS4Controller or CommandJoystick if needed @@ -156,6 +158,8 @@ public RobotContainer() { ManualShootSup = driverController::getL1Button; ForceVisionSup = driverController::getOptionsButton; GroundIntakeSup = operatorController::getTouchpad; + FarStageAngleSup = driverController::getTouchpad; + OperatorPreRevSup = operatorController::getL2Button; // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); limelightInit(); @@ -205,7 +209,7 @@ private void shooterInst() { } private void angleShooterInst(){ angleShooterSubsystem = new AngleShooterSubsystem(); - defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup); + defaultShooterAngleCommand = new AimShooter(angleShooterSubsystem, driverController::getPOV, HumanPlaySup, SubwooferAngleSup, StageAngleSup, GroundIntakeSup, FarStageAngleSup); angleShooterSubsystem.setDefaultCommand(defaultShooterAngleCommand); } private void intakeInst() { @@ -218,7 +222,7 @@ private void indexInit() { indexer = new IndexerSubsystem(); } private void indexCommandInst() { - defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup); + defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorPreRevSup); indexer.setDefaultCommand(defaulNoteHandlingCommand); } @@ -257,8 +261,10 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL), () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button, - driverController::getCrossButton, - driverController::getTriangleButton)); + StageAngleSup, + FarStageAngleSup, + SubwooferAngleSup + )); if(Preferences.getBoolean("Detector Limelight", false)) { new Trigger(operatorController::getR1Button).onTrue(new SequentialCommandGroup( diff --git a/src/main/java/frc/robot/commands/AimRobotMoving.java b/src/main/java/frc/robot/commands/AimRobotMoving.java index ffbbcd5a..2b96e8d1 100644 --- a/src/main/java/frc/robot/commands/AimRobotMoving.java +++ b/src/main/java/frc/robot/commands/AimRobotMoving.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.DriveConstants; +import frc.robot.settings.Constants.Field; import frc.robot.settings.Constants.ShooterConstants; import frc.robot.subsystems.DrivetrainSubsystem; import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD; @@ -27,13 +28,14 @@ public class AimRobotMoving extends Command { DoubleSupplier translationXSupplier; DoubleSupplier translationYSupplier; BooleanSupplier run; - BooleanSupplier cancel1; - BooleanSupplier cancel2; + BooleanSupplier PodiumAngleSup; + BooleanSupplier FarStageAngleSup; DoubleSupplier rotationSupplier; double rotationSpeed; double allianceOffset; + BooleanSupplier SubwooferAngleSup; - public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier cancel1, BooleanSupplier cancel2){ + public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSupplier, DoubleSupplier translationXSupplier, DoubleSupplier translationYSupplier, BooleanSupplier run, BooleanSupplier PodiumAngleSup, BooleanSupplier FarStageAngleSup, BooleanSupplier SubwooferAngleSup){ m_drivetrain = drivetrain; speedController = new PIDController( AUTO_AIM_ROBOT_kP, @@ -43,9 +45,10 @@ public AimRobotMoving(DrivetrainSubsystem drivetrain, DoubleSupplier rotationSup speedController.enableContinuousInput(-180, 180); this.translationXSupplier = translationXSupplier; this.translationYSupplier = translationYSupplier; + this.SubwooferAngleSup = SubwooferAngleSup; this.rotationSupplier = rotationSupplier; - this.cancel1 = cancel1; - this.cancel2 = cancel2; + this.FarStageAngleSup = FarStageAngleSup; + this.PodiumAngleSup = PodiumAngleSup; this.run = run; addRequirements(drivetrain); } @@ -61,26 +64,41 @@ public void initialize() { @Override public void execute() { desiredRobotAngle = m_drivetrain.calculateSpeakerAngleMoving(); - speedController.setSetpoint(desiredRobotAngle); - //move robot to desired angle - this.currentHeading = m_drivetrain.getPose().getRotation().getDegrees(); - if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) { - rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; - } else { - rotationSpeed = speedController.calculate(currentHeading); - } - if(DriverStation.getAlliance().get() == Alliance.Red) { - allianceOffset = Math.PI; - } else { - allianceOffset = 0; - } - if(!cancel1.getAsBoolean()) { - m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds( - translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, - translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, - rotationSpeed, - new Rotation2d(m_drivetrain.getPose().getRotation().getRadians()+allianceOffset))); - } + double podiumRobotAngle; + double farStageRobotAngle; + if(DriverStation.getAlliance().get() == Alliance.Red) { + podiumRobotAngle = Field.RED_PODIUM_ROBOT_ANGLE; + farStageRobotAngle = Field.RED_FAR_STAGE_ROBOT_ANGLE; + } else { + podiumRobotAngle = Field.BLUE_PODIUM_ROBOT_ANGLE; + farStageRobotAngle = Field.BLUE_FAR_STAGE_ROBOT_ANGLE; + } + if(PodiumAngleSup.getAsBoolean()) { + speedController.setSetpoint(podiumRobotAngle); + } else if(FarStageAngleSup.getAsBoolean()) { + speedController.setSetpoint(farStageRobotAngle); + } else { + speedController.setSetpoint(desiredRobotAngle); + } + //move robot to desired angle + this.currentHeading = m_drivetrain.getPose().getRotation().getDegrees(); + if(Math.abs(rotationSupplier.getAsDouble()) > 0.3) { + rotationSpeed = rotationSupplier.getAsDouble() * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; + } else { + rotationSpeed = speedController.calculate(currentHeading); + } + if(DriverStation.getAlliance().get() == Alliance.Red) { + allianceOffset = Math.PI; + } else { + allianceOffset = 0; + } + if(!SubwooferAngleSup.getAsBoolean()){ + m_drivetrain.drive(ChassisSpeeds.fromFieldRelativeSpeeds( + translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, + translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, + rotationSpeed, + new Rotation2d(m_drivetrain.getPose().getRotation().getRadians()+allianceOffset))); + } // m_drivetrain.drive(new ChassisSpeeds( // translationXSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, // translationYSupplier.getAsDouble() * DriveConstants.MAX_VELOCITY_METERS_PER_SECOND, @@ -102,6 +120,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return (!run.getAsBoolean() || cancel1.getAsBoolean() || cancel2.getAsBoolean()); + return (!run.getAsBoolean()); } } diff --git a/src/main/java/frc/robot/commands/AimShooter.java b/src/main/java/frc/robot/commands/AimShooter.java index 5c5d1bbd..7b7afad9 100644 --- a/src/main/java/frc/robot/commands/AimShooter.java +++ b/src/main/java/frc/robot/commands/AimShooter.java @@ -15,16 +15,18 @@ public class AimShooter extends Command { BooleanSupplier humanPlayerSupplier; BooleanSupplier SubwooferSupplier1; BooleanSupplier StageAngleSupplier; - BooleanSupplier intakeSupplier; + BooleanSupplier groundIntakeSup; + BooleanSupplier farStageAngleSup; public AimShooter(AngleShooterSubsystem angleShooterSubsystem, DoubleSupplier POVSupplier, BooleanSupplier humanPlayerSupplier, - BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier intakeSup) { + BooleanSupplier SubwooferSupplier1, BooleanSupplier StageAngleSupplier, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup) { this.angleShooterSubsystem = angleShooterSubsystem; this.POVSupplier = POVSupplier; this.humanPlayerSupplier = humanPlayerSupplier; this.SubwooferSupplier1 = SubwooferSupplier1; this.StageAngleSupplier = StageAngleSupplier; - this.intakeSupplier = intakeSup; + this.groundIntakeSup = groundIntakeSup; + this.farStageAngleSup = farStageAngleSup; addRequirements(angleShooterSubsystem); } @@ -38,12 +40,14 @@ public void execute() { if(SubwooferSupplier1.getAsBoolean()) { angleShooterSubsystem.setDesiredShooterAngle(Field.SUBWOOFER_ANGLE); } else if (StageAngleSupplier.getAsBoolean()) { - angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_ANGLE); + angleShooterSubsystem.setDesiredShooterAngle(Field.PODIUM_SHOOTER_ANGLE); + } else if (farStageAngleSup.getAsBoolean()) { + angleShooterSubsystem.setDesiredShooterAngle(Field.FAR_STAGE_SHOOTER_ANGLE); } else if (POVSupplier.getAsDouble() == 90 || POVSupplier.getAsDouble() == 45 || POVSupplier.getAsDouble() == 135) { - angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_ANGLE); + angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE); } else if(humanPlayerSupplier.getAsBoolean()) { angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE); - } else if(intakeSupplier.getAsBoolean()) { + } else if (groundIntakeSup.getAsBoolean()){ angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE); } else { angleShooterSubsystem.setDesiredShooterAngle(angleShooterSubsystem.calculateSpeakerAngle()); diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index ba9fd35d..34a17848 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -42,6 +42,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return intake.isNoteIn(); } } diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index 80b88440..2ae52f42 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -28,7 +28,7 @@ public class IndexCommand extends Command { BooleanSupplier revUpSupplier; Boolean revUp; BooleanSupplier shootIfReadySupplier; - DoubleSupplier ampSupplier;; + DoubleSupplier ampSupplier; BooleanSupplier groundIntakeSup; Boolean shootIfReady; // DoubleSupplier POVSupplier; @@ -40,11 +40,13 @@ public class IndexCommand extends Command { AngleShooterSubsystem angleShooterSubsytem; BooleanSupplier stageAngleSup; BooleanSupplier subwooferAngleSup; + BooleanSupplier farStageAngleSup; + BooleanSupplier operatorRevSup; boolean auto; double runsEmpty = 0; /** Creates a new IndexCommand. */ - public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, BooleanSupplier stageAngleSup, BooleanSupplier SubwooferSup, BooleanSupplier groundIntakeSup) { + public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, BooleanSupplier stageAngleSup, BooleanSupplier SubwooferSup, BooleanSupplier groundIntakeSup, BooleanSupplier farStageAngleSup, BooleanSupplier operatorRevSup) { this.m_Indexer = m_IndexerSubsystem; this.shootIfReadySupplier = shootIfReadySupplier;//R2 this.revUpSupplier = revUpSupplier;//L2 @@ -55,10 +57,12 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf this.humanPlayerSupplier = humanPlaySupplier;//R1 this.subwooferAngleSup = SubwooferSup; this.stageAngleSup = stageAngleSup; + this.farStageAngleSup = farStageAngleSup; this.groundIntakeSup = groundIntakeSup; + this.operatorRevSup = operatorRevSup; SmartDashboard.putNumber("amp RPS", AMP_RPS); SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED); - SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE); + SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_SHOOTER_ANGLE); // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_IndexerSubsystem, shooter, intake); } @@ -83,7 +87,7 @@ public void execute() { if(runsEmpty>20) { if(humanPlayerSupplier.getAsBoolean()) { m_Indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED); - shooter.shootRPS(ShooterConstants.HUMAN_PLAYER_RPS); + shooter.shootSameRPS(ShooterConstants.HUMAN_PLAYER_RPS); intake.intakeOff(); } else { @@ -99,38 +103,41 @@ public void execute() { } else { runsEmpty = 0; intake.intakeOff(); - if(shootIfReadySupplier.getAsBoolean()&&revUpSupplier.getAsBoolean()&&humanPlayerSupplier.getAsBoolean()) { - shooter.shootRPS(ShooterConstants.SUBWOOFER_RPS); - } else { - if(revUpSupplier.getAsBoolean()||stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()) { - if(angleShooterSubsytem.shortSpeakerDist()) { - shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS); - } else { - shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS); - } + if(revUpSupplier.getAsBoolean()||stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()) { + if(angleShooterSubsytem.shortSpeakerDist()) { + shooter.shootRPS(ShooterConstants.SHORT_SHOOTING_RPS); } else { - shooter.turnOff(); + shooter.shootRPS(ShooterConstants.LONG_SHOOTING_RPS); } - boolean indexer = false; - if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot()) { - RobotState.getInstance().ShooterReady = true; - if (shootIfReadySupplier.getAsBoolean()) { - indexer = true; - } + } else { + if (operatorRevSup.getAsBoolean()){ + shooter.shootRPSWithCurrent(100, 10, 20); } else { - RobotState.getInstance().ShooterReady = false; + shooter.turnOff(); } - if(SmartDashboard.getBoolean("feedMotor", false)) { + } + boolean indexer = false; + if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot() && shooter.isReving()) { + RobotState.getInstance().ShooterReady = true; + if (shootIfReadySupplier.getAsBoolean()) { indexer = true; } - if (indexer) { - m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); - } else { - m_Indexer.off(); - } + } else { + RobotState.getInstance().ShooterReady = false; + } + if(SmartDashboard.getBoolean("feedMotor", false)) { + indexer = true; + } + if((stageAngleSup.getAsBoolean()||subwooferAngleSup.getAsBoolean()||farStageAngleSup.getAsBoolean())&&revUpSupplier.getAsBoolean()&& shooter.validShot()) { + indexer = true; + } + if (indexer) { + m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED); + } else { + m_Indexer.off(); + } } } - } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 1a4085bd..ca503c9b 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -85,7 +85,7 @@ public double getValue() { * Whether the drive motor should be counterclockwise or clockwise positive. * If there is an odd number of gear reductions this is typically clockwise-positive. */ - public static final InvertedValue DRIVETRAIN_DRIVE_INVERTED = InvertedValue.CounterClockwise_Positive; + public static final InvertedValue DRIVETRAIN_DRIVE_INVERTED = InvertedValue.Clockwise_Positive; /** * The overall steer reduction of the module. Multiplying motor rotations by @@ -263,7 +263,7 @@ public static final class ShooterConstants{ public static final double COMP_MAXIMUM_SHOOTER_ANGLE = 108; public static final double PRAC_MAXIMUM_SHOOTER_ANGLE = 101; public static final double HUMAN_PLAYER_ANGLE = 97; - public static final double HUMAN_PLAYER_RPS = -10; + public static final double HUMAN_PLAYER_RPS = -15; public static final double SAFE_SHOOTER_ANGLE = 15; public static final double GROUND_INTAKE_SHOOTER_ANGLE = 90; @@ -299,7 +299,7 @@ public static final class ShooterConstants{ public static final class ClimberConstants{ public static final int CLIMBER_MOTOR_RIGHT = 23; public static final int CLIMBER_MOTOR_LEFT = 22; - public static final double CLIMBER_SPEED_DOWN = 0.8; + public static final double CLIMBER_SPEED_DOWN = 1; public static final double CLIMBER_SPEED_UP = -1; public static final double MAX_MOTOR_ROTATIONS = 235; @@ -324,7 +324,7 @@ public static final class IntakeConstants{ public static final int BRUSH_1_MOTOR = 2491; public static final int BRUSH_2_MOTOR = 2491; public static final int BRUSH_3_MOTOR = 2491; - public static final double INTAKE_SPEED = 0.8; + public static final double INTAKE_SPEED = 1; } public static final class CTREConfigs { public TalonFXConfiguration driveMotorConfig; @@ -367,6 +367,7 @@ public CTREConfigs() { driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; driveMotorConfig.CurrentLimits.SupplyCurrentLimit = DriveConstants.DRIVE_CURRENT_LIMIT; driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = false; // Steer encoder. steerEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; @@ -403,14 +404,21 @@ public final class PS4Operator{ public final class Field{ public static final double BLUE_SPEAKER_X = 0.23; public static final double RED_SPEAKER_X = 16.87; - public static final double SPEAKER_Y = 5.3;//16.412; + public static final double SPEAKER_Y = 5.6; public static final double SPEAKER_Z = 2.08; //height of opening public static final double MAX_SHOOTING_DISTANCE = 9; public static final double SHORT_RANGE_SHOOTING_DIST = 3; - public static final double AMPLIFIER_ANGLE = 108; + public static final double AMPLIFIER_SHOOTER_ANGLE = 108; public static final double SUBWOOFER_ANGLE = 60; - public static final double PODIUM_ANGLE = 37.5; + public static final double PODIUM_SHOOTER_ANGLE = 36.3; + public static final double FAR_STAGE_SHOOTER_ANGLE = 24.5; + + public static final double BLUE_PODIUM_ROBOT_ANGLE = 149; + public static final double RED_PODIUM_ROBOT_ANGLE = 31; + public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 184; + public static final double RED_FAR_STAGE_ROBOT_ANGLE = -4; + //angle at 60 for bounce techinque, didn't work } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 75a7d6ca..b36c44b3 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -54,27 +54,6 @@ public Climber() { } public void climberGo(double speed){ runSpeed = speed; - // if (speed>0) { - // if(!hallEffectL.isPressed()) { - // climbMotorL.set(speed); - // } - // if(!hallEffectR.isPressed()) { - // climbMotorR.set(speed); - // } - // } else { - // if (currentEncoderRotationsL < ClimberConstants.MAX_MOTOR_ROTATIONS){ - // climbMotorL.set(speed); - // } else { - // climbMotorL.stopMotor(); - // } - // if (currentEncoderRotationsR < ClimberConstants.MAX_MOTOR_ROTATIONS){ - // climbMotorR.set(speed); - // } else { - // climbMotorR.stopMotor(); - // } - // } - // climbMotorL.set(speed); - // climbMotorR.set(speed); } public void climberStop(){ diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 849967bc..68bfc1b7 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -113,7 +113,6 @@ public DrivetrainSubsystem() { Preferences.initDouble("BR offset", 0); PathPlannerLogging.setLogActivePathCallback((poses) -> m_field.getObject("path").setPoses(poses)); SmartDashboard.putData("Field", m_field); - SmartDashboard.putData("resetOdometry", new InstantCommand(() -> this.resetOdometry())); SmartDashboard.putBoolean("force use limelight", false); modules = new SwerveModule[4]; lastAngles = new Rotation2d[] {new Rotation2d(), new Rotation2d(), new Rotation2d(), new Rotation2d()}; // manually make empty angles to avoid null errors. @@ -202,9 +201,6 @@ public Pose2d getPose() { } public void resetOdometry(Pose2d pose) { odometer.resetPosition(getGyroscopeRotation(), getModulePositions(), pose); - } - public void resetOdometry() { - resetOdometry(DRIVE_ODOMETRY_ORIGIN); } /** * Sets the modules speed and rotation to zero. diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ad09c669..7351a00a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -19,9 +19,6 @@ public class IntakeSubsystem extends SubsystemBase { CANSparkMax intake1; CANSparkMax intake2; SparkAnalogSensor m_DistanceSensor; - CANSparkMax brush1; - CANSparkMax brush2; - CANSparkMax brush3; double intakeRunSpeed; public IntakeSubsystem() { @@ -34,20 +31,6 @@ public IntakeSubsystem() { } else { m_DistanceSensor = intake2.getAnalog(Mode.kAbsolute); } - if(Preferences.getBoolean("Brushes", false)) { - brush1 = new CANSparkMax(IntakeConstants.BRUSH_1_MOTOR, MotorType.kBrushless); - brush2 = new CANSparkMax(IntakeConstants.BRUSH_2_MOTOR, MotorType.kBrushless); - brush3 = new CANSparkMax(IntakeConstants.BRUSH_3_MOTOR, MotorType.kBrushless); - brush1.setSmartCurrentLimit(20, 20, 60); - brush2.setSmartCurrentLimit(20, 20, 60); - brush3.setSmartCurrentLimit(20, 20, 60); - brush1.burnFlash(); - brush2.burnFlash(); - brush3.burnFlash(); - brush1.setIdleMode(IdleMode.kCoast); - brush2.setIdleMode(IdleMode.kCoast); - brush3.setIdleMode(IdleMode.kCoast); - } intake2.follow(intake1); intake2.setInverted(false); intake1.setInverted(true); @@ -61,28 +44,12 @@ public IntakeSubsystem() { public void intakeYes(double intakeRunSpeed) { intake1.set(intakeRunSpeed); - if(Preferences.getBoolean("Brushes", false)) { - brush1.set(intakeRunSpeed); - brush2.set(intakeRunSpeed); - brush3.set(intakeRunSpeed); - } - } public void intakeNo(double intakeRunSpeed) { intake1.set(-intakeRunSpeed); - if(Preferences.getBoolean("Brushes", false)) { - brush1.set(-intakeRunSpeed); - brush2.set(-intakeRunSpeed); - brush3.set(-intakeRunSpeed); - } } public void intakeOff() { intake1.set(0); - if(Preferences.getBoolean("Brushes", false)) { - brush2.set(0); - brush1.set(0); - brush3.set(0); - } } public boolean isNoteIn() { return m_DistanceSensor.getVoltage()<2; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 1c773fc7..83f0f513 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -73,31 +73,43 @@ public ShooterSubsystem(double runSpeed) { configuratorR.apply(new FeedbackConfigs().withSensorToMechanismRatio(0.5).withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor).withRotorToSensorRatio(1)); currentLimitConfigs = new CurrentLimitsConfigs(); - currentLimitConfigs.SupplyCurrentLimit = ShooterConstants.CURRENT_LIMIT; currentLimitConfigs.SupplyCurrentLimitEnable = true; + currentLimitConfigs.StatorCurrentLimitEnable = true; + + configuratorR.apply(PIDRightconfigs); + configuratorL.apply(PIDLeftconfigs); + } + private void adjCurrentLimit( double supplyLimit, double statorLimit){ + currentLimitConfigs.SupplyCurrentLimit = supplyLimit; + currentLimitConfigs.StatorCurrentLimit = statorLimit; configuratorL.apply(currentLimitConfigs); configuratorR.apply(currentLimitConfigs); - - configuratorR.apply(PIDRightconfigs); - configuratorL.apply(PIDLeftconfigs); - } - - - public void shootThing(double runSpeed) { - shooterR.set(runSpeed); - shooterL.set(runSpeed); - } + } + + //public void shootThing(double runSpeed) { + // currentLimitConfigs.SupplyCurrentLimit = ShooterConstants.CURRENT_LIMIT; + // shooterR.set(runSpeed); + // shooterL.set(runSpeed); + //} public void shootRPS(double RPS) { - shooterR.setControl(new VelocityDutyCycle(RPS).withSlot(0)); - shooterL.setControl(new VelocityDutyCycle(RPS/2).withSlot(0)); + shootRPSWithCurrent(RPS, ShooterConstants.CURRENT_LIMIT, 100); } public void shootSameRPS(double RPS) { + adjCurrentLimit(ShooterConstants.CURRENT_LIMIT, 100); shooterR.setControl(new VelocityDutyCycle(RPS).withSlot(0)); shooterL.setControl(new VelocityDutyCycle(RPS).withSlot(0)); } + public void shootRPSWithCurrent(double RPS, double currentLimit, double statorLimit){ + adjCurrentLimit(currentLimit, statorLimit); + shooterR.setControl(new VelocityDutyCycle(RPS).withSlot(0)); + shooterL.setControl(new VelocityDutyCycle(RPS/2).withSlot(0)); + } private double getError() { return Math.abs(shooterR.getClosedLoopError().getValueAsDouble()); } + public boolean isReving() { + return shooterR.getVelocity().getValueAsDouble()>15; + } public boolean validShot() { return runsValid >= Constants.LOOPS_VALID_FOR_SHOT; } @@ -111,6 +123,7 @@ public void turnOff(){ @Override public void periodic() { SmartDashboard.putNumber("TESTING shooter speed error", getError()); + SmartDashboard.getBoolean("shooter speed rev'ed", validShot()); SmartDashboard.putNumber("shooter current right", shooterR.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); if(getError()