Skip to content

Commit

Permalink
Add pre-zero lockout and lockout between note lift and climber via Po…
Browse files Browse the repository at this point in the history 
…sitionTracker
  • Loading branch information
@superpenguin612
superpenguin612 committed Feb 29, 2024
1 parent 65dd817 commit 44c9667
Show file tree
Hide file tree
Showing 10 changed files with 110 additions and 40 deletions.
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;

Expand All @@ -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 {
Expand Down
25 changes: 13 additions & 12 deletions src/main/java/frc/robot/Controls.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down Expand Up @@ -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(() ->
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/GlobalStates.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

public enum GlobalStates {
// normal
INITIALIZED(false);
INITIALIZED(true);

// coast
// DRIVETRAIN_COAST(false),
Expand Down
42 changes: 42 additions & 0 deletions src/main/java/frc/robot/PositionTracker.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
package frc.robot;

import java.util.function.Supplier;

public class PositionTracker {
private Supplier<Double> intakePositionSupplier;
private Supplier<Double> shooterTiltPositionSupplier;
private Supplier<Double> climberPositionSupplier;
private Supplier<Double> noteLiftPositionSupplier;

public void setIntakePositionSupplier(Supplier<Double> intakePositionSupplier) {
this.intakePositionSupplier = intakePositionSupplier;
}

public void setShooterTiltPositionSupplier(Supplier<Double> shooterTiltPositionSupplier) {
this.shooterTiltPositionSupplier = shooterTiltPositionSupplier;
}

public void setClimberPositionSupplier(Supplier<Double> climberPositionSupplier) {
this.climberPositionSupplier = climberPositionSupplier;
}

public void setNoteLiftPositionSupplier(Supplier<Double> 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();
}
}
18 changes: 11 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -35,32 +34,31 @@
* 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" })
private final Drivetrain drivetrain = new Drivetrain();

@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" })
private final Shooter shooter = new Shooter();

@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" })
Expand Down Expand Up @@ -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());
Expand Down
19 changes: 12 additions & 7 deletions src/main/java/frc/robot/subsystems/Climber.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -55,7 +56,7 @@ public class Climber extends SubsystemBase implements BaseElevator<ClimberPositi
DRUM_RADIUS_METERS,
MIN_HEIGHT_METERS,
MAX_HEIGHT_METERS,
true,
false,
0);

@Log(groups = "control")
Expand All @@ -72,11 +73,11 @@ public class Climber extends SubsystemBase implements BaseElevator<ClimberPositi

private final SysIdRoutine sysIdRoutine;

private Supplier<Double> shooterTiltAngleSupplier;

private boolean initialized = false;

public Climber(Supplier<Double> shooterTiltPositionSupplier) {
private PositionTracker positionTracker;

public Climber(PositionTracker positionTracker) {
motor = SparkConfigurator.createSparkFlex(
MOTOR_ID, MotorType.kBrushless, true,
(s) -> s.setIdleMode(IdleMode.kBrake),
Expand All @@ -98,7 +99,7 @@ public Climber(Supplier<Double> shooterTiltPositionSupplier) {
},
this));

this.shooterTiltAngleSupplier = shooterTiltPositionSupplier;
this.positionTracker = positionTracker;

setDefaultCommand(moveToCurrentGoalCommand());
}
Expand Down Expand Up @@ -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);
}
Expand Down
7 changes: 6 additions & 1 deletion src/main/java/frc/robot/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -107,7 +108,9 @@ public class Intake extends SubsystemBase implements BaseSingleJointedArm<Intake

private boolean initialized = false;

public Intake() {
private PositionTracker positionTracker;

public Intake(PositionTracker positionTracker) {
leftArmMotor = SparkConfigurator.createSparkFlex(PRIMARY_ARM_MOTOR_ID, MotorType.kBrushless, true,
(s) -> s.setIdleMode(IdleMode.kBrake),
(s) -> s.setSmartCurrentLimit(ARM_CURRENT_LIMIT),
Expand Down Expand Up @@ -136,6 +139,8 @@ public Intake() {
},
this));

this.positionTracker = positionTracker;

pidController.setTolerance(TOLERANCE);
pidController.setGoal(IntakePosition.GROUND.value);

Expand Down
13 changes: 11 additions & 2 deletions src/main/java/frc/robot/subsystems/NoteLift.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -55,7 +56,7 @@ public class NoteLift extends SubsystemBase implements BaseElevator<NoteLiftPosi
DRUM_RADIUS_METERS,
MIN_HEIGHT_METERS,
MAX_HEIGHT_METERS,
true,
false,
MAX_HEIGHT_METERS);

@Log(groups = "control")
Expand All @@ -74,14 +75,18 @@ public class NoteLift extends SubsystemBase implements BaseElevator<NoteLiftPosi

private boolean initialized = false;

public NoteLift() {
private PositionTracker positionTracker;

public NoteLift(PositionTracker positionTracker) {
motor = SparkConfigurator.createSparkFlex(
MOTOR_ID, MotorType.kBrushless, true,
(s) -> s.setIdleMode(IdleMode.kBrake),
(s) -> s.setSmartCurrentLimit(CURRENT_LIMIT),
(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(
Expand Down Expand Up @@ -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;
}
Expand Down
8 changes: 6 additions & 2 deletions src/main/java/frc/robot/subsystems/ShooterTilt.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.*;
Expand Down Expand Up @@ -86,8 +87,9 @@ public class ShooterTilt extends SubsystemBase implements BaseSingleJointedArm<S
private boolean beyondMaxDistance = false;

private boolean initialized = false;
private PositionTracker positionTracker;

public ShooterTilt() {
public ShooterTilt(PositionTracker positionTracker) {
motor = SparkConfigurator.createSparkFlex(MOTOR_ID,
MotorType.kBrushless, false,
(s) -> s.setIdleMode(IdleMode.kBrake),
Expand All @@ -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(() -> {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/Vision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ public void periodic() {

@Override
public void simulationPeriodic() {
visionSim.update(simPoseSupplier.get());
// visionSim.update(simPoseSupplier.get());
}

public void updatePoseEstimator() {
Expand Down

0 comments on commit 44c9667

Please sign in to comment.