diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59313e4..8c81ad0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,14 +42,14 @@ import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { - private PhotonVision vision; + //private PhotonVision vision; private Drivetrain drivetrain; private Hood hood; private Shooter shooter; private IntakePivot intakePivot; private IntakeRoller intakeRoller; private Spindexer spindexer; - private Climber climber; + //private Climber climber; private CommandXboxController driver; private CommandXboxController secondary; @@ -59,22 +59,23 @@ public class RobotContainer { private Timer shiftTimer; public RobotContainer() { - vision = new PhotonVision(); + //vision = new PhotonVision(); drivetrain = new Drivetrain(null); hood = new Hood(); shooter = new Shooter(); intakePivot = new IntakePivot(); intakeRoller = new IntakeRoller(); spindexer = new Spindexer(); - climber = new Climber(); + //climber = new Climber(); + /* vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( "Vision/" + vp.cameraName() + "/Pose", vp.visualPose() ); - }); + });*/ driver = new CommandXboxController(OIConstants.kDriverControllerPort); secondary = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -82,7 +83,8 @@ public class RobotContainer { shiftTimer = new Timer(); shiftTimer.reset(); - configureBindings(); + //configureBindings(); + testConfigureBindings(); configureShiftDisplay(); if(AutoConstants.kAutoConfigOk) { @@ -106,7 +108,8 @@ public class RobotContainer { // This should just work, if it doesn't it's likely modules aren't assigned the right IDs // after the electronics rebuild. For testing normal operation nothing about the Drivetrain // class should need to change - drivetrain.setDefaultCommand( + drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true)); + /*drivetrain.setDefaultCommand( drivetrain.drive( driver::getLeftY, driver::getLeftX, @@ -138,7 +141,7 @@ public class RobotContainer { driver::getLeftY, driver::getLeftX ) - ); + );*/ // Stop everything by default other than the drivetrain shooter.setDefaultCommand(shooter.stop()); @@ -146,7 +149,7 @@ public class RobotContainer { intakeRoller.setDefaultCommand(intakeRoller.stop()); hood.setDefaultCommand(hood.stop()); spindexer.setDefaultCommand(spindexer.stop()); - climber.setDefaultCommand(climber.stop()); + //climber.setDefaultCommand(climber.stop()); // While holding POV up of the driver controller, the climber // should move such that its motor moves the climber down with the left @@ -154,9 +157,9 @@ public class RobotContainer { // trigger axis. // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted // from the constants file for the subsystem having the problem. - driver.povUp().whileTrue(climber.manualSpeed(() -> { - return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis(); - })); + //driver.povUp().whileTrue(climber.manualSpeed(() -> { + // return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis(); + //})); // While holding the right bumper of the driver controller, the intake rollers // and the spindexer and feeder should move such that all motors are moving in such a way @@ -165,7 +168,8 @@ public class RobotContainer { // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the // constants file for the subsystem having the problem driver.rightBumper().whileTrue( - intakeRoller.runIn().alongWith(spindexer.spinToShooter()) + //intakeRoller.runIn().alongWith(spindexer.spinToShooter()) + spindexer.spinToShooter() ); // While holding the left bumper of the driver controller, the intake rollers @@ -244,6 +248,7 @@ public class RobotContainer { ) ); + /* driver.b().whileTrue( drivetrain.lockToYaw( () -> { @@ -254,7 +259,7 @@ public class RobotContainer { driver::getLeftY, driver::getLeftX ) - ); + );*/ shooter.setDefaultCommand( shooter.maintainSpeed(ShooterSpeeds.kHubSpeed) diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 501ce8c..76ff74e 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -20,15 +20,15 @@ public class DrivetrainConstants { public static final double kRearLeftMagEncoderOffset = 3.761; public static final double kRearRightMagEncoderOffset = 2.573; - public static final int kFrontLeftDrivingCANID = 0; + public static final int kFrontLeftDrivingCANID = 4; public static final int kFrontRightDrivingCANID = 3; public static final int kRearLeftDrivingCANID = 1; public static final int kRearRightDrivingCANID = 2; - public static final int kFrontLeftTurningCANID = 8; - public static final int kFrontRightTurningCANID = 9; - public static final int kRearLeftTurningCANID = 7; - public static final int kRearRightTurningCANID = 6; + public static final int kFrontLeftTurningCANID = 7; // 8 + public static final int kFrontRightTurningCANID = 21; //9 + public static final int kRearLeftTurningCANID = 6; //7 + public static final int kRearRightTurningCANID = 8; //6 public static final int kFrontLeftAnalogInPort = 3; public static final int kFrontRightAnalogInPort = 2; diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index 6bdeff0..f5c7ec6 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj.Filesystem; public class HoodConstants { // TODO Real Values - public static final int kMotorCANID = 0; + public static final int kMotorCANID = 12; public static final double kConversionFactor = 3.0*147.0/8.0; @@ -26,7 +26,7 @@ public class HoodConstants { public static final double kV = 0; public static final double kA = 0; public static final double kStartupAngle = 0; - public static final double kMaxManualSpeedMultiplier = .5; + public static final double kMaxManualSpeedMultiplier = 1; public static final double kAmpsToTriggerPositionReset = 10; diff --git a/src/main/java/frc/robot/constants/IntakePivotConstants.java b/src/main/java/frc/robot/constants/IntakePivotConstants.java index c8f2b0b..e7fefca 100644 --- a/src/main/java/frc/robot/constants/IntakePivotConstants.java +++ b/src/main/java/frc/robot/constants/IntakePivotConstants.java @@ -20,8 +20,8 @@ public class IntakePivotConstants { } } - public static final int kLeftMotorCANID = 0; - public static final int kRightMotorCANID = 0; + public static final int kLeftMotorCANID = 16; + public static final int kRightMotorCANID = 9; public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0; @@ -40,7 +40,7 @@ public class IntakePivotConstants { public static final int kCurrentLimit = 30; - public static final IdleMode kIdleMode = IdleMode.kBrake; + public static final IdleMode kIdleMode = IdleMode.kCoast; // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM diff --git a/src/main/java/frc/robot/constants/IntakeRollerConstants.java b/src/main/java/frc/robot/constants/IntakeRollerConstants.java index a7b76a6..f088f2e 100644 --- a/src/main/java/frc/robot/constants/IntakeRollerConstants.java +++ b/src/main/java/frc/robot/constants/IntakeRollerConstants.java @@ -5,30 +5,24 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; public class IntakeRollerConstants { // TODO Real values - public static final int kLeftMotorCANID = 0; - public static final int kRightMotorCANID = 0; + public static final int kMotorCANID = 20; public static final int kCurrentLimit = 40; - public static final boolean kInvertMotors = false; + public static final boolean kInvertMotors = true; + + public static final double kSpeed = .6; public static final IdleMode kIdleMode = IdleMode.kCoast; // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig(); - public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig(); static { leftMotorConfig .idleMode(kIdleMode) .smartCurrentLimit(kCurrentLimit) .inverted(kInvertMotors); - - rightMotorConfig - .idleMode(kIdleMode) - .smartCurrentLimit(kCurrentLimit) - .inverted(kInvertMotors) - .follow(kLeftMotorCANID); } } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 84d7642..c4ea1e4 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.util.Units; public class ShooterConstants { public enum ShooterSpeeds { kHubSpeed(0), - kFeedSpeed(0); + kFeedSpeed(0.35); private double speedMPS; @@ -27,30 +27,28 @@ public class ShooterConstants { public static final double kWheelDiameter = Units.inchesToMeters(4); // TODO Real values - public static final int kLeftShooterMotor1CANID = 0; - public static final int kLeftShooterMotor2CANID = 0; - public static final int kRightShooterMotor1CANID = 0; - public static final int kRightShooterMotor2CANID = 0; + public static final int kLeftShooterMotorCANID = 2; + public static final int kRightShooterMotorCANID = 5; - public static final boolean kLeftShooterMotor1Inverted = false; - public static final boolean kLeftShooterMotor2Inverted = false; - public static final boolean kRightShooterMotor1Inverted = false; - public static final boolean kRightShooterMotor2Inverted = false; + public static final boolean kLeftShooterMotorInverted = true; + public static final boolean kRightShooterMotorInverted = false; - public static final double kLeftP = 0; + public static final double kLeftP = 0.1; public static final double kLeftI = 0; public static final double kLeftD = 0; public static final double kLeftS = 0; public static final double kLeftV = 0.21; public static final double kLeftA = 0; - public static final double kRightP = 0; + public static final double kRightP = 0.1; public static final double kRightI = 0; public static final double kRightD = 0; public static final double kRightS = 0; public static final double kRightV = 0.21; public static final double kRightA = 0; + public static final double kMaxManualSpeedMultiplier = 1; + public static final double kShooterHeightMeters = 0; // TODO Is this value sane? @@ -60,51 +58,37 @@ public class ShooterConstants { // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM - public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig(); - public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig(); - public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig(); - public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig(); + public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig(); + public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig(); static { - kLeftMotor1Config + kLeftMotorConfig .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kLeftShooterMotor1Inverted); - kLeftMotor1Config.absoluteEncoder + .inverted(kLeftShooterMotorInverted); + kLeftMotorConfig.absoluteEncoder .positionConversionFactor(kWheelDiameter * Math.PI) .velocityConversionFactor(kWheelDiameter * Math.PI / 60); - kLeftMotor1Config.closedLoop + kLeftMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .pid(kLeftP, kLeftI, kLeftD) .outputRange(-1, 1) .feedForward .sva(kLeftS, kLeftV, kLeftA); - kLeftMotor2Config + kRightMotorConfig .idleMode(kShooterIdleMode) .smartCurrentLimit(kCurrentLimit) - .inverted(kLeftShooterMotor2Inverted) - .follow(kLeftShooterMotor1CANID); - - kRightMotor1Config - .idleMode(kShooterIdleMode) - .smartCurrentLimit(kCurrentLimit) - .inverted(kRightShooterMotor1Inverted); - kRightMotor1Config.absoluteEncoder + .inverted(kRightShooterMotorInverted); + kRightMotorConfig.absoluteEncoder .positionConversionFactor(kWheelDiameter * Math.PI) .velocityConversionFactor(kWheelDiameter * Math.PI / 60); - kRightMotor1Config.closedLoop + kRightMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .pid(kRightP, kRightI, kRightD) .outputRange(-1, 1) .feedForward .sva(kRightS, kRightV, kRightA); - - kRightMotor2Config - .idleMode(kShooterIdleMode) - .smartCurrentLimit(kCurrentLimit) - .inverted(kRightShooterMotor2Inverted) - .follow(kRightShooterMotor1CANID); } } diff --git a/src/main/java/frc/robot/constants/SpindexerConstants.java b/src/main/java/frc/robot/constants/SpindexerConstants.java index d64c6d6..86c5041 100644 --- a/src/main/java/frc/robot/constants/SpindexerConstants.java +++ b/src/main/java/frc/robot/constants/SpindexerConstants.java @@ -10,16 +10,19 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; public class SpindexerConstants { // TODO Real values public static final int kSpindexerMotorCANID = 0; - public static final int kFeederMotorCANID = 0; + public static final int kFeederMotorCANID = 4; - public static final int kSpindexerStatorCurrentLimit = 90; - public static final int kSpindexerSupplyCurrentLimit = 50; + public static final int kSpindexerStatorCurrentLimit = 110; + public static final int kSpindexerSupplyCurrentLimit = 60; public static final int kFeederCurrentLimit = 40; - public static final boolean kFeederMotorInverted = false; + public static final double kSpindexerSpeed = 1; + public static final double kFeederSpeed = 1; + + public static final boolean kFeederMotorInverted = true; public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive; - public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake; + public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast; public static final IdleMode kFeederIdleMode = IdleMode.kBrake; diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index f45b30a..b47e507 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -4,7 +4,6 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; diff --git a/src/main/java/frc/robot/subsystems/IntakeRoller.java b/src/main/java/frc/robot/subsystems/IntakeRoller.java index 287f5b0..e31f84e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRoller.java +++ b/src/main/java/frc/robot/subsystems/IntakeRoller.java @@ -10,41 +10,33 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IntakeRollerConstants; public class IntakeRoller extends SubsystemBase { - private SparkMax leftMotor; - private SparkMax rightMotor; + private SparkMax motor; public IntakeRoller() { - leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless); - rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless); + motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless); - leftMotor.configure( + motor.configure( IntakeRollerConstants.leftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - - rightMotor.configure( - IntakeRollerConstants.rightMotorConfig, - ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters - ); } public Command runIn() { return run(() -> { - leftMotor.set(1); + motor.set(IntakeRollerConstants.kSpeed); }); } public Command runOut() { return run(() -> { - leftMotor.set(-1); + motor.set(-IntakeRollerConstants.kSpeed); }); } public Command stop() { return run(() -> { - leftMotor.set(0); + motor.set(0); }); } diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 974bfcb..9e316a7 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -75,19 +75,19 @@ public class PhotonVision extends SubsystemBase { if(!results.isEmpty()) { latestResults.set(i, results.get(results.size() - 1)); - } - Optional pose = estimators[i].update(latestResults.get(i)); + Optional pose = estimators[i].update(latestResults.get(i)); - if(!pose.isEmpty()) { - VisualPose visualPose = new VisualPose( - cameras[i].getName(), - pose.get().estimatedPose.toPose2d(), - pose.get().timestampSeconds - ); + if(!pose.isEmpty()) { + VisualPose visualPose = new VisualPose( + cameras[i].getName(), + pose.get().estimatedPose.toPose2d(), + pose.get().timestampSeconds + ); - for(Consumer consumer: poseEstimateConsumers) { - consumer.accept(visualPose); + for(Consumer consumer: poseEstimateConsumers) { + consumer.accept(visualPose); + } } } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8b685f3..6902f5c 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -19,10 +19,8 @@ import frc.robot.constants.ShooterConstants; import frc.robot.constants.ShooterConstants.ShooterSpeeds; public class Shooter extends SubsystemBase { - private SparkMax leftMotor1; - private SparkMax leftMotor2; - private SparkMax rightMotor1; - private SparkMax rightMotor2; + private SparkMax leftMotor; + private SparkMax rightMotor; private AbsoluteEncoder leftEncoder; private AbsoluteEncoder rightEncoder; @@ -33,40 +31,26 @@ public class Shooter extends SubsystemBase { private ShooterSpeeds targetSpeeds; public Shooter() { - leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless); - leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless); - rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless); - rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless); + leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless); + rightMotor = new SparkMax(ShooterConstants.kRightShooterMotorCANID, MotorType.kBrushless); - leftMotor1.configure( - ShooterConstants.kLeftMotor1Config, + leftMotor.configure( + ShooterConstants.kLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - rightMotor1.configure( - ShooterConstants.kRightMotor1Config, + rightMotor.configure( + ShooterConstants.kRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters ); - leftMotor2.configure( - ShooterConstants.kLeftMotor2Config, - ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters - ); + leftEncoder = leftMotor.getAbsoluteEncoder(); + rightEncoder = rightMotor.getAbsoluteEncoder(); - rightMotor2.configure( - ShooterConstants.kRightMotor2Config, - ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters - ); - - leftEncoder = leftMotor1.getAbsoluteEncoder(); - rightEncoder = rightMotor1.getAbsoluteEncoder(); - - leftClosedLoopController = leftMotor1.getClosedLoopController(); - rightClosedLoopController = rightMotor1.getClosedLoopController(); + leftClosedLoopController = leftMotor.getClosedLoopController(); + rightClosedLoopController = rightMotor.getClosedLoopController(); // TODO Set this to the initial startup speed targetSpeeds = null; @@ -92,8 +76,8 @@ public class Shooter extends SubsystemBase { return run(() -> { if(targetSpeeds == null) { - leftMotor1.disable(); - rightMotor1.disable(); + leftMotor.disable(); + rightMotor.disable(); } else { leftClosedLoopController.setSetpoint( targetSpeeds.getSpeedMPS(), @@ -101,7 +85,7 @@ public class Shooter extends SubsystemBase { ); rightClosedLoopController.setSetpoint( - targetSpeeds.getSpeedMPS(), + -targetSpeeds.getSpeedMPS(), ControlType.kVelocity ); } @@ -112,8 +96,8 @@ public class Shooter extends SubsystemBase { targetSpeeds = null; return run(() -> { - leftMotor1.set(speed.getAsDouble()); - rightMotor1.set(speed.getAsDouble()); + leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); + rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); }); } diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index 34180bc..b57c0e3 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -37,15 +37,19 @@ public class Spindexer extends SubsystemBase { public Command spinToShooter() { return run(() -> { - spindexerMotor.setControl(spindexerMotorOutput.withOutput(1)); - feederMotor.set(1); + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed) + ); + feederMotor.set(SpindexerConstants.kFeederSpeed); }); } public Command spinToIntake() { return run(() -> { - spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1)); - feederMotor.set(-1); + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed) + ); + feederMotor.set(-SpindexerConstants.kFeederSpeed); }); }