import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.commands.gpm.BrownOutControl;
-import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.IntakeMovementCommand;
import frc.robot.commands.gpm.LockedShoot;
import frc.robot.commands.gpm.RunSpindexer;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.LED.LED;
import frc.robot.subsystems.drivetrain.Drivetrain;
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
- private LinearClimb linearClimb = null;
private LED led = null;
// TODO: move to correct robot and put the correct port?
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb);
+ driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
operator = new Operator(drive);
// Detected objects need access to the drivetrain
hood.forceHoodDown(false);
}));
}
-
- if (linearClimb != null && drive != null) {
- NamedCommands.registerCommand("Climb", new ClimbDriveCommand(linearClimb, drive));
- }
-
}
public void addAuto(String name) {
import frc.robot.subsystems.turret.Turret;
import frc.robot.subsystems.turret.TurretConstants;
import frc.robot.util.ShooterPhysics;
-import frc.robot.util.ShooterPhysics.Constraints;
import frc.robot.util.ShooterPhysics.TurretState;
public class AutoShootCommand extends Command {
private Hood hood;
private Shooter shooter;
private Spindexer spindexer;
-
- //TODO: find maximum interpolation
- private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
-
+
private double turretSetpoint;
private double hoodSetpoint;
+++ /dev/null
-package frc.robot.commands.gpm;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.commands.drive_comm.DriveToPose;
-import frc.robot.constants.FieldConstants;
-import frc.robot.subsystems.Climb.LinearClimb;
-import frc.robot.subsystems.drivetrain.Drivetrain;
-
-public class ClimbDriveCommand extends SequentialCommandGroup{
-
- public ClimbDriveCommand(LinearClimb climb, Drivetrain drive){
- addCommands(
- new ParallelCommandGroup(
- new InstantCommand(() -> climb.climbPosition()),
- new DriveToPose(drive, () -> FieldConstants.getClimbLocation())
- )
- );
- }
-
-
-}
package frc.robot.commands.gpm;
-import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.LinearFilter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
import frc.robot.constants.FieldConstants;
private Hood hood;
private Shooter shooter;
- private double turretSetpoint;
- private double hoodSetpoint;
-
private Pose2d drivepose;
- private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
- private Rotation2d lastTurretAngle;
private Rotation2d targetAngle;
- private double turretVelocity;
private double lastHoodAngle;
private double hoodAngle;
private PhaseManager phaseManager = new PhaseManager();
- private double hoodOffset = 0.0;
-
- private double turretOffset = 0.0;
-
private double distanceFromTarget = 0.0;
private double TOFAdjustment = 0.85;
// Pitch is in radians
hoodAngle = goalState.pitch();
- hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
lastHoodAngle = hoodAngle;
//spindexer.stopSpindexer();
}
- // shoot higher
- public void bumpUpHoodOffset() {
- hoodOffset += 1.0; // 1 degree
- }
-
- // shoot lower
- public void bumpDownHoodOffset() {
- hoodOffset -= 1.0; // 1 degree
- }
-
- // aim more left
- public void bumpUpTurretOffset() {
- turretOffset += 2.5; // 2.5 degree
- }
- // aim more right
- public void bumpDownTurretOffset() {
- turretOffset -= 2.5; // 2.5 degree
- }
-
@Override
public void execute() {
updateDrivePose();
hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
}
- double x = drivepose.getX(); // compared as meters
- double y = drivepose.getY();
-
// if (FieldConstants.underTrench(x, y)) {
// System.out.println("Hood forced down");
// } else {
hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
}
- double x = drivepose.getX(); // compared as meters
- double y = drivepose.getY();
-
// if (FieldConstants.underTrench(x, y)) {
// System.out.println("Hood forced down");
// } else {
public static FieldZone getZone(Translation2d drivepose) {
double x = drivepose.getX();
- double y = drivepose.getY();
if ((x < FIELD_LENGTH / 2 - Units.inchesToMeters(120.0)
&& x > (BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) // blue alliance line
import frc.robot.commands.gpm.Superstructure;
import frc.robot.constants.Constants;
import frc.robot.constants.swerve.DriveConstants;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.hood.Hood;
private Hood hood;
private Intake intake;
private Spindexer spindexer;
- private LinearClimb climb;
public PS5ControllerDriverConfig(
Drivetrain drive,
Turret turret,
Hood hood,
Intake intake,
- Spindexer spindexer,
- LinearClimb climb) {
+ Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.hood = hood;
this.intake = intake;
this.spindexer = spindexer;
- this.climb = climb;
}
public void configureControls() {
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Robot;
import frc.robot.commands.gpm.AutoShootCommand;
-import frc.robot.commands.gpm.ClimbDriveCommand;
import frc.robot.commands.gpm.ReverseMotors;
import frc.robot.constants.Constants;
-import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.hood.Hood;
import frc.robot.subsystems.Intake.Intake;
private Hood hood;
private Intake intake;
private Spindexer spindexer;
- private LinearClimb climb;
// PS5 button aliases
private final Button CROSS = Button.A;
Turret turret,
Hood hood,
Intake intake,
- Spindexer spindexer,
- LinearClimb climb) {
+ Spindexer spindexer) {
super(drive);
this.shooter = shooter;
this.turret = turret;
this.hood = hood;
this.intake = intake;
this.spindexer = spindexer;
- this.climb = climb;
}
public void configureControls() {
}));
}
- // Climb
- if (climb != null) {
- // Calibration
- controller.get(OPTIONS).onTrue(new InstantCommand(() -> {
- climb.hardstopCalibration();
- })).onFalse(new InstantCommand(() -> {
- climb.stopCalibrating();
- }));
-
- // Climb retract
- controller.get(CROSS).onTrue(new InstantCommand(() -> {
- climb.retract();
- }));
-
- // Drive to climb position and rumble
- controller.get(TRIANGLE).onTrue(new SequentialCommandGroup(
- new ClimbDriveCommand(climb, getDrivetrain()),
- new InstantCommand(() -> this.startRumble()),
- new WaitCommand(1),
- new InstantCommand(() -> this.endRumble())));
- }
-
// Hood
if (hood != null) {
controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> {
+++ /dev/null
-package frc.robot.subsystems.Climb;
-
-
-import edu.wpi.first.math.util.Units;
-
-public class ClimbConstants {
-
- // CHANGE LATER
- // gear ratio for converting motor rotations to linear distance
- public final static double CLIMB_GEAR_RATIO = 45.0;
- // TODO: Get actual winch bobbin radius measurement
- /** Winch spool radius in meters */
- public final static double WHEEL_RADIUS = Units.inchesToMeters(0.5);
- /** climber stowed ? position in meters */
- public final static double BOTTOM_POSITION = Units.inchesToMeters(8);
- /** Calibration position: lower than BOTTOM_POSITION to reduce motor strain */
- // public final static double CALIBRATION_POSITION = Units.inchesToMeters(8.5);
- /** position that should put the robot off the ground? in meters. 6 inches */
- public final static double CLIMB_POSITION = Units.inchesToMeters(6);
- public final static double UP_POSITION = 0.0;
-
- // current limits (in amps)
- // CALIBRATION: Low current while finding hardstop to prevent damage
- // NORMAL: Moderate current for PID-controlled movement
- // CLIMB: High current for full-power climbing
- public final static double CALIBRATION_CURRENT = 20.0;
- public final static double CLIMB_CURRENT = 42.0;
- public final static double CALIBRATION_CURRENT_THRESHOLD = 18.0;
-
- // PID Constants
- // TODO: what are the units? Inches? Meters?
- public final static double PID_P = 0.1;
- public final static double PID_I = 0.0;
- public final static double PID_D = 0.0;
- public final static double PID_TOLERANCE = 0.2;
-
- // Motor Limits
- public final static double CALIBRATION_POWER = 0.15;
-
- // Calibration
- public final static int CALIBRATION_COUNTER_LIMIT = 250;
-}
+++ /dev/null
-package frc.robot.subsystems.Climb;
-
-import org.littletonrobotics.junction.Logger;
-
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-
-/**
- * Climber subsystem
- */
-public class LinearClimb extends SubsystemBase implements LinearClimbIO {
- /** climber motor */
- private final TalonFX motor;
- /** whether the subsysgtem is calibrating */
- private boolean calibrating = false;
-
- /** should the subsystem perform calibration automatically */
- private boolean calibrateOnStartUp = false;
-
- private double MAX_POWER = 0.2;
-
- private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
-
- // logging information
- private LinearClimbIOInputsAutoLogged inputs = new LinearClimbIOInputsAutoLogged();
-
- /** This PID controller uses motor rotations */
- private final PIDController pid = new PIDController(
- ClimbConstants.PID_P,
- ClimbConstants.PID_I,
- ClimbConstants.PID_D);
-
- public LinearClimb() {
- motor = new TalonFX(IdConstants.CLIMB_MOTOR_ID, Constants.CANIVORE_SUB);
- pid.setTolerance(ClimbConstants.PID_TOLERANCE);
-
- motor.setNeutralMode(NeutralModeValue.Brake);
-
- TalonFXConfiguration config = new TalonFXConfiguration();
- config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
- motor.getConfigurator().apply(config);
-
- setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Calibrate", new InstantCommand(() -> hardstopCalibration()));
- SmartDashboard.putData("Go Up", new InstantCommand(() -> goUp())); // 0
- SmartDashboard.putData("Go Down", new InstantCommand(() -> retract())); // 8
- SmartDashboard.putData("Climb", new InstantCommand(() -> climbPosition())); // 6
- }
-
- // starting position
- motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION));
- setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
-
- // calibrate on startup to find hardstop
- if (calibrateOnStartUp) {
- hardstopCalibration();
- }
- }
-
- /**
- * set the setpoint for the pid
- *
- * @param setpoint in rotations
- */
- public void setSetpoint(double setpoint) {
- pid.setSetpoint(setpoint);
- }
-
- /**
- * @return when the position is within 0.2 rotations
- */
- public boolean atSetPoint() {
- return pid.atSetpoint();
- }
-
- /**
- * Returns the current position of the climb motor.
- *
- * @return Position in motor rotations. Positive values move the climb mechanism
- * UP (toward the hardstop). Higher values = higher physical position.
- * Use {@link #getAsMeters()} for linear distance in meters.
- */
- public double getPosition() {
- return motor.getPosition().getValueAsDouble();
- }
-
- /**
- * Returns the climb position converted to linear distance in meters.
- * This is useful for debugging and logging.
- *
- * @return Linear position in meters, calculated as:
- * rotations * gearRatio * 2 * PI * radius
- */
- public double getAsMeters() {
- return inputs.positionMeters;
- }
-
- /**
- * goes to the up position
- */
- public void goUp() { // 0
- MAX_POWER = 0.8;
- setSetpoint(metersToRotations(ClimbConstants.UP_POSITION));
- }
-
- /**
- * goes to the down position
- */
- public void retract() { // 8
- MAX_POWER = 0.2;
- setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
- }
-
- /**
- * goes to the climb position
- */
- public void climbPosition() { // 6
- MAX_POWER = 0.8;
- setSetpoint(metersToRotations(ClimbConstants.CLIMB_POSITION));
- }
-
- @Override
- public void periodic() {
- double power = pid.calculate(motor.getPosition().getValueAsDouble());
- // if it is not calibrating, do normal stuff
- if (!calibrating) {
- power = MathUtil.clamp(power, -MAX_POWER, MAX_POWER);
- } else {
- power = ClimbConstants.CALIBRATION_POWER;
- boolean atHardStop = Math
- .abs(motor.getStatorCurrent().getValueAsDouble()) >= ClimbConstants.CALIBRATION_CURRENT_THRESHOLD;
- if (calibrationDebouncer.calculate(atHardStop)) {
- stopCalibrating();
- }
- }
-
- // motor.set(power); // during calibration we have 20ms of high power before we stop calibration\
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Climb Power from PID", power);
- SmartDashboard.putNumber("Climb PID_Setpoint_Rotations", pid.getSetpoint());
- SmartDashboard.putNumber("Climb Motor_Actual_Rotations", motor.getPosition().getValueAsDouble());
- SmartDashboard.putNumber("Climb Motor_Actual_Meters", inputs.positionMeters);
- SmartDashboard.putBoolean("Climb Calibrated", !calibrating);
- SmartDashboard.putBoolean("Climb At Setpoint", atSetPoint());
- }
-
- if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("LinearClimb setpointMeters", Units.rotationsToRadians(pid.getSetpoint())
- * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO);
- }
- updateInputs();
- Logger.processInputs("LinearClimb", inputs);
- }
-
- /**
- * converts motor rotations to meters
- *
- * @param motorRotations
- * @return
- */
- public double rotationsToMeters(double motorRotations) {
- double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS;
- double meters = motorRotations / ClimbConstants.CLIMB_GEAR_RATIO * circ;
- return meters;
- }
-
- /**
- * converts meters to motor rotations
- *
- * @param meters
- * @return
- */
- public double metersToRotations(double meters) {
- double circ = 2 * Math.PI * ClimbConstants.WHEEL_RADIUS;
- double motorRotations = meters / circ * ClimbConstants.CLIMB_GEAR_RATIO;
- return motorRotations;
- }
-
- /**
- * sets supply and stator current limits
- *
- * @param limit the current limit for stator and supply current
- */
- public void setCurrentLimits(double limit) {
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- config.CurrentLimits = new CurrentLimitsConfigs();
-
- config.CurrentLimits.StatorCurrentLimitEnable = true;
- config.CurrentLimits.StatorCurrentLimit = limit;
- config.CurrentLimits.SupplyCurrentLimitEnable = true;
- config.CurrentLimits.SupplyCurrentLimit = limit;
-
- motor.getConfigurator().apply(config);
- }
-
- /**
- * Sets the motor to a slow speed until it hits the hard stop
- */
- public void hardstopCalibration() {
- calibrating = true;
- setCurrentLimits(ClimbConstants.CALIBRATION_CURRENT);
- }
-
- /**
- * stops calibration and sets current limits to normal.
- */
- public void stopCalibrating() {
- motor.setPosition(metersToRotations(ClimbConstants.BOTTOM_POSITION));
- calibrating = false;
- setCurrentLimits(ClimbConstants.CLIMB_CURRENT);
- setSetpoint(metersToRotations(ClimbConstants.BOTTOM_POSITION));
- }
-
- @Override
- public void updateInputs() {
- inputs.positionMeters = Units.rotationsToRadians(motor.getPosition().getValueAsDouble())
- * ClimbConstants.WHEEL_RADIUS / ClimbConstants.CLIMB_GEAR_RATIO;
- inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
- inputs.power = pid.calculate(motor.getPosition().getValueAsDouble());
- }
-}
+++ /dev/null
-package frc.robot.subsystems.Climb;
-
-import org.littletonrobotics.junction.AutoLog;
-
-public interface LinearClimbIO {
- @AutoLog
- public static class LinearClimbIOInputs{
- public double positionMeters = 0.0;
- public double motorCurrent = 0.0;
- public double power = 0.0;
- }
-
- public void updateInputs();
-}
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
private double setpointInches = 0.0;
private boolean calibrating = false;
- private Debouncer calibrationDebouncer = new Debouncer(0.5, DebounceType.kRising);
private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
leftMotor.set(-0.1);
rightMotor.set(-0.1);
boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD;
- // if(calibrationDebouncer.calculate(atHardStop)){
- // stopCalibrating();
- // }
}
updateInputs();
motor.getConfigurator().apply(limits);
}
- // Also ignore this for now
- private double wrapUnit(double value) {
- value %= 1.0;
- if (value < 0) value += 1.0;
- return value;
- }
-
private void calibrate(){
setCurrentLimits(TurretConstants.CALIBRATION_CURRENT_LIMIT);
calibrating = true;