diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index cdd61e73..81877770 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -1,17 +1,17 @@ { "location": { - "front": -12, - "left": 9 + "front": -10.875, + "left": 10 }, - "absoluteEncoderOffset": 211.0, + "absoluteEncoderOffset": 56.5, "drive": { "type": "sparkmax", - "id": 5, + "id": 8, "canbus": null }, "angle": { "type": "sparkmax", - "id": 6, + "id": 4, "canbus": null }, "conversionFactor": { @@ -20,7 +20,7 @@ }, "encoder": { "type": "thrifty", - "id": 2, + "id": 1, "canbus": null }, "inverted": { diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 9d808f46..69cc0987 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -1,17 +1,17 @@ { "location": { - "front": -12, - "left": -9 + "front": -10.875, + "left": -10 }, - "absoluteEncoderOffset": 31.9, + "absoluteEncoderOffset": 209.9, "drive": { "type": "sparkmax", - "id": 7, + "id": 5, "canbus": null }, "angle": { "type": "sparkmax", - "id": 8, + "id": 6, "canbus": null }, "conversionFactor": { diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index eb6d146c..cfb2d2ed 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -1,17 +1,17 @@ { "location": { - "front": 12, - "left": 9 + "front": 10.875, + "left": 10 }, - "absoluteEncoderOffset": 140.6, + "absoluteEncoderOffset": 260.5, "drive": { "type": "sparkmax", - "id": 3, + "id": 9, "canbus": null }, "angle": { "type": "sparkmax", - "id": 4, + "id": 10, "canbus": null }, "conversionFactor": { diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index 700ce186..35384fa5 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -1,17 +1,17 @@ { "location": { - "front": 12, - "left": -9 + "front": 10.875, + "left": -10 }, - "absoluteEncoderOffset": 5.5, + "absoluteEncoderOffset": 1.5, "drive": { "type": "sparkmax", - "id": 1, + "id": 3, "canbus": null }, "angle": { "type": "sparkmax", - "id": 2, + "id": 7, "canbus": null }, "conversionFactor": { @@ -20,7 +20,7 @@ }, "encoder": { "type": "thrifty", - "id": 1, + "id": 2, "canbus": null }, "inverted": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cf018088..2932b242 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -34,25 +34,25 @@ public static class Mailbox { /** Constants for the Pneumatics system. */ public static class MailboxPneumatics { /** The channel on the PCM for the forward direction on the left solenoid. */ - public static final int LEFT_SOLENOID_FORWARD_CHANNEL = 0; + public static final int LEFT_SOLENOID_FORWARD_CHANNEL = 5; /** The channel on the PCM for the reverse direction on the left solenoid. */ - public static final int LEFT_SOLENOID_REVERSE_CHANNEL = 1; + public static final int LEFT_SOLENOID_REVERSE_CHANNEL = 4; /** The channel on the PCM for the forward direction on the right solenoid. */ - public static final int RIGHT_SOLENOID_FORWARD_CHANNEL = 2; + public static final int RIGHT_SOLENOID_FORWARD_CHANNEL = 7; /** The channel on the PCM for the reverse direction on the right solenoid. */ - public static final int RIGHT_SOLENOID_REVERSE_CHANNEL = 3; + public static final int RIGHT_SOLENOID_REVERSE_CHANNEL = 6; } /** Constants for the Belts system. */ public static class MailboxBelts { /** The CAN ID for the upper belt motor */ - public static final int UPPER_BELT_MOTOR_ID = 0; + public static final int UPPER_BELT_MOTOR_ID = 11; /** The CAN ID for the lower belt motor */ - public static final int LOWER_BELT_MOTOR_ID = 0; + public static final int LOWER_BELT_MOTOR_ID = 12; /** The speed for the belt motors. */ public static final double BELT_MOTOR_SPEED = 1; @@ -62,6 +62,9 @@ public static class MailboxBelts { /** Inversion state of the belts follower motor. */ public static final boolean BELTS_FOLLOWER_INVERSE_STATE = false; + + /** Current limit (in amps) for the belt motor(s) */ + public static final int BELT_MOTOR_CURRENT_LIMIT = 40; } /** Constants that are relating to the controllers. */ @@ -70,7 +73,7 @@ public static class Controllers { public static final int PILOT_CONTROLLER_PORT = 0; /** Driver station port number for the operator controller */ - public static final int OPERATOR_CONTROLLER_PORT = 1; + public static final int OPERATOR_CONTROLLER_PORT = 2; /** Axis deadband for driver controller. */ public static double DRIVER_CONTROLLER_DEADBAND = 0.1; @@ -118,23 +121,26 @@ public static class Auto { /** Constants for the Intake System */ public static class Intake { /** Motor id of the Lower Intake motor. */ - public static final int LOWER_INTAKE_MOTOR_ID = 0; + public static final int LOWER_INTAKE_MOTOR_ID = 1; /** Motor id of the Upper Intake motor. */ - public static final int UPPER_INTAKE_MOTOR_ID = 0; + public static final int UPPER_INTAKE_MOTOR_ID = 2; /** Inversion state of the upper intake motor. */ public static final boolean UPPER_INTAKE_MOTOR_INVERTED = false; // It was me, DIO! /** DIO Port ID for the Intake limit switch. */ - public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 0; + public static final int INTAKE_LIMIT_SWITCH_DIO_PORT = 1; /** Speed we want to run the Intake at. */ public static final double INTAKE_MOTOR_SPEED = 1; /** Inversion state for the intake follower motor. */ public static final boolean INTAKE_FOLLOWER_INVERSE_STATE = false; + + /** Current limit (in amps) for the intake motor(s) */ + public static final int INTAKE_MOTOR_CURRENT_LIMIT = 40; } /** Holds contstants for the Limelights. */ @@ -145,39 +151,39 @@ public static class AimingLimelight { public static final String LIMELIGHT_NAME = "limelight"; /** The number of degrees the Limelight is mounted back from perfectly vertical */ - public static final double MOUNT_ANGLE = 0; + public static final double MOUNT_ANGLE = 50; /** The number of inches from the center of the Limelight lens to the floor */ - public static final double MOUNT_HEIGHT = 0; + public static final double MOUNT_HEIGHT = 10.0625; /** The height to the Amp Apriltag off the ground. */ - public static final double AMP_APRILTAG_HEIGHT = 0; + public static final double AMP_APRILTAG_HEIGHT = 53.13; /** How far in inches we want to be from the target when we shoot */ - public static final double DISTANCE_FROM_TARGET = 0; + public static final double DISTANCE_FROM_TARGET = 14; /** * How hard to turn toward the target. Double between 0 and 1, standard way to drive a motor */ - public static final double STEER_STRENGTH = 0; + public static final double STEER_STRENGTH = 0.01; /** How hard to drive toward the target. Same notation as above. */ - public static final double DRIVE_STRENGTH = 0; + public static final double DRIVE_STRENGTH = 0.01; /** VERY BASIC speed limit to make sure we don't drive too fast towards the target. */ - public static final double SPEED_LIMIT = 0; + public static final double SPEED_LIMIT = 0.2; /** * When we're at or below this number of degrees from where we want to be, we'll consider the * Limelight's aim routine "done" */ - public static final double TURN_DONE_THRESHOLD = 0; + public static final double TURN_DONE_THRESHOLD = 1; /** * When we're at or below this number of inches from the target distance, we'll consider the * Limelight's drive routine "done" */ - public static final double DISTANCE_DONE_THRESHOLD = 0; + public static final double DISTANCE_DONE_THRESHOLD = 4; /** Holds pipeline numbers for this Limelight. */ public static class PipelineNumbers { diff --git a/src/main/java/frc/robot/Keybinds.java b/src/main/java/frc/robot/Keybinds.java index c95c6a9a..3e7e7760 100644 --- a/src/main/java/frc/robot/Keybinds.java +++ b/src/main/java/frc/robot/Keybinds.java @@ -29,7 +29,7 @@ public static void configureDefaultKeybinds( operatorController.y().whileTrue(RobotContainer.climbUp); operatorController.a().whileTrue(RobotContainer.climbDown); operatorController.povUp().whileTrue(RobotContainer.runIntake); - /* TODO: bind intake reverse to povDown when issue 92 is closed. */ + operatorController.povDown().whileTrue(RobotContainer.runIntakeReverse); operatorController.leftTrigger().whileTrue(RobotContainer.aimToAmp); operatorController.rightTrigger().whileTrue(RobotContainer.fireNote); @@ -49,7 +49,7 @@ public static void configureOperatorlessKeybinds( CommandXboxController pilotController, CommandXboxController operatorController) { pilotController.leftStick().toggleOnTrue(RobotContainer.driveFieldOriented); /* TODO: angle / velocity steering toggle w/ right stick (no issue) and boost on right bumper (issue 86) */ - /* TODO: bind intake reverse to povDown when issue 92 is closed. */ + pilotController.povDown().whileTrue(RobotContainer.runIntakeReverse); pilotController.povUp().toggleOnTrue(RobotContainer.enterXMode); pilotController.rightTrigger().whileTrue(RobotContainer.runIntake); pilotController.a().whileTrue(RobotContainer.aimToAmp); @@ -70,7 +70,7 @@ public static void configureOriginalKeybinds( pilotController.x().onTrue(RobotContainer.enterXMode); pilotController.y().whileTrue(RobotContainer.fireNote); pilotController.a().whileTrue(RobotContainer.runIntake); - /* TODO: bind intake reverse to povDown when issue 92 is closed. */ + pilotController.povDown().whileTrue(RobotContainer.runIntakeReverse); pilotController.b().whileTrue(RobotContainer.aimToAmp); pilotController.leftTrigger().whileTrue(RobotContainer.climbDown); pilotController.rightTrigger().whileTrue(RobotContainer.climbUp); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 998cba57..bf88fc56 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -178,7 +178,7 @@ public class RobotContainer { new FireNoteRoutineNoLimitSwitch(); /** Singleton instance of {@link RunIntakeReverse} for the whole robot. */ - private RunIntakeReverse runIntakeReverse = new RunIntakeReverse(); + public static RunIntakeReverse runIntakeReverse = new RunIntakeReverse(); /** Singleton instance of {@link RunIntake} for the whole robot. */ public static RunIntake runIntake = new RunIntake(); @@ -195,7 +195,8 @@ public class RobotContainer { Constants.Limelight.AimingLimelight.SPEED_LIMIT, Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD, Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD, - Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT); + Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT, + Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER); /** Singleton instance of {@link AimAndFireRoutine} for the whole robot. */ public static AimAndFireRoutine aimAndFire = new AimAndFireRoutine(); @@ -253,7 +254,8 @@ private void configureAuto() { } private void configureBindings() { - Keybinds.configureOperatorlessKeybinds(pilotController, operatorController); + // TODO: CHANGE THIS TO configureDefaultKeybinds() FOR COMP + Keybinds.configureDefaultKeybinds(pilotController, operatorController); } /** @@ -266,9 +268,12 @@ public Command getAutonomousCommand() { } private static double scaleAxis(double axis) { + if (axis == 0) { + return 0; + } double deadbanded = MathUtil.applyDeadband(axis, Constants.Controllers.DRIVER_CONTROLLER_DEADBAND); - return Math.pow(deadbanded, 3); + return -Math.pow(deadbanded, 2) * (Math.abs(axis) / axis); } /** diff --git a/src/main/java/frc/robot/commands/AimAndFireRoutine.java b/src/main/java/frc/robot/commands/AimAndFireRoutine.java index 08d7bdf1..2b333f32 100644 --- a/src/main/java/frc/robot/commands/AimAndFireRoutine.java +++ b/src/main/java/frc/robot/commands/AimAndFireRoutine.java @@ -36,7 +36,8 @@ public AimAndFireRoutine() { Constants.Limelight.AimingLimelight.SPEED_LIMIT, Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD, Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD, - Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT), + Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT, + Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER), new EnterXMode(), new ParallelDeadlineGroup( new WaitCommand(Constants.Auto.FIRE_NOTE_FOR_TIME), new FireNoteRoutine())); diff --git a/src/main/java/frc/robot/commands/AimWithLimelight.java b/src/main/java/frc/robot/commands/AimWithLimelight.java index 2a8ba820..781f6de8 100644 --- a/src/main/java/frc/robot/commands/AimWithLimelight.java +++ b/src/main/java/frc/robot/commands/AimWithLimelight.java @@ -33,7 +33,9 @@ public class AimWithLimelight extends Command { speedLimit, turnDoneThreshold, distanceDoneThreshold, - targetHeight; + targetHeight, + pipelineNumber, + oldPipelineNumber; /** * Creates a new command to aim with the Limelight. @@ -61,7 +63,8 @@ public AimWithLimelight( double speedLimit, double turnDoneThreshold, double distanceDoneThreshold, - double targetHeight) { + double targetHeight, + double pipelineNumber) { this.steerStrength = steerStrength; this.distanceFromTarget = distanceFromTarget; this.mountHeight = mountHeight; @@ -71,6 +74,7 @@ public AimWithLimelight( this.turnDoneThreshold = turnDoneThreshold; this.distanceDoneThreshold = distanceDoneThreshold; this.targetHeight = targetHeight; + this.pipelineNumber = pipelineNumber; this.drive = RobotContainer.drive; this.limelight = limelight; // Use addRequirements() here to declare subsystem dependencies. @@ -80,6 +84,8 @@ public AimWithLimelight( // Called when the command is initially scheduled. @Override public void initialize() { + this.oldPipelineNumber = limelight.getLimelightPipeline(); + limelight.setLimelightPipeline(pipelineNumber); this.finished = false; this.counter = 0; } @@ -115,7 +121,9 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + limelight.setLimelightPipeline(oldPipelineNumber); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java index 35a384fe..dd27ac9e 100644 --- a/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveFieldOriented.java @@ -40,7 +40,9 @@ public DriveFieldOriented( // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + drive.setHeadingCorrection(false); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java index e9b5c02e..df0b2441 100644 --- a/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java +++ b/src/main/java/frc/robot/commands/drive/DriveRobotOriented.java @@ -40,7 +40,9 @@ public DriveRobotOriented( // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + drive.setHeadingCorrection(false); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0baf0b18..4e4b022b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -11,6 +11,7 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.DigitalInput; @@ -32,6 +33,12 @@ public Intake() { new CANSparkMax(Constants.Intake.UPPER_INTAKE_MOTOR_ID, MotorType.kBrushless); this.limitSwitch = new DigitalInput(Constants.Intake.INTAKE_LIMIT_SWITCH_DIO_PORT); + intakeLower.setSmartCurrentLimit(Constants.Intake.INTAKE_MOTOR_CURRENT_LIMIT); + intakeUpper.setSmartCurrentLimit(Constants.Intake.INTAKE_MOTOR_CURRENT_LIMIT); + + intakeLower.setIdleMode(IdleMode.kBrake); + intakeUpper.setIdleMode(IdleMode.kBrake); + intakeLower.follow(intakeUpper, Constants.Intake.INTAKE_FOLLOWER_INVERSE_STATE); intakeUpper.setInverted(Constants.Intake.UPPER_INTAKE_MOTOR_INVERTED); } @@ -52,8 +59,8 @@ public void runIntakeReverse() { * @return the status of the limit switch */ public boolean getLimitSwitch() { - /* Assumes the limit switch is wired to be normally open. */ - return limitSwitch.get(); + /* Assumes the limit switch is wired to be normally closed. */ + return !limitSwitch.get(); } /** Stops the intake motors. */ diff --git a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java index b455643d..36e608b5 100644 --- a/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java +++ b/src/main/java/frc/robot/subsystems/mailbox/MailboxBelts.java @@ -24,9 +24,12 @@ public class MailboxBelts extends SubsystemBase { /** Constructer for MailboxBelts subsystem */ public MailboxBelts() { upperBeltMotor = - new CANSparkMax(Constants.MailboxBelts.UPPER_BELT_MOTOR_ID, MotorType.kBrushless); + new CANSparkMax(Constants.MailboxBelts.UPPER_BELT_MOTOR_ID, MotorType.kBrushed); lowerBeltMotor = - new CANSparkMax(Constants.MailboxBelts.LOWER_BELT_MOTOR_ID, MotorType.kBrushless); + new CANSparkMax(Constants.MailboxBelts.LOWER_BELT_MOTOR_ID, MotorType.kBrushed); + + upperBeltMotor.setSmartCurrentLimit(Constants.MailboxBelts.BELT_MOTOR_CURRENT_LIMIT); + lowerBeltMotor.setSmartCurrentLimit(Constants.MailboxBelts.BELT_MOTOR_CURRENT_LIMIT); lowerBeltMotor.follow(upperBeltMotor, Constants.MailboxBelts.BELTS_FOLLOWER_INVERSE_STATE); upperBeltMotor.setInverted(Constants.MailboxBelts.UPPER_BELT_MOTOR_INVERTED); diff --git a/src/test/BaseTest.java b/src/test/BaseTest.java index 1c470c31..99dec433 100644 --- a/src/test/BaseTest.java +++ b/src/test/BaseTest.java @@ -15,7 +15,7 @@ public class BaseTests { /* HEY! If your test is failing, don't comment this out! Your code won't work on the robot. So fix it! /lh /nm */ @Test - public void TestAutoTasks() { + public void testAutoTasks() { new RobotContainer(); } } \ No newline at end of file