diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7849fa4..663d83b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -449,16 +449,16 @@ private ClimberPosition(double value) { public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); - public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.18, new Rotation3d(0, 0, 0)); + public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.19, new Rotation3d(0, 0, 0)); } public static final class NoteLift { public static enum NoteLiftPosition { BOTTOM(0), - INTAKE(0.03), - CLIMB_PREP(0.42), - STOW(0.49), - TOP(0.53); + INTAKE(0.16), + CLIMB_PREP(0.55), + STOW(0.62), + TOP(0.66); public final double value; @@ -477,7 +477,7 @@ private NoteLiftPosition(double value) { public static final double ENCODER_ROTATIONS_TO_METERS = WHEEL_CIRCUMFERENCE / GEARING; public static final double MIN_HEIGHT_METERS = 0; - public static final double MAX_HEIGHT_METERS = 0.56; + public static final double MAX_HEIGHT_METERS = 0.69; public static final int CURRENT_LIMIT = 40; @@ -496,7 +496,7 @@ private NoteLiftPosition(double value) { public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); - public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.41, new Rotation3d(0, 0, 0)); + public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.20, new Rotation3d(0, 0, 0)); } public static final class Vision { diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index c9fabc0..d83ccd4 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -85,14 +85,19 @@ public static void configureTestingControl(int port, Drivetrain drivetrain, Inta ShooterTilt shooterTilt, Climber climber, NoteLift noteLift) { CommandXboxController controller = new CommandXboxController(port); - // drivetrain.setDefaultCommand( - // drivetrain.teleopDriveCommand( - // () -> -controller.getLeftY() * speedMultiplier, - // () -> -controller.getLeftX() * speedMultiplier, - // () -> -controller.getRightX() * speedMultiplier)); - - // controller.a().whileTrue(RobotCommands.targetSpeakerOnTheMoveCommand(drivetrain, - // shooter, shooterTilt)); + controller.x().toggleOnTrue(intake.setOverridenSpeedCommand(() -> -controller.getLeftY() * 0.25) + .finallyDo(intake.resetControllersCommand()::schedule)); + controller.y().toggleOnTrue(shooterTilt.setOverridenSpeedCommand(() -> -controller.getRightY() * 0.75) + .finallyDo(shooterTilt.resetControllersCommand()::schedule)); + controller.a().toggleOnTrue(climber.setOverridenSpeedCommand(() -> -controller.getLeftY() * 0.75) + .finallyDo(climber.resetControllersCommand()::schedule)); + controller.b().toggleOnTrue(noteLift.setOverridenSpeedCommand(() -> -controller.getRightY() * 0.75) + .finallyDo(noteLift.resetControllersCommand()::schedule)); + + controller.x().and(controller.povDown()).whileTrue(intake.moveToPositionCommand(() -> IntakePosition.GROUND)); + controller.x().and(controller.povUp()).whileTrue(intake.moveToPositionCommand(() -> IntakePosition.STOW)); + controller.x().and(controller.povLeft()).whileTrue(intake.moveToPositionCommand(() -> IntakePosition.AMP)); + controller.x().and(controller.povRight()).whileTrue(intake.moveToPositionCommand(() -> IntakePosition.SOURCE)); controller.povDown().whileTrue(RobotCommands.resetClimb(intake, shooter, shooterTilt, climber, noteLift)); @@ -143,10 +148,6 @@ public static void configureTestingControl(int port, Drivetrain drivetrain, Inta // controller.b().whileTrue(RobotCommands.targetSpeakerCommand(drivetrain, // shooter, shooterTilt)); - controller.x().whileTrue(drivetrain.sysIdDriveQuasistatic(Direction.kForward)); - controller.y().whileTrue(drivetrain.sysIdDriveQuasistatic(Direction.kReverse)); - controller.a().whileTrue(drivetrain.sysIdDriveDynamic(Direction.kForward)); - controller.b().whileTrue(drivetrain.sysIdDriveDynamic(Direction.kReverse)); // controller.a().whileTrue(noteLift.moveToPositionCommand(() -> // NoteLiftPosition.BOTTOM)); // controller.x().whileTrue(noteLift.moveToPositionCommand(() -> diff --git a/src/main/java/frc/robot/GlobalStates.java b/src/main/java/frc/robot/GlobalStates.java index 2760750..43207c0 100644 --- a/src/main/java/frc/robot/GlobalStates.java +++ b/src/main/java/frc/robot/GlobalStates.java @@ -5,7 +5,7 @@ public enum GlobalStates { // normal - INITIALIZED(false); + INITIALIZED(true); // coast // DRIVETRAIN_COAST(false), diff --git a/src/main/java/frc/robot/PositionTracker.java b/src/main/java/frc/robot/PositionTracker.java new file mode 100644 index 0000000..b750046 --- /dev/null +++ b/src/main/java/frc/robot/PositionTracker.java @@ -0,0 +1,42 @@ +package frc.robot; + +import java.util.function.Supplier; + +public class PositionTracker { + private Supplier intakePositionSupplier; + private Supplier shooterTiltPositionSupplier; + private Supplier climberPositionSupplier; + private Supplier noteLiftPositionSupplier; + + public void setIntakePositionSupplier(Supplier intakePositionSupplier) { + this.intakePositionSupplier = intakePositionSupplier; + } + + public void setShooterTiltPositionSupplier(Supplier shooterTiltPositionSupplier) { + this.shooterTiltPositionSupplier = shooterTiltPositionSupplier; + } + + public void setClimberPositionSupplier(Supplier climberPositionSupplier) { + this.climberPositionSupplier = climberPositionSupplier; + } + + public void setNoteLiftPositionSupplier(Supplier noteLiftPositionSupplier) { + this.noteLiftPositionSupplier = noteLiftPositionSupplier; + } + + public double getIntakePosition() { + return intakePositionSupplier.get(); + } + + public double getShooterTiltPosition() { + return shooterTiltPositionSupplier.get(); + } + + public double getClimberPosition() { + return climberPositionSupplier.get(); + } + + public double getNoteLiftPosition() { + return noteLiftPositionSupplier.get(); + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c56382..b2642be 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -35,8 +34,7 @@ * The container for the robot. Contains subsystems, OI devices, and commands. */ public class RobotContainer { - @SendableLog(groups = "wpilib") - public static Mechanism2d mechanisms = new Mechanism2d(5, 2); + public final PositionTracker positionTracker = new PositionTracker(); @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) @@ -44,7 +42,7 @@ public class RobotContainer { @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) - private final Intake intake = new Intake(); + private final Intake intake = new Intake(positionTracker); @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) @@ -52,15 +50,15 @@ public class RobotContainer { @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) - private final ShooterTilt shooterTilt = new ShooterTilt(); + private final ShooterTilt shooterTilt = new ShooterTilt(positionTracker); @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) - private final Climber climber = new Climber(shooterTilt::getAngle); + private final Climber climber = new Climber(positionTracker); @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) - private final NoteLift noteLift = new NoteLift(); + private final NoteLift noteLift = new NoteLift(positionTracker); @Log(groups = "subsystems") @SendableLog(groups = { "wpilib", "subsystems" }) @@ -104,6 +102,12 @@ public class RobotContainer { public RobotContainer() { vision.setPoseEstimator(drivetrain.getPoseEstimator()); vision.setSimPoseSupplier(drivetrain::getSimPose); + + positionTracker.setIntakePositionSupplier(intake::getPosition); + positionTracker.setShooterTiltPositionSupplier(shooterTilt::getPosition); + positionTracker.setClimberPositionSupplier(climber::getPosition); + positionTracker.setNoteLiftPositionSupplier(noteLift::getPosition); + SparkConfigurator.safeBurnFlash(); DataLogManager.logNetworkTables(true); DriverStation.startDataLog(DataLogManager.getLog()); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index fd852dd..4662190 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import frc.robot.Constants.Climber.ClimberPosition; import frc.robot.GlobalStates; +import frc.robot.PositionTracker; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; @@ -55,7 +56,7 @@ public class Climber extends SubsystemBase implements BaseElevator shooterTiltAngleSupplier; - private boolean initialized = false; - public Climber(Supplier shooterTiltPositionSupplier) { + private PositionTracker positionTracker; + + public Climber(PositionTracker positionTracker) { motor = SparkConfigurator.createSparkFlex( MOTOR_ID, MotorType.kBrushless, true, (s) -> s.setIdleMode(IdleMode.kBrake), @@ -98,7 +99,7 @@ public Climber(Supplier shooterTiltPositionSupplier) { }, this)); - this.shooterTiltAngleSupplier = shooterTiltPositionSupplier; + this.positionTracker = positionTracker; setDefaultCommand(moveToCurrentGoalCommand()); } @@ -142,11 +143,15 @@ public void setVoltage(double voltage) { if (getPosition() < 0.02) { voltage = MathUtil.clamp(voltage, -3, 12); } - if (shooterTiltAngleSupplier.get() < 1.1) { + // if (positionTracker.getShooterTiltPosition() < 1.1) { + // voltage = 0; + // } + + if (positionTracker.getNoteLiftPosition() - getPosition() < 0.06 && voltage > 0) { voltage = 0; } if (!GlobalStates.INITIALIZED.enabled()) { - voltage = 0.0; + voltage = 0; } motor.setVoltage(voltage); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 80bb72d..6b092aa 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.GlobalStates; +import frc.robot.PositionTracker; import frc.robot.Constants.Intake.IntakePosition; import static frc.robot.Constants.Intake.*; import static edu.wpi.first.units.Units.Radians; @@ -107,7 +108,9 @@ public class Intake extends SubsystemBase implements BaseSingleJointedArm s.setIdleMode(IdleMode.kBrake), (s) -> s.setSmartCurrentLimit(ARM_CURRENT_LIMIT), @@ -136,6 +139,8 @@ public Intake() { }, this)); + this.positionTracker = positionTracker; + pidController.setTolerance(TOLERANCE); pidController.setGoal(IntakePosition.GROUND.value); diff --git a/src/main/java/frc/robot/subsystems/NoteLift.java b/src/main/java/frc/robot/subsystems/NoteLift.java index 6d18bea..648ab80 100644 --- a/src/main/java/frc/robot/subsystems/NoteLift.java +++ b/src/main/java/frc/robot/subsystems/NoteLift.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import frc.robot.Constants.NoteLift.NoteLiftPosition; import frc.robot.GlobalStates; +import frc.robot.PositionTracker; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; @@ -55,7 +56,7 @@ public class NoteLift extends SubsystemBase implements BaseElevator s.setIdleMode(IdleMode.kBrake), @@ -82,6 +85,8 @@ public NoteLift() { (s) -> s.getEncoder().setPositionConversionFactor(ENCODER_ROTATIONS_TO_METERS), (s) -> s.getEncoder().setVelocityConversionFactor(ENCODER_ROTATIONS_TO_METERS / 60.0)); + this.positionTracker = positionTracker; + pidController.setTolerance(TOLERANCE); sysIdRoutine = new SysIdRoutine( @@ -137,6 +142,10 @@ public void setVoltage(double voltage) { voltage = Utils.applySoftStops(voltage, getPosition(), MIN_HEIGHT_METERS, MAX_HEIGHT_METERS + 0.03); // allows note lift to unspool slightly + if (getPosition() - positionTracker.getClimberPosition() < 0.06 && voltage < 0) { + voltage = 0; + } + if (!GlobalStates.INITIALIZED.enabled()) { voltage = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/ShooterTilt.java b/src/main/java/frc/robot/subsystems/ShooterTilt.java index 36a921b..b5f194d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterTilt.java +++ b/src/main/java/frc/robot/subsystems/ShooterTilt.java @@ -37,6 +37,7 @@ import frc.robot.Constants.ShooterTilt.ShooterTiltPosition; import frc.robot.FieldConstants; import frc.robot.GlobalStates; +import frc.robot.PositionTracker; import static frc.robot.Constants.Shooter.MAX_SHOOTING_DISTANCE; import static frc.robot.Constants.ShooterTilt.*; @@ -86,8 +87,9 @@ public class ShooterTilt extends SubsystemBase implements BaseSingleJointedArm s.setIdleMode(IdleMode.kBrake), @@ -107,7 +109,9 @@ public ShooterTilt() { }, this)); - pidController.setTolerance(0.02); + this.positionTracker = positionTracker; + + pidController.setTolerance(TOLERANCE); pidController.setGoal(getLinearActuatorLength(ShooterTiltPosition.BOTTOM.value)); new Thread(() -> { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index c1152b6..bb9c092 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -63,7 +63,7 @@ public void periodic() { @Override public void simulationPeriodic() { - visionSim.update(simPoseSupplier.get()); + // visionSim.update(simPoseSupplier.get()); } public void updatePoseEstimator() {