diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index fb8649a1..3e4aef38 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -310,4 +310,11 @@ public void disabledInit() { subsystems.shooterSubsystem.stopFlywheel(); } } + + @Override + public void disabledPeriodic() { + if (subsystems.drivebaseSubsystem != null) { + subsystems.drivebaseSubsystem.setToX(); + } + } } diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 1319695f..fd7c395c 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -12,7 +12,7 @@ public static class SubsystemConstants { public static final boolean POST_CLIMB_ENABLED = false; public static final boolean DRIVE_ENABLED = true; - public static final boolean DRIVER_VIS_ENABLED = true; + public static final boolean DRIVER_VIS_ENABLED = false; public static final boolean SHOOTER_VISION_ENABLED = true; public static final boolean INDEX_ENABLED = true; public static final boolean INTAKE_ENABLED = true; diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java index 3bf920dd..4aa76a1f 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java @@ -16,6 +16,7 @@ import org.frcteam2910.common.math.Vector2; import frc.team2412.robot.commands.shooter.ShooterTargetCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; import frc.team2412.robot.subsystem.IndexSubsystem; import frc.team2412.robot.subsystem.IntakeSubsystem; @@ -29,8 +30,8 @@ public class JackFiveBallAutoCommand extends SequentialCommandGroup implements U public static class FiveBallConstants { public static final TrajectoryConstraint[] NORMAL_SPEED = { new FeedforwardConstraint(11.0, - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old // value // was // 11.0 @@ -41,8 +42,8 @@ public static class FiveBallConstants { public static final TrajectoryConstraint[] FAST_SPEED = { new FeedforwardConstraint(11.0, - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old // value // was // 11.0 diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java index b3c17122..a6e7afde 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealFourBallAutoCommand.java @@ -15,6 +15,7 @@ import frc.team2412.robot.commands.intake.IntakeSetInCommand; import frc.team2412.robot.commands.intake.IntakeSetOutCommand; import frc.team2412.robot.commands.shooter.ShooterTargetCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; import frc.team2412.robot.subsystem.IndexSubsystem; import frc.team2412.robot.subsystem.IntakeSubsystem; @@ -28,8 +29,8 @@ public class JackStealFourBallAutoCommand extends DynamicRequirementSequentialCo public static class StealFourBallConstants { public static final TrajectoryConstraint[] NORMAL_SPEED = { new FeedforwardConstraint(11.0, - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old // value // was // 11.0 diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java index 85bec3a5..ad07bf8d 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallAutoCommand.java @@ -14,6 +14,7 @@ import frc.team2412.robot.commands.index.IndexSpitCommand; import frc.team2412.robot.commands.intake.IntakeSetInCommand; import frc.team2412.robot.commands.shooter.ShooterTargetCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; import frc.team2412.robot.subsystem.IndexSubsystem; import frc.team2412.robot.subsystem.IntakeSubsystem; @@ -27,8 +28,8 @@ public class JackStealThreeBallAutoCommand extends DynamicRequirementSequentialC public static class StealThreeBallConstants { public static final TrajectoryConstraint[] NORMAL_SPEED = { new FeedforwardConstraint(11.0, - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old // value // was // 11.0 diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java index cb9e6f3d..75f3fe1a 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackStealThreeBallCompatAutoCommand.java @@ -14,6 +14,7 @@ import frc.team2412.robot.commands.index.IndexSpitCommand; import frc.team2412.robot.commands.intake.IntakeSetInCommand; import frc.team2412.robot.commands.shooter.ShooterTargetCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; import frc.team2412.robot.subsystem.IndexSubsystem; import frc.team2412.robot.subsystem.IntakeSubsystem; @@ -28,8 +29,8 @@ public class JackStealThreeBallCompatAutoCommand extends DynamicRequirementSeque public static class StealThreeBallConstants { public static final TrajectoryConstraint[] NORMAL_SPEED = { new FeedforwardConstraint(11.0, - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), - DrivebaseSubsystem.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getVelocityConstant(), + Constants.DriveConstants.FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old // value // was // 11.0 diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java index c4e1e953..23cde664 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/OneBallAutoCommand.java @@ -17,7 +17,7 @@ public OneBallAutoCommand(IndexSubsystem indexSubsystem, ShooterSubsystem shoote new SimplePathBuilder(Vector2.ZERO, Rotation2.ZERO) .lineTo(new Vector2(0, 70)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem, localizer); diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java index d579acf2..4bda7d6d 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandLeft.java @@ -23,7 +23,7 @@ public TwoBallAutoCommandLeft(IndexSubsystem indexSubsystem, ShooterSubsystem sh .lineTo(new Vector2(429.597, 88.203), Rotation2.fromDegrees(315)) .lineTo(new Vector2(426.841, 94.110), Rotation2.fromDegrees(234)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); addCommands( new IntakeSetExtendCommand(intakeSubsystem), diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java index 613a6e79..a40599b0 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandMiddle.java @@ -22,7 +22,7 @@ public TwoBallAutoCommandMiddle(IndexSubsystem indexSubsystem, ShooterSubsystem .lineTo(new Vector2(426.405, 210.657), Rotation2.fromDegrees(25)) .lineTo(new Vector2(420, 215), Rotation2.fromDegrees(-65)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem, localizer); diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java index 1c2495de..0e1a257e 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/TwoBallAutoCommandRight.java @@ -24,7 +24,7 @@ public TwoBallAutoCommandRight(IndexSubsystem indexSubsystem, ShooterSubsystem s .lineTo(new Vector2(337.850, 290.717), Rotation2.fromDegrees(90)) .lineTo(new Vector2(337.850, 287), Rotation2.fromDegrees(0)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); addCommands( new IntakeSetExtendCommand(intakeSubsystem), diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java index b33c8416..16b68974 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/LinePath.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; public class LinePath extends SequentialCommandGroup { @@ -15,7 +16,7 @@ public LinePath(DrivebaseSubsystem drivebaseSubsystem) { new SimplePathBuilder(new Vector2(0, 0), Rotation2.ZERO) .lineTo(new Vector2(24, 0)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, linePathAuto)); } diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java index 23613953..958f701c 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/SquarePath.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; public class SquarePath extends SequentialCommandGroup { @@ -18,7 +19,7 @@ public SquarePath(DrivebaseSubsystem drivebaseSubsystem) { .lineTo(new Vector2(0, 24)) .lineTo(new Vector2(0, 0)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, squarePathAuto)); } diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java b/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java index d98412eb..94c58d63 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/debug/StarPath.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.team2412.robot.commands.autonomous.Follow2910TrajectoryCommand; +import frc.team2412.robot.subsystem.Constants; import frc.team2412.robot.subsystem.DrivebaseSubsystem; public class StarPath extends SequentialCommandGroup { @@ -24,7 +25,7 @@ public StarPath(DrivebaseSubsystem drivebaseSubsystem) { .lineTo(new Vector2(18, 18)) .lineTo(new Vector2(12, 0)) .build(), - DrivebaseSubsystem.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); + Constants.DriveConstants.TRAJECTORY_CONSTRAINTS, 0.1); addCommands(new Follow2910TrajectoryCommand(drivebaseSubsystem, starPathAuto)); } diff --git a/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java b/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java deleted file mode 100644 index fe72dfa9..00000000 --- a/src/main/java/frc/team2412/robot/commands/climb/ClimbResetCommand.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.team2412.robot.commands.climb; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.team2412.robot.subsystem.ClimbSubsystem; - -/** - * Reset the climb subsystem, by lowering the arm fully, - * resetting the encoder and stopping the arm once the - * limit switch has been graciously reached. - */ -public class ClimbResetCommand extends CommandBase { - - private ClimbSubsystem climbSubsystem; - - public ClimbResetCommand(ClimbSubsystem climbSubsystem) { - this.climbSubsystem = climbSubsystem; - addRequirements(climbSubsystem); - } - - @Override - public void execute() { - climbSubsystem.lowerArm(); - } - - @Override - public void end(boolean interrupted) { - climbSubsystem.resetEncoder(true); - climbSubsystem.stopArm(true); - } - - @Override - public boolean isFinished() { - return climbSubsystem.isHittingLimitSwitch(); - } - -} diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java index 4be39cf8..ea0c4fb2 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetInCommand.java @@ -1,6 +1,6 @@ package frc.team2412.robot.commands.intake; -import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_IN_SPEED; +import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_IN_SPEED; import frc.team2412.robot.subsystem.IntakeSubsystem; diff --git a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java index 1c6d0c06..6b0b8895 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/IntakeSetOutCommand.java @@ -1,6 +1,6 @@ package frc.team2412.robot.commands.intake; -import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.INTAKE_OUT_SPEED; +import static frc.team2412.robot.subsystem.Constants.IntakeConstants.INTAKE_OUT_SPEED; import frc.team2412.robot.subsystem.IntakeSubsystem; diff --git a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java index 9eff2ea2..94a54beb 100644 --- a/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/ClimbSubsystem.java @@ -1,13 +1,11 @@ package frc.team2412.robot.subsystem; -import static frc.team2412.robot.subsystem.ClimbSubsystem.ClimbConstants.*; +import static frc.team2412.robot.subsystem.Constants.ClimbConstants.*; import static frc.team2412.robot.Hardware.*; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.Timer; @@ -19,45 +17,6 @@ public class ClimbSubsystem extends SubsystemBase implements Loggable { - public static class ClimbConstants { - // Climb dynamic motor speeds - public static final double EXTEND_SPEED = 0.15; - public static final double RETRACT_SPEED = -0.15; - - // PID stuff - public static final int PID_EXTENSION_SLOT = 0; - public static final double EXTENSION_P = 0.5; - public static final double EXTENSION_I = 0; - public static final double EXTENSION_D = 0; - public static final double EXTENSION_F = 0; - - public static final int PID_RETRACTION_SLOT = 1; - public static final double RETRACTION_P = 0.5; // TODO: figure out values - public static final double RETRACTION_I = 0; - public static final double RETRACTION_D = 0; - public static final double RETRACTION_F = 0.18; - // This is based on the minimum amount of motor power need to keep climb arm in place, need to test - - // Relating to physical climb structure things - // was previously mid - public static final double MID_RUNG_HEIGHT = 5.5; - public static final double RETRACT_HEIGHT = 0.166; - - public static final double CLIMB_OFFSET = 4.75; - - // Doing integer division, which returns 11757 (previously 8789) - // Probably should do floating point division, which returns 11759.3 - public static final double ENCODER_TICKS_PER_REMY = ((272816.0 / 58) * 2 * 5) / 4 * 6; - - // Max robot height is 66 inches - public static final double MAX_ENCODER_TICKS = (11 - CLIMB_OFFSET) * ENCODER_TICKS_PER_REMY; - public static final double MIN_ENCODER_TICKS = 0; - - // Motor current limit config - public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration( - true, 40, 60, 15); - } - @Log.MotorController private final WPI_TalonFX motor; @@ -68,14 +27,7 @@ public ClimbSubsystem() { setName("ClimbSubsystem"); motor = new WPI_TalonFX(CLIMB_FIXED_MOTOR); - // Configure motor soft limits, current limits and peak outputs - TalonFXConfiguration motorConfig = new TalonFXConfiguration(); - motorConfig.forwardSoftLimitEnable = false; - motorConfig.reverseSoftLimitEnable = false; - motorConfig.forwardSoftLimitThreshold = MAX_ENCODER_TICKS; - motorConfig.reverseSoftLimitThreshold = MIN_ENCODER_TICKS; - motorConfig.supplyCurrLimit = MOTOR_CURRENT_LIMIT; - motor.configAllSettings(motorConfig); + motor.configSupplyCurrentLimit(MOTOR_CURRENT_LIMIT); motor.setNeutralMode(NeutralMode.Brake); setPIDExtend(EXTENSION_P, EXTENSION_I, EXTENSION_D); @@ -140,6 +92,20 @@ public void setMotorSpeed(double speed) { motor.set(speed); } + /** + * Graciously lowers arm at set retract speed + */ + public void lowerArm() { + setMotorSpeed(RETRACT_SPEED); + } + + /** + * Graciously extends arm at set speed + */ + public void extendArmSlowly() { + setMotorSpeed(EXTEND_SPEED); + } + /** * Periodic function 🙊🙉🙈 in the simulation * Graciously updates the encoder position in the sim @@ -218,28 +184,4 @@ private void setPIDRetract(@Config(name = "RETRACTION P", defaultValueNumeric = motor.config_kI(PID_RETRACTION_SLOT, i); motor.config_kD(PID_RETRACTION_SLOT, d); } - - /** - * Graciously lowers arm at set retract speed - */ - public void lowerArm() { - motor.set(RETRACT_SPEED); - } - - /** - * Graciously extends arm at set speed - */ - public void extendArmSlowly() { - motor.set(EXTEND_SPEED); - } - - /** - * Determine whether the limit switch has been triggered - * - * @return whether the limit switch has been hit - */ - public boolean isHittingLimitSwitch() { - return true; - } - } diff --git a/src/main/java/frc/team2412/robot/subsystem/Constants.java b/src/main/java/frc/team2412/robot/subsystem/Constants.java new file mode 100644 index 00000000..d6e5ba0c --- /dev/null +++ b/src/main/java/frc/team2412/robot/subsystem/Constants.java @@ -0,0 +1,141 @@ +package frc.team2412.robot.subsystem; + +import org.frcteam2910.common.control.CentripetalAccelerationConstraint; +import org.frcteam2910.common.control.FeedforwardConstraint; +import org.frcteam2910.common.control.MaxAccelerationConstraint; +import org.frcteam2910.common.control.MaxVelocityConstraint; +import org.frcteam2910.common.control.TrajectoryConstraint; +import org.frcteam2910.common.math.Rotation2; +import org.frcteam2910.common.util.DrivetrainFeedforwardConstants; + +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + +import edu.wpi.first.wpilibj.DoubleSolenoid; + +public class Constants { + + public static class ClimbConstants { + // Climb dynamic motor speeds + public static final double EXTEND_SPEED = 0.15; + public static final double RETRACT_SPEED = -0.15; + + // PID stuff + public static final int PID_EXTENSION_SLOT = 0; + public static final double EXTENSION_P = 0.5; + public static final double EXTENSION_I = 0; + public static final double EXTENSION_D = 0; + public static final double EXTENSION_F = 0; + + public static final int PID_RETRACTION_SLOT = 1; + public static final double RETRACTION_P = 0.5; // TODO: figure out values + public static final double RETRACTION_I = 0; + public static final double RETRACTION_D = 0; + public static final double RETRACTION_F = 0.18; + // This is based on the minimum amount of motor power need to keep climb arm in place, need to test + + // Relating to physical climb structure things + // was previously mid + public static final double MID_RUNG_HEIGHT = 5.5; + public static final double RETRACT_HEIGHT = 0.166; + + public static final double CLIMB_OFFSET = 4.75; + + // Doing integer division, which returns 11757 (previously 8789) + // Probably should do floating point division, which returns 11759.3 + public static final double ENCODER_TICKS_PER_REMY = ((272816.0 / 58) * 2 * 5) / 4 * 6; + + // Motor current limit config + public static final SupplyCurrentLimitConfiguration MOTOR_CURRENT_LIMIT = new SupplyCurrentLimitConfiguration( + true, 40, 60, 15); + } + + public static class IndexConstants { + + public static final double CURRENT_LIMIT_TRIGGER_SECONDS = 0.5; + public static final double CURRENT_LIMIT_RESET_AMPS = 10; + public static final double CURRENT_LIMIT_TRIGGER_AMPS = 20; + + // Index Motor Speeds + + public static final double INDEX_FEEDER_SPEED = 0.125; + public static final double INDEX_FEEDER_SHOOT_SPEED = 0.2; + public static final double INDEX_INGEST_SHOOT_SPEED = 0.2; + + public static final double INDEX_IN_SPEED = IntakeConstants.INTAKE_IN_SPEED / 2; + public static final double INDEX_OUT_SPEED = -0.3; + + // The current limit + public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration( + true, CURRENT_LIMIT_RESET_AMPS, CURRENT_LIMIT_TRIGGER_AMPS, CURRENT_LIMIT_TRIGGER_SECONDS); + + } + + public static class ShooterVisionConstants { + // Dimensions are in inches + public static final double LIMELIGHT_HEIGHT_OFFSET = 37.5; + public static final double RIM_HEIGHT = 104; // 8ft8in + public static final double HEIGHT_TO_RIM = RIM_HEIGHT - LIMELIGHT_HEIGHT_OFFSET; + public static final double HUB_RADIUS = 24; + // Angles are in degrees + public static final double LIMELIGHT_ANGLE_OFFSET = Math.toDegrees(Math.atan2(HEIGHT_TO_RIM, 360 - HUB_RADIUS)); // 10.95 + + public static final int COMP_PIPELINE_NUM = 5; + // -0.766666 limelight crosshair offset (3/19 update) + } + + // Constants + public static class IntakeConstants { + + public static final double INNER_INTAKE_IN_SPEED = 0.35; // TODO + public static final double INTAKE_IN_SPEED = 0.85; + public static final double INTAKE_OUT_SPEED = -0.3; + + public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration( + true, 20, 20, 1); + + // Enums + + public static enum IntakeSolenoidState { + EXTEND(DoubleSolenoid.Value.kForward, "Extended"), RETRACT(DoubleSolenoid.Value.kReverse, "Reversed"); + + public final DoubleSolenoid.Value value; + public final String state; + + private IntakeSolenoidState(DoubleSolenoid.Value value, String state) { + this.value = value; + this.state = state; + } + } + + } + + public static class DriveConstants { + + public static final double TRACKWIDTH = 1.0; + public static final double WHEELBASE = 1.0; + + public static final DrivetrainFeedforwardConstants FEEDFORWARD_CONSTANTS = new DrivetrainFeedforwardConstants( + 0.0593, // velocity + 0.00195, // acceleration + 0.236); // static + + public static final TrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = { + new FeedforwardConstraint(11.0, FEEDFORWARD_CONSTANTS.getVelocityConstant(), + FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old value was 11.0 + new MaxAccelerationConstraint(3 * 12.0), // old value was 12.5 * 12.0 + new MaxVelocityConstraint(4 * 12.0), + new CentripetalAccelerationConstraint(6 * 12.0), // old value was 15 * 12.0 + }; + + public static final int MAX_LATENCY_COMPENSATION_MAP_ENTRIES = 25; + + public static final boolean ANTI_TIP_DEFAULT = true; + + public static final boolean FIELD_CENTRIC_DEFAULT = true; + + public static final double TIP_P = 0.05, TIP_F = 0, TIP_TOLERANCE = 10, ACCEL_LIMIT = 4; + + public static final Rotation2 PRACTICE_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(-90), // should be 90 + COMP_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(0); + } +} diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java index b7458276..5eb6360b 100644 --- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java @@ -1,82 +1,36 @@ package frc.team2412.robot.subsystem; import static frc.team2412.robot.Hardware.*; +import static frc.team2412.robot.subsystem.Constants.DriveConstants.*; -import com.google.errorprone.annotations.concurrent.GuardedBy; -import com.swervedrivespecialties.swervelib.SwerveModule; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team2412.robot.Hardware; -import frc.team2412.robot.Robot; -import frc.team2412.robot.util.GeoConvertor; -import frc.team2412.robot.util.PFFController; -import frc.team2412.robot.util.VectorSlewLimiter; -import io.github.oblarg.oblog.annotations.Config; +import java.util.*; import org.frcteam2910.common.control.*; import org.frcteam2910.common.drivers.Gyroscope; -import org.frcteam2910.common.kinematics.ChassisVelocity; -import org.frcteam2910.common.kinematics.SwerveKinematics; -import org.frcteam2910.common.kinematics.SwerveOdometry; -import org.frcteam2910.common.math.RigidTransform2; -import org.frcteam2910.common.math.Rotation2; -import org.frcteam2910.common.math.Vector2; +import org.frcteam2910.common.kinematics.*; +import org.frcteam2910.common.math.*; import org.frcteam2910.common.robot.UpdateManager; -import org.frcteam2910.common.robot.drivers.NavX; -import org.frcteam2910.common.robot.drivers.PigeonTwo; +import org.frcteam2910.common.robot.drivers.*; import org.frcteam2910.common.util.*; +import org.frcteam2910.common.util.InterpolatingTreeMap; -import java.util.Map; -import java.util.Optional; +import com.google.errorprone.annotations.concurrent.GuardedBy; +import com.swervedrivespecialties.swervelib.SwerveModule; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.smartdashboard.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team2412.robot.*; +import frc.team2412.robot.util.*; import io.github.oblarg.oblog.Loggable; - -import static frc.team2412.robot.subsystem.DrivebaseSubsystem.DriveConstants.*; +import io.github.oblarg.oblog.annotations.Config; +import io.github.oblarg.oblog.annotations.Log; public class DrivebaseSubsystem extends SubsystemBase implements UpdateManager.Updatable, Loggable { - // TODO find values as these are just copied from 2910 - public static class DriveConstants { - - public static final double TRACKWIDTH = 1.0; - public static final double WHEELBASE = 1.0; - - public static final DrivetrainFeedforwardConstants FEEDFORWARD_CONSTANTS = new DrivetrainFeedforwardConstants( - 0.0593, // velocity - 0.00195, // acceleration - 0.236); // static - - public static final TrajectoryConstraint[] TRAJECTORY_CONSTRAINTS = { - new FeedforwardConstraint(11.0, FEEDFORWARD_CONSTANTS.getVelocityConstant(), - FEEDFORWARD_CONSTANTS.getAccelerationConstant(), false), // old value was 11.0 - new MaxAccelerationConstraint(3 * 12.0), // old value was 12.5 * 12.0 - new MaxVelocityConstraint(4 * 12.0), - new CentripetalAccelerationConstraint(6 * 12.0), // old value was 15 * 12.0 - }; - - public static final int MAX_LATENCY_COMPENSATION_MAP_ENTRIES = 25; - - public static final boolean ANTI_TIP_DEFAULT = true; - - public static final boolean FIELD_CENTRIC_DEFAULT = true; - - public static final double TIP_P = 0.05, TIP_F = 0, TIP_TOLERANCE = 10, ACCEL_LIMIT = 4; - - public static final Rotation2 PRACTICE_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(-90), // should be 90 - COMP_BOT_DRIVE_OFFSET = Rotation2.fromDegrees(0); - } - private final HolonomicMotionProfiledTrajectoryFollower follower = new HolonomicMotionProfiledTrajectoryFollower( new PidConstants(0.0708, 0.0, 0.0), new PidConstants(5.0, 0.0, 0.0), @@ -90,7 +44,6 @@ public static class DriveConstants { ); private SwerveModule[] modules; - private final double moduleMaxVelocityMetersPerSec; private final Object sensorLock = new Object(); @GuardedBy("sensorLock") @@ -113,17 +66,39 @@ public static class DriveConstants { private HolonomicDriveSignal driveSignal = null; // Logging - private final NetworkTableEntry odometryXEntry; - private final NetworkTableEntry odometryYEntry; - private final NetworkTableEntry odometryAngleEntry; - private final NetworkTableEntry speedModifier; - private final NetworkTableEntry shootSpeedToggle; - private final NetworkTableEntry shootSpeed; - private final NetworkTableEntry antiTip; - private final NetworkTableEntry fieldCentric; - private final NetworkTableEntry poseSetX; - private final NetworkTableEntry poseSetY; - private final NetworkTableEntry poseSetAngle; + @Log(name = "X", columnIndex = 0, rowIndex = 0) + private double odometryXEntry; + @Log(name = "Y", columnIndex = 0, rowIndex = 1) + private double odometryYEntry; + @Log(name = "Angle", columnIndex = 0, rowIndex = 2) + private double odometryAngleEntry; + + @Config.NumberSlider(name = "Speed Modifier", columnIndex = 2, rowIndex = 1, defaultValue = 0.95) + private double speedModifier; + + @Config.NumberSlider(name = "Shoot Speed", columnIndex = 4, rowIndex = 1, defaultValue = 0.6) + private double shootSpeed; + @Config.ToggleSwitch(name = "Shoot speed toggled", columnIndex = 4, rowIndex = 3) + private boolean shootSpeedToggle; + + @Config.ToggleSwitch(name = "Anti tip", columnIndex = 3, rowIndex = 1, width = 2, defaultValue = ANTI_TIP_DEFAULT) + private boolean antiTip; + @Config.ToggleSwitch(name = "Field Centric", columnIndex = 2, rowIndex = 2, width = 2, defaultValue = FIELD_CENTRIC_DEFAULT) + private boolean fieldCentric; + + @Config(name = "Set Pose X", columnIndex = 5, rowIndex = 3) + private double poseSetX; + @Config(name = "Set Pose Y", columnIndex = 6, rowIndex = 3) + private double poseSetY; + @Config(name = "Set Pose Angle", columnIndex = 6, rowIndex = 3) + private double poseSetAngle; + + @Log(name = "Trajectory X", columnIndex = 1, rowIndex = 0) + private double trajectoryX; + @Log(name = "Trajectory Y", columnIndex = 1, rowIndex = 1) + private double trajectoryY; + @Log(name = "Rotation Voltage", columnIndex = 1, rowIndex = 1) + private double rotationVoltage; private final Field2d field = new Field2d(); @@ -131,6 +106,7 @@ public static class DriveConstants { private final VectorSlewLimiter accelLimiter; public DrivebaseSubsystem() { + setName("Drivebase"); boolean comp = Robot.getInstance().isCompetition(); synchronized (sensorLock) { @@ -141,112 +117,15 @@ public DrivebaseSubsystem() { SmartDashboard.putData("Field", field); } - ShuffleboardTab tab = Shuffleboard.getTab("Drivebase"); - boolean supportAbsoluteEncoder = comp && !Robot.isSimulation(); modules = new SwerveModule[] { FRONT_LEFT_CONFIG.create(supportAbsoluteEncoder), FRONT_RIGHT_CONFIG.create(supportAbsoluteEncoder), BACK_LEFT_CONFIG.create(supportAbsoluteEncoder), BACK_RIGHT_CONFIG.create(supportAbsoluteEncoder) }; - moduleMaxVelocityMetersPerSec = MODULE_MAX_VELOCITY_METERS_PER_SEC; - - odometryXEntry = tab.add("X", 0.0) - .withPosition(0, 0) - .withSize(1, 1) - .getEntry(); - odometryYEntry = tab.add("Y", 0.0) - .withPosition(0, 1) - .withSize(1, 1) - .getEntry(); - odometryAngleEntry = tab.add("Angle", 0.0) - .withPosition(0, 2) - .withSize(1, 1) - .getEntry(); - tab.addNumber("Trajectory X", () -> { - if (follower.getLastState() == null) { - return 0.0; - } - return follower.getLastState().getPathState().getPosition().x; - }) - .withPosition(1, 0) - .withSize(1, 1); - tab.addNumber("Trajectory Y", () -> { - if (follower.getLastState() == null) { - return 0.0; - } - return follower.getLastState().getPathState().getPosition().y; - }) - .withPosition(1, 1) - .withSize(1, 1); - - tab.addNumber("Rotation Voltage", () -> { - HolonomicDriveSignal signal; - synchronized (stateLock) { - signal = driveSignal; - } - - if (signal == null) { - return 0.0; - } - - return signal.getRotation() * RobotController.getBatteryVoltage(); - }); - - speedModifier = tab.add("Speed Modifier", 1f) - .withPosition(2, 1) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("min", 0.0, "max", 1.0, "defaultValueNumeric", 0.95)) - .getEntry(); - - shootSpeed = tab.add("ShootSpeed", 0.6f) - .withPosition(4, 1) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("min", 0.0, "max", 1.0, "defaultValueNumeric", 0.6)) - .getEntry(); - - shootSpeedToggle = tab.add("ShootSpeedToggled", false) - .withPosition(4, 2) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); - - tab.addNumber("Average Velocity", this::getAverageAbsoluteValueVelocity); - - antiTip = tab.add("Anti Tip", ANTI_TIP_DEFAULT) - .withPosition(3, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); - - fieldCentric = tab.add("Field Centric", FIELD_CENTRIC_DEFAULT) - .withPosition(2, 2) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kToggleSwitch) - .getEntry(); tipController = PFFController.ofVector2(TIP_P, TIP_F).setTargetPosition(getGyroscopeXY()) .setTargetPositionTolerance(TIP_TOLERANCE); - poseSetX = tab.add("Set Pose X", 0.0) - .withPosition(5, 3) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - poseSetY = tab.add("Set Pose Y", 0.0) - .withPosition(6, 3) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - - poseSetAngle = tab.add("Set Pose Angle", 0.0) - .withPosition(7, 3) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kTextView) - .getEntry(); - accelLimiter = new VectorSlewLimiter(ACCEL_LIMIT); } @@ -286,9 +165,9 @@ public Pose2d getPoseAsPoseMeters() { } public void setPose() { - resetPose(new Pose2d(poseSetX.getDouble(0.0), poseSetY.getDouble(0.0), - Rotation2d.fromDegrees(poseSetAngle.getDouble(0.0)))); - resetGyroAngle(Rotation2.fromDegrees(poseSetAngle.getDouble(0.0)).inverse()); + resetPose(new Pose2d(poseSetX, poseSetY, + Rotation2d.fromDegrees(poseSetAngle))); + resetGyroAngle(Rotation2.fromDegrees(poseSetAngle).inverse()); } public Vector2 getVelocity() { @@ -313,10 +192,10 @@ public Rotation2 getAngle() { public void drive(Vector2 translationalVelocity, double rotationalVelocity) { synchronized (stateLock) { driveSignal = new HolonomicDriveSignal( - translationalVelocity.scale(speedModifier.getDouble(1.0)) - .scale(shootSpeedToggle.getBoolean(false) ? shootSpeed.getDouble(1) : 1) + translationalVelocity.scale(speedModifier) + .scale(shootSpeedToggle ? shootSpeed : 1) .rotateBy(getRotationAdjustment()), - (rotationalVelocity * speedModifier.getDouble(1.0)), false); + (rotationalVelocity * speedModifier), false); } } @@ -364,6 +243,7 @@ private Rotation2 getRotationAdjustment() { : COMP_BOT_DRIVE_OFFSET; } + @Log(name = "Average Velocity", columnIndex = 8, rowIndex = 3) public double getAverageAbsoluteValueVelocity() { double averageVelocity = 0; for (var module : modules) { @@ -404,7 +284,7 @@ public void updateModules(HolonomicDriveSignal driveSignal) { ChassisVelocity chassisVelocity; if (driveSignal == null) { chassisVelocity = new ChassisVelocity(Vector2.ZERO, 0.0); - } else if (fieldCentric.getBoolean(true)) { + } else if (fieldCentric) { chassisVelocity = new ChassisVelocity( driveSignal.getTranslation().rotateBy(getAngle()), driveSignal.getRotation()); @@ -438,8 +318,8 @@ public void setToX() { public void updateModules(ChassisSpeeds chassisSpeeds) { HolonomicDriveSignal holonomicDriveSignal = new HolonomicDriveSignal( new Vector2( - (chassisSpeeds.vxMetersPerSecond / moduleMaxVelocityMetersPerSec), - (chassisSpeeds.vyMetersPerSecond / moduleMaxVelocityMetersPerSec)), + (chassisSpeeds.vxMetersPerSecond / MODULE_MAX_VELOCITY_METERS_PER_SEC), + (chassisSpeeds.vyMetersPerSecond / MODULE_MAX_VELOCITY_METERS_PER_SEC)), chassisSpeeds.omegaRadiansPerSecond, false); synchronized (stateLock) { @@ -491,12 +371,23 @@ public void update(double time, double dt) { @Override public void periodic() { - // Pose2d pose = getPoseAsPoseMeters(); synchronized (kinematicsLock) { - odometryXEntry.setDouble(Units.inchesToMeters(pose.translation.x)); - odometryYEntry.setDouble(Units.inchesToMeters(pose.translation.y)); - odometryAngleEntry.setDouble(pose.rotation.toDegrees()); + odometryXEntry = Units.inchesToMeters(pose.translation.x); + odometryYEntry = Units.inchesToMeters(pose.translation.y); + odometryAngleEntry = pose.rotation.toDegrees(); + } + + synchronized (stateLock) { + if (driveSignal != null) { + rotationVoltage = driveSignal.getRotation() * RobotController.getBatteryVoltage(); + } } + + if (follower.getLastState() != null) { + trajectoryX = follower.getLastState().getPathState().getPosition().x; + trajectoryY = follower.getLastState().getPathState().getPosition().y; + } + // System.out.println(pose); Pose2d pose = getPoseAsPoseMeters(); field.setRobotPose(pose); @@ -512,10 +403,10 @@ public void follow(Path p) { } public boolean getFieldCentric() { - return fieldCentric.getBoolean(false); + return fieldCentric; } public boolean getAntiTip() { - return antiTip.getBoolean(false); + return antiTip; } } diff --git a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java index d604ed97..818d4987 100644 --- a/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/IndexSubsystem.java @@ -1,10 +1,9 @@ package frc.team2412.robot.subsystem; -import static frc.team2412.robot.subsystem.IndexSubsystem.IndexConstants.*; +import static frc.team2412.robot.subsystem.Constants.IndexConstants.*; import static frc.team2412.robot.Hardware.*; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.DigitalInput; @@ -17,31 +16,7 @@ public class IndexSubsystem extends SubsystemBase implements Loggable { - // Constants - - public static class IndexConstants { - - public static final double CURRENT_LIMIT_TRIGGER_SECONDS = 0.5; - public static final double CURRENT_LIMIT_RESET_AMPS = 10; - public static final double CURRENT_LIMIT_TRIGGER_AMPS = 20; - - // Index Motor Speeds - - public static final double INDEX_FEEDER_SPEED = 0.125; - public static final double INDEX_FEEDER_SHOOT_SPEED = 0.2; - public static final double INDEX_INGEST_SHOOT_SPEED = 0.2; - - public static final double INDEX_IN_SPEED = IntakeSubsystem.IntakeConstants.INTAKE_IN_SPEED / 2; - public static final double INDEX_OUT_SPEED = -0.3; - - // The current limit - public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration( - true, CURRENT_LIMIT_RESET_AMPS, CURRENT_LIMIT_TRIGGER_AMPS, CURRENT_LIMIT_TRIGGER_SECONDS); - - } - // Define Hardware - private final DigitalInput leftFeederProximity; private final DigitalInput rightFeederProximity; diff --git a/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java index b64b19bb..4209e567 100644 --- a/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/IntakeSubsystem.java @@ -1,11 +1,10 @@ package frc.team2412.robot.subsystem; -import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.*; -import static frc.team2412.robot.subsystem.IntakeSubsystem.IntakeConstants.IntakeSolenoidState.*; +import static frc.team2412.robot.subsystem.Constants.IntakeConstants.*; +import static frc.team2412.robot.subsystem.Constants.IntakeConstants.IntakeSolenoidState.*; import static frc.team2412.robot.Hardware.*; import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.DigitalInput; @@ -20,31 +19,6 @@ public class IntakeSubsystem extends SubsystemBase implements Loggable { - // Constants - public static class IntakeConstants { - - public static final double INNER_INTAKE_IN_SPEED = 0.35; // TODO - public static final double INTAKE_IN_SPEED = 0.85; - public static final double INTAKE_OUT_SPEED = -0.3; - - public static final SupplyCurrentLimitConfiguration MAX_MOTOR_CURRENT = new SupplyCurrentLimitConfiguration( - true, 20, 20, 1); - - // Enums - - public static enum IntakeSolenoidState { - EXTEND(DoubleSolenoid.Value.kForward, "Extended"), RETRACT(DoubleSolenoid.Value.kReverse, "Reversed"); - - public final DoubleSolenoid.Value value; - public final String state; - - private IntakeSolenoidState(DoubleSolenoid.Value value, String state) { - this.value = value; - this.state = state; - } - } - } - // Define Hardware @Log diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java index 9de0c124..774332d6 100644 --- a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java @@ -124,7 +124,8 @@ public ShooterSubsystem() { this.hoodEncoder = hoodMotor.getEncoder(); this.hoodPID = hoodMotor.getPIDController(); configMotors(); - flywheelFF = new SimpleMotorFeedforward(0.735, 0.1193, 0.0056666); + flywheelFF = new SimpleMotorFeedforward(0.471, 0.1193); + // Fixed this by subtracting offset * battery voltage from kS, previously 0.735 } /* FUNCTIONS */ @@ -187,6 +188,10 @@ private void configMotors() { @Override public void periodic() { + if (shooterOverride) { + setFlywheelRPM(flywheelTestRPM); + setHoodAngle(hoodTestAngle); + } } public void simInit(PhysicsSim sim) { @@ -320,7 +325,7 @@ public double getFlywheelRPMError() { public void setFlywheelVelocity(double velocity) { // flywheelMotor1.set(ControlMode.Velocity, velocity); double rps = velocity / 2048 * 10; - double feedForward = flywheelFF.calculate(rps) / Robot.getInstance().getVoltage() - FLYWHEEL_FEEDFORWARD_OFFSET; + double feedForward = flywheelFF.calculate(rps) / Robot.getInstance().getVoltage(); flywheelMotor1.set(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward, feedForward); @@ -415,6 +420,7 @@ public void resetHoodEncoder(boolean reset) { public void setTurretAngle(double angle) { if (isTurretAtAngle(angle) || turretDisable) { + turretMotor.stopMotor(); return; } @@ -467,7 +473,7 @@ public double getTurretAngle() { * @param angle * The angle (in degrees) to compare the turret's angle to. * @return True if difference between turret angle and given angle is less than - * HOOD_ANGLE_TOLERANCE, False otherwise. + * TURRET_ANGLE_TOLERANCE, False otherwise. */ public boolean isTurretAtAngle(double angle) { return Math.abs(getTurretAngle() - angle) < TURRET_ANGLE_TOLERANCE; diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java index e61168c0..4c1a1a02 100644 --- a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java @@ -1,6 +1,6 @@ package frc.team2412.robot.subsystem; -import static frc.team2412.robot.subsystem.ShooterVisionSubsystem.ShooterVisionConstants.*; +import static frc.team2412.robot.subsystem.Constants.ShooterVisionConstants.*; import static frc.team2412.robot.Hardware.*; @@ -12,18 +12,6 @@ import io.github.oblarg.oblog.annotations.Log; public class ShooterVisionSubsystem extends SubsystemBase implements Loggable { - public static class ShooterVisionConstants { - // Dimensions are in inches - public static final double LIMELIGHT_HEIGHT_OFFSET = 37.5; - public static final double RIM_HEIGHT = 104; // 8ft8in - public static final double HEIGHT_TO_RIM = RIM_HEIGHT - LIMELIGHT_HEIGHT_OFFSET; - public static final double HUB_RADIUS = 24; - // Angles are in degrees - public static final double LIMELIGHT_ANGLE_OFFSET = Math.toDegrees(Math.atan2(HEIGHT_TO_RIM, 360 - HUB_RADIUS)); // 10.95 - - public static final int COMP_PIPELINE_NUM = 5; - // -0.766666 limelight crosshair offset (3/19 update) - } public NetworkTable limelight;