diff --git a/.github/workflows/ci.sh b/.github/workflows/ci.sh index 4beca4c..ba6f835 100644 --- a/.github/workflows/ci.sh +++ b/.github/workflows/ci.sh @@ -3,6 +3,7 @@ # Make sure that Gradlew is executable. chmod +x gradlew + # Start simulation. ./gradlew simulateJavaRelease & sim_pid=$! diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ec68183..b4cf72d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,6 +27,11 @@ jobs: - name: Grant execute permission for ci script run: chmod +x ./.github/workflows/ci.sh + + # Fix any violations with lib + - name: Fix any violations with lib + run: ./gradlew :PARTsLib:spotlessApply + # Runs a single command using the runners shell - name: Compile robot code run: ./gradlew build diff --git a/PARTsLib b/PARTsLib index 7d8c81d..621fc4c 160000 --- a/PARTsLib +++ b/PARTsLib @@ -1 +1 @@ -Subproject commit 7d8c81db87d1a11ba1fce7ffbf892417923578c2 +Subproject commit 621fc4c5daf021b21d9b8d6ec42c3cb202b698c0 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca98322..9600ce3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,16 +9,12 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.constants.RobotConstants; import frc.robot.subsystems.LimelightVision.MegaTagMode; import frc.robot.util.Hub; -import org.parts3492.partslib.network.PARTsDashboard; -import org.parts3492.partslib.network.PARTsDashboard.DashboardTab; - import com.pathplanner.lib.commands.FollowPathCommand; import org.parts3492.partslib.PARTsLogger; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52b78a3..b1675dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -36,6 +37,7 @@ import frc.robot.states.CandleState; import frc.robot.subsystems.Candle; import frc.robot.subsystems.LimelightVision; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.LimelightVision.MegaTagMode; import frc.robot.subsystems.Drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.Drivetrain.PARTsDrivetrain; @@ -45,15 +47,18 @@ import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Intake.IntakePhys; import frc.robot.subsystems.Intake.IntakeSim; +import frc.robot.subsystems.Intake.IntakeSysid; import frc.robot.subsystems.Kicker.Kicker; import frc.robot.subsystems.Kicker.KickerPhys; import frc.robot.subsystems.Kicker.KickerSim; import frc.robot.subsystems.Shooter.Shooter; import frc.robot.subsystems.Shooter.ShooterPhys; import frc.robot.subsystems.Shooter.ShooterSim; +import frc.robot.subsystems.Shooter.ShooterSysid; import frc.robot.subsystems.Turret.Turret; import frc.robot.subsystems.Turret.TurretPhys; import frc.robot.subsystems.Turret.TurretSim; +import frc.robot.subsystems.Turret.TurretSysid; import frc.robot.util.Field; import org.parts3492.partslib.input.PARTsButtonBoxController; @@ -77,7 +82,7 @@ public class RobotContainer { private static Alliance alliance; - //region Subsystems + // region Subsystems public final PARTsDrivetrain drivetrain = new PARTsDrivetrain( TunerConstants.DrivetrainConstants, @@ -90,7 +95,7 @@ public class RobotContainer { public final Candle candle = new Candle(); - private final Shooter shooter = Robot.isReal() ? new ShooterPhys() : new ShooterSim(); + private final Shooter shooter = Robot.isReal() ? new ShooterPhys(drivetrain.supplierGetPose()) : new ShooterSim(drivetrain.supplierGetPose()); private final Turret turret = Robot.isReal() ? new TurretPhys(drivetrain.supplierGetPose()) : new TurretSim(drivetrain.supplierGetPose()); @@ -100,24 +105,34 @@ public class RobotContainer { private final Hopper hopper = Robot.isReal() ? new HopperPhys() : new HopperSim(); private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim(); - // private final ShooterSysid shooter = new ShooterSysid(); //for sysid + //private final ShooterSysid shooter = new ShooterSysid(drivetrain.supplierGetPose()); //for sysid + // private final IntakeSysid intake = new IntakeSysid(); //for sysid + // private final TurretSysid turret = new TurretSysid(drivetrain.supplierGetPose()); + + private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret); private final ArrayList subsystems = new ArrayList( - Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake)); + Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake, superstructure)); - //endregion End Subsystems + // endregion End Subsystems public RobotContainer() { configureDrivetrainBindings(); configureCandleBindings(); configureShooterBindings(); + configureTurretBindings(); configureAutonomousCommands(); + configureIntakeBindings(); + configureHopperBindings(); + configureSuperstructureBindings(); + operatorController.povUp().onTrue(Commands.runOnce(() -> SignalLogger.start())); + operatorController.povDown().onTrue(Commands.runOnce(() -> SignalLogger.stop())); partsNT.putSmartDashboardSendable("field", Field.FIELD2D); hubFieldObject2d = Field.FIELD2D.getObject("hub"); } - //region Configs + // region Configs private void configureDrivetrainBindings() { @@ -142,10 +157,10 @@ private void configureDrivetrainBindings() { // reset the field-centric heading on left bumper press driveController.leftBumper().onTrue(drivetrain.commandSeedFieldCentric()); - driveController.x().onTrue( - drivetrain.targetPoseCommand(() -> Field.blueHubCenter, () -> driveController.y().getAsBoolean())); - driveController.a().onTrue(drivetrain.commandSnapToAngle(90)); - driveController.b().onTrue(drivetrain.commandAlign(Field.getTag(28).getLocation().toPose2d())); + //driveController.x().onTrue( + //drivetrain.targetPoseCommand(() -> Field.blueHubCenter, () -> driveController.y().getAsBoolean())); + //driveController.a().onTrue(drivetrain.commandSnapToAngle(90)); + //driveController.b().onTrue(drivetrain.commandAlign(Field.getTag(28).getLocation().toPose2d())); /* * if (RobotConstants.DEBUGGING) { @@ -177,23 +192,64 @@ private void configureDrivetrainBindings() { } private void configureShooterBindings() { - // driveController.a().onTrue(shooter.shoot()); - // driveController.b().onTrue(shooter.idle()); + //driveController.a().onTrue(shooter.shoot()); + //driveController.b().onTrue(shooter.idle()); + + + /*operatorController.a().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + operatorController.b().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + operatorController.x().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + operatorController.y().and(operatorController.rightBumper()) + .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));*/ + } + + private void configureCandleBindings() { + + } + + private void configureHopperBindings() { + //driveController.a().onTrue(hopper.roll()); + } + + private void configureTurretBindings() { + //driveController.a().onTrue(turret.track()); + //driveController.b().onTrue(turret.idle()); /* * operatorController.a().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + * .whileTrue(turret.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); * operatorController.b().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + * .whileTrue(turret.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); * operatorController.x().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + * .whileTrue(turret.sysIdDynamic(SysIdRoutine.Direction.kForward)); * operatorController.y().and(operatorController.rightBumper()) - * .whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + * .whileTrue(turret.sysIdDynamic(SysIdRoutine.Direction.kReverse)); */ } - private void configureCandleBindings() { + private void configureIntakeBindings() { + driveController.x().onTrue(intake.intake()); + //driveController.b().onTrue(intake.intakeIdle()); + //driveController.x().onTrue(intake.home()); + + /* + * operatorController.a().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + * operatorController.b().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + * operatorController.x().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kForward)); + * operatorController.y().and(operatorController.rightBumper()) + * .whileTrue(intake.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + */ + + } + private void configureSuperstructureBindings() { + driveController.a().onTrue(superstructure.shoot(driveController.b()::getAsBoolean)); } public void configureAutonomousCommands() { @@ -201,13 +257,13 @@ public void configureAutonomousCommands() { SmartDashboard.putData("Auto Chooser", autoChooser); } - //endregion End Configs + // endregion End Configs public Command getAutonomousCommand() { return autoChooser.getSelected(); } - //region Custom Public Functions + // region Custom Public Functions public void outputTelemetry() { subsystems.forEach(s -> s.outputTelemetry()); @@ -270,6 +326,7 @@ public void runOnEnabled() { setLimelightMainMode(); setIdleCandleState(); hubFieldObject2d.setPose(Field.getAllianceHubPose()); + subsystems.forEach(s -> s.reset()); CommandScheduler.getInstance().schedule(new WaitCommand(2).andThen(Commands.runOnce(() -> { /* * if (!RobotContainer.isBlue()) { diff --git a/src/main/java/frc/robot/constants/CandleConstants.java b/src/main/java/frc/robot/constants/CandleConstants.java index 6943c83..8cffec0 100644 --- a/src/main/java/frc/robot/constants/CandleConstants.java +++ b/src/main/java/frc/robot/constants/CandleConstants.java @@ -3,4 +3,5 @@ public class CandleConstants { public static final int CAN_ID = 33; public static final int LED_LENGTH = 1000; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 8616a4a..422970c 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -13,12 +13,6 @@ public class DrivetrainConstants { public static final double MAX_RANGE_VELOCITY = 2;// m/s public static final double MAX_RANGE_ACCELERATION = 2;// m/2^s - /*SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND = 5.41; - SWERVE_CONSTANTS.MAX_DRIVING_ACCELERATION_METERS_PER_SECOND_SQUARED = 32; - SWERVE_CONSTANTS.MAX_STEER_VELOCITY_RADIANS_PER_SECOND = 125.5 * 2 * Math.PI; - // max velocity in 1/10 sec - SWERVE_CONSTANTS.MAX_STEER_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 10 * 125.5 * 2 * Math.PI;*/ - public static final double THETA_P = 10; // Proprotinal //4.5 15 public static final double THETA_I = 0; // Gradual corretction public static final double THETA_D = 0; // Smooth oscilattions diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java deleted file mode 100644 index c081603..0000000 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.constants; - -import org.parts3492.partslib.PARTsUnit; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; - -public class FieldConstants { - public static final PARTsUnit ZONE1_RADIUS = new PARTsUnit(4, PARTsUnitType.Foot); - public static final PARTsUnit ZONE2_RADIUS = new PARTsUnit(6, PARTsUnitType.Foot); - public static final PARTsUnit ZONE3_RADIUS = new PARTsUnit(8, PARTsUnitType.Foot); - public static final PARTsUnit ZONE4_RADIUS = new PARTsUnit(10, PARTsUnitType.Foot); -} diff --git a/src/main/java/frc/robot/constants/HopperConstants.java b/src/main/java/frc/robot/constants/HopperConstants.java index 54aabd4..7dddf85 100644 --- a/src/main/java/frc/robot/constants/HopperConstants.java +++ b/src/main/java/frc/robot/constants/HopperConstants.java @@ -1,7 +1,6 @@ package frc.robot.constants; public class HopperConstants { - public static final double ROLL_SPEED = 0.3; - public static final double BACKROLL_SPEED = -0.3; public static final int HOPPER_MOTOR_ID = 37; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index a5f1883..7fbbc1b 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -3,4 +3,19 @@ public class IntakeConstants { public static final int INTAKE_MOTOR_ID = 36; public static final int PIVOT_MOTOR_ID = 38; + public static final String CAN_BUS_NAME = "bye"; + + /** The pivot gear ratio. The total is {@code 36/1}. */ + public static final double PIVOT_GEAR_RATIO = (12.0/1.0)*(3.0/1.0); + + // PID Controller + public static final double P = 0.07; + public static final double I = 0; + public static final double D = 0; + public static final int PID_THRESHOLD = 1; + + // Feedforward + public static final double S = 0; + public static final double V = 0; + public static final double A = 0; } diff --git a/src/main/java/frc/robot/constants/KickerConstants.java b/src/main/java/frc/robot/constants/KickerConstants.java index ec9b887..a246475 100644 --- a/src/main/java/frc/robot/constants/KickerConstants.java +++ b/src/main/java/frc/robot/constants/KickerConstants.java @@ -1,6 +1,6 @@ package frc.robot.constants; public class KickerConstants { - public static final double KICKER_SPEED = 0.1; // TBD public static final int KICKER_MOTOR_ID = 34; + public static final String CAN_BUS_NAME = "bye"; } diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index 96afedb..59bb58c 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -12,6 +12,6 @@ public class RobotConstants { PARTsUnitType.Inch); public static boolean LOGGING = true; - public static boolean DEBUGGING = true; + public static boolean DEBUGGING = false; public static boolean ALLOW_AUTO_CONTROLLER_DETECTION = false; -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index f71a32a..5a9b9a7 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -4,9 +4,9 @@ import org.parts3492.partslib.PARTsUnit.PARTsUnitType; public class ShooterConstants { - public static final double SHOOTER_RPM = 3000; public static final int LEFT_MOTOR_ID = 33; - public static final int RIGHT_MOTOR_ID = 0; + public static final int RIGHT_MOTOR_ID = 35; + public static final String CAN_BUS_NAME = "bye"; public static final PARTsUnit SHOOTER_WHEEL_RADIUS = new PARTsUnit(1.5, PARTsUnitType.Inch); @@ -20,7 +20,7 @@ public class ShooterConstants { public static final int PID_THRESHOLD = 50; // Feedforward - public static final double S = 0.27506; - public static final double V = 0.53757; - public static final double A = 0.057542; + public static final double S = 0.041561; + public static final double V = 0.48125; + public static final double A = 0.019433; } diff --git a/src/main/java/frc/robot/constants/TurretConstants.java b/src/main/java/frc/robot/constants/TurretConstants.java index d660436..64c30c3 100644 --- a/src/main/java/frc/robot/constants/TurretConstants.java +++ b/src/main/java/frc/robot/constants/TurretConstants.java @@ -2,12 +2,15 @@ public class TurretConstants { public static final int TURRET_MOTOR_ID = 39; + public static final String CAN_BUS_NAME = "bye"; + /** The turret gear ratio. The total ratio is {@code 40/1}. */ + public static final double TURRET_GEAR_RATIO = (200.0/1.0)*(1.0/20.0)*(4.0/1.0); // PID Controller - public static final double P = 0; + public static final double P = 0.3; public static final double I = 0; public static final double D = 0; - public static final int PID_THRESHOLD = 0; + public static final int PID_THRESHOLD = 1; // Feedforward public static final double S = 0; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 8a99127..ac8463c 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -2,19 +2,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N3; -import org.parts3492.partslib.PARTsUnit; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; public class VisionConstants { - /*public static final String FRONT_LIMELIGHT = "limelight-slimmy"; // The_Real - public static final String BACK_LIMELIGHT = "limelight-thereal"; - public static final PARTsUnit LIMELIGHT_LENS_HEIGHT = new PARTsUnit(9, PARTsUnitType.Inch); // Inches - public static final PARTsUnit LIMELIGHT_ANGLE = new PARTsUnit(0, PARTsUnitType.Angle); // Degrees - - //public static final double REEF_APRILTAG_HEIGHT = 16; // Distance.ofBaseUnits(6.875, Inches); - //public static final double PROCCESSOR_APRILTAG_HEIGHT = 45.875; // Inches - //public static final double CORAL_APRILTAG_HEIGHT = 53.25; // Inches*/ - public static final edu.wpi.first.math.Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); public static final edu.wpi.first.math.Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 3492); } diff --git a/src/main/java/frc/robot/states/CandleState.java b/src/main/java/frc/robot/states/CandleState.java index 8644efa..0db5a8d 100644 --- a/src/main/java/frc/robot/states/CandleState.java +++ b/src/main/java/frc/robot/states/CandleState.java @@ -4,7 +4,7 @@ package frc.robot.states; -/** Add your docs here. */ +/** The state the Candle is in */ public enum CandleState { IDLE, DISABLED, diff --git a/src/main/java/frc/robot/states/HopperState.java b/src/main/java/frc/robot/states/HopperState.java index 6a5a182..76b08e1 100644 --- a/src/main/java/frc/robot/states/HopperState.java +++ b/src/main/java/frc/robot/states/HopperState.java @@ -1,13 +1,11 @@ package frc.robot.states; -import frc.robot.constants.HopperConstants; - -/* The state the hopepr is in */ +/** The state the Hopper is in */ public enum HopperState { IDLE(0), DISABLED(0), - ROLLING(HopperConstants.ROLL_SPEED), - BACKROLLING(HopperConstants.BACKROLL_SPEED); + ROLLING(1), + BACKROLLING(-1); private final double speed; diff --git a/src/main/java/frc/robot/states/IntakeState.java b/src/main/java/frc/robot/states/IntakeState.java index 8166198..ca07a35 100644 --- a/src/main/java/frc/robot/states/IntakeState.java +++ b/src/main/java/frc/robot/states/IntakeState.java @@ -1,18 +1,27 @@ package frc.robot.states; +/** The state the Intake is in */ public enum IntakeState { - IDLE(0), - DISABLED(0), - INTAKING(0), - OUTTAKING(0); + IDLE(0, 0), + DISABLED(0, 0), + INTAKING(0.75, 190), + OUTTAKING(-0.75, 190), + SHOOTING(0, 90), + HOME(0, 0); private double speed; + private double angle; - private IntakeState(double speed){ + private IntakeState(double speed, double angle){ this.speed = speed; + this.angle = angle; } public double getSpeed(){ return speed; } + + public double getAngle() { + return angle; + } } diff --git a/src/main/java/frc/robot/states/KickerState.java b/src/main/java/frc/robot/states/KickerState.java index 5dc6533..05a2948 100644 --- a/src/main/java/frc/robot/states/KickerState.java +++ b/src/main/java/frc/robot/states/KickerState.java @@ -1,12 +1,10 @@ package frc.robot.states; -import frc.robot.constants.KickerConstants; - -/** The state that the kicker is in. */ +/** The state that the Kicker is in. */ public enum KickerState { IDLE(0), DISABLED(0), - ROLLING(KickerConstants.KICKER_SPEED); + ROLLING(0.75); private final double speed; diff --git a/src/main/java/frc/robot/states/PivotState.java b/src/main/java/frc/robot/states/PivotState.java deleted file mode 100644 index e06a781..0000000 --- a/src/main/java/frc/robot/states/PivotState.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.states; - -public enum PivotState { - HOME(0), - INTAKE(0); - - private double position; - - private PivotState(double position){ - this.position = position; - } - - public double getPosition(){ - return position; - } -} diff --git a/src/main/java/frc/robot/states/ShooterState.java b/src/main/java/frc/robot/states/ShooterState.java index 3a2a1c1..b30e9a7 100644 --- a/src/main/java/frc/robot/states/ShooterState.java +++ b/src/main/java/frc/robot/states/ShooterState.java @@ -4,14 +4,14 @@ package frc.robot.states; -import frc.robot.constants.ShooterConstants; +import frc.robot.util.Hub.Targets; -/** The state that the shooter is in. */ +/** The state that the Shooter is in. */ public enum ShooterState { IDLE(0), DISABLED(0), - CHARGING(ShooterConstants.SHOOTER_RPM), - SHOOTING(ShooterConstants.SHOOTER_RPM); + CHARGING(3500), + SHOOTING(3500); private final double rpm; ShooterState(double rpm) { @@ -22,4 +22,22 @@ public enum ShooterState { public double getRPM() { return rpm; } + + public double getZoneRPM(Targets zone) { + if (zone == null) { + return 0; + } + switch (zone) { + case ZONE1: + return 1; + case ZONE2: + return 2; + case ZONE3: + return 3; + case ZONE4: + return 4; + default: + return 0; + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/states/TurretState.java b/src/main/java/frc/robot/states/TurretState.java index a8cc0bd..99625c8 100644 --- a/src/main/java/frc/robot/states/TurretState.java +++ b/src/main/java/frc/robot/states/TurretState.java @@ -1,5 +1,6 @@ package frc.robot.states; +/** The state that the Turret is in. */ public enum TurretState { DISABLED(0), IDLE(0), diff --git a/src/main/java/frc/robot/subsystems/Candle.java b/src/main/java/frc/robot/subsystems/Candle.java index f14b119..7e17d5e 100644 --- a/src/main/java/frc/robot/subsystems/Candle.java +++ b/src/main/java/frc/robot/subsystems/Candle.java @@ -20,7 +20,7 @@ public class Candle extends PARTsCandle { private Set candleStates = new HashSet<>(); public Candle() { - super("Candle", CandleConstants.CAN_ID, CandleConstants.LED_LENGTH); + super("Candle", CandleConstants.CAN_ID, CandleConstants.LED_LENGTH, CandleConstants.CAN_BUS_NAME); } /*---------------------------------- Custom Public Functions ----------------------------------*/ @@ -37,12 +37,12 @@ public void removeState(CandleState state) { } public Command commandAddState(CandleState state) { - return PARTsCommandUtils.setCommandName("commandAddState", + return PARTsCommandUtils.setCommandName("Candle.commandAddState", Commands.runOnce(() -> addState(state)).ignoringDisable(true)); } public Command commandRemoveState(CandleState state) { - return PARTsCommandUtils.setCommandName("commandRemoveState", + return PARTsCommandUtils.setCommandName("Candle.commandRemoveState", Commands.runOnce(() -> removeState(state)).ignoringDisable(true)); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java index 871e1fc..cadb875 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain/PARTsDrivetrain.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.Drivetrain; -import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -24,7 +23,6 @@ import java.io.IOException; import java.util.Arrays; import java.util.HashSet; -import java.util.function.BiConsumer; import java.util.function.BiFunction; import java.util.function.BooleanSupplier; import java.util.function.Consumer; @@ -37,7 +35,6 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -45,9 +42,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -161,8 +155,6 @@ private void instantiate() { @Override public void outputTelemetry() { partsNT.putBoolean("Fine Grain Drive", fineGrainDrive); - Targets zone = Hub.getZone(getPose()); - partsNT.putString("Zone", zone == null ? "No zone" : zone.toString()); partsNT.putDouble("HUB X coordinate", new PARTsUnit(Field.getAllianceHubPose().getX(), PARTsUnitType.Meter) .to(PARTsUnitType.Inch)); @@ -233,7 +225,7 @@ public void toggleFineGrainDrive() { public Command commandJoystickDrive(PARTsCommandController controller) { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. - return PARTsCommandUtils.setCommandName("commandJoystickDrive", applyRequest(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandJoystickDrive", applyRequest(() -> { double limit = MaxSpeed * frc.robot.constants.DrivetrainConstants.SPEED_PERCENT; if (fineGrainDrive) limit *= 0.25; @@ -257,18 +249,18 @@ public Command commandJoystickDrive(PARTsCommandController controller) { } public Command commandBrake() { - return PARTsCommandUtils.setCommandName("commandBrake", + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandBrake", applyRequest(() -> getBrakeDriveRequest())); } public Command commandPointWheels(PARTsCommandController controller) { - return PARTsCommandUtils.setCommandName("commandPointWheels", applyRequest(() -> getPointDriveRequest() + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPointWheels", applyRequest(() -> getPointDriveRequest() .withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX())))); } public Command commandSeedFieldCentric() { - return PARTsCommandUtils.setCommandName("commandSeedFieldCentric", + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandSeedFieldCentric", this.runOnce(() -> super.seedFieldCentric())); } @@ -365,8 +357,7 @@ public Command commandAlign(Supplier goalPose) { () -> ((xRangeController.atGoal() && yRangeController.atGoal() && thetaController.atGoal()) || timerElapsed)); - c.setName("align"); - return c; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandAlign", c); } public Pose2d getPose() { @@ -395,7 +386,7 @@ public Command commandSnapToAngle(double angle) { .withVelocityY(0) .withRotationalRate(thetaOutput.getRadians())); }, (Boolean b) -> stop(), () -> thetaController.atGoal(), this); - return c.withName("drivetrain.commandSnapToAngle"); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandSnapToAngle", c); } public void setChassisSpeeds(ChassisSpeeds robotSpeeds) { @@ -436,8 +427,7 @@ public Command commandPathFindToPath(String pathname) { Command pathfindingCommand = AutoBuilder.pathfindThenFollowPath( path, constraints); - pathfindingCommand.setName("pathFindToPathCommand"); - return pathfindingCommand; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPath", pathfindingCommand); } catch (IOException e) { e.printStackTrace(); @@ -448,7 +438,7 @@ public Command commandPathFindToPath(String pathname) { // TODO Auto-generated catch block e.printStackTrace(); } - return new WaitCommand(0); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPath.wait", new WaitCommand(0)); } public Command commandPathFindToPose(Pose2d pose) { @@ -464,14 +454,12 @@ public Command commandPathFindToPose(Pose2d pose) { Command pathfindingCommand = AutoBuilder.pathfindToPose( Field.conditionallyTransformToOppositeAlliance(pose), constraints, 0.0); // Goal end velocity in meters/sec - pathfindingCommand.setName("pathFindToPoseCommand"); - - return pathfindingCommand; + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathFindToPose", pathfindingCommand); } public Command commandPathOnTheFly(Pose2d pose) { - return PARTsCommandUtils.setCommandName("commandPathOnTheFly", Commands.defer(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.commandPathOnTheFly", Commands.defer(() -> { PathConstraints constraints = new PathConstraints(2, 2, 2 * Math.PI, 4 * Math.PI); // The // constraints // for this path. @@ -517,7 +505,7 @@ public Command commandPathOnTheFly(Pose2d pose) { * @return the command */ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier condition) { - return Commands.run(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.controlledRotateCommand", Commands.run(() -> { if (!isControlledRotationEnabled) { thetaController.reset(getPose().getRotation().getRadians()); } @@ -526,7 +514,7 @@ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier con thetaController.setGoal(angle.getAsDouble() + Math.PI); else thetaController.setGoal(angle.getAsDouble()); - }).until(condition).andThen(disableControlledRotation()).withName("drivetrain.controlledRotate"); + }).until(condition).andThen(disableControlledRotation())); } /** @@ -537,17 +525,17 @@ public Command controlledRotateCommand(DoubleSupplier angle, BooleanSupplier con * @return the command */ public Command targetPoseCommand(Supplier targetPose, BooleanSupplier condition) { - return controlledRotateCommand(() -> { + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.targetPoseCommand", controlledRotateCommand(() -> { Pose2d target = targetPose.get(); Transform2d diff = getPose().minus(target); Rotation2d rot = new Rotation2d(diff.getX(), diff.getY()); rot = rot.plus(Rotation2d.kPi); return rot.getRadians(); - }, condition).withName("drivetrain.targetPose"); + }, condition)); } public Command disableControlledRotation() { - return Commands.runOnce(() -> isControlledRotationEnabled = false); + return PARTsCommandUtils.setCommandName("PARTsDrivetrain.disableControlledRotation", Commands.runOnce(() -> isControlledRotationEnabled = false)); } public Consumer> consumerSetVisionMeasurementStdDevs() { @@ -565,17 +553,20 @@ public Consumer consumerResetPose() { public Supplier supplierGetPose() { return this::getPose; } - //endregion + - public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { + public boolean acceptVisionMeasurement(Pose2d measurement, double timestamp) { + // accept values rotating less than + // 2*pi rad/s = 360 deg/s if (Math.max(Math.abs(getXAngularVelocity()), Math.abs(getYAngularVelocity())) < 2 * Math.PI) { super.addVisionMeasurement(measurement, timestamp); return true; } return false; } + //endregion - /*---------------------------------- Custom Private Functions ---------------------------------*/ + //region Custom Private Functions private void alignCommandInitTelemetry(Pose2d holdDist) { partsNT.putDouble("align/holdDistX", new PARTsUnit(holdDist.getX(), PARTsUnitType.Meter) .to(PARTsUnitType.Inch)); diff --git a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java index 9863277..129aea2 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper/Hopper.java @@ -47,14 +47,8 @@ public void periodic() { else { switch(hopperstate) { case DISABLED: - setSpeed(hopperstate.getSpeed()); - break; case ROLLING: - setSpeed(hopperstate.getSpeed()); - break; case IDLE: - setSpeed(hopperstate.getSpeed()); - break; case BACKROLLING: setSpeed(hopperstate.getSpeed()); break; @@ -75,19 +69,19 @@ public void periodic() { public HopperState getState() { return hopperstate; } public Command roll() { - return PARTsCommandUtils.setCommandName("Command Roll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.roll", this.runOnce(() -> { hopperstate = HopperState.ROLLING; })); } public Command backRoll() { - return PARTsCommandUtils.setCommandName("Command BackRoll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.backRoll", this.runOnce(() -> { hopperstate = HopperState.BACKROLLING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Hopper Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Hopper.idle", this.runOnce(() -> { hopperstate = HopperState.IDLE; })); } diff --git a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java index 5813899..a43b151 100644 --- a/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java +++ b/src/main/java/frc/robot/subsystems/Hopper/HopperPhys.java @@ -3,13 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.HopperConstants; @@ -20,8 +14,9 @@ public HopperPhys() { super(); TalonFXConfiguration hopperConfig = new TalonFXConfiguration(); hopperConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - hopperMotor = new TalonFX(HopperConstants.HOPPER_MOTOR_ID); + hopperMotor = new TalonFX(HopperConstants.HOPPER_MOTOR_ID, HopperConstants.CAN_BUS_NAME); hopperMotor.getConfigurator().apply(hopperConfig); + hopperMotor.setNeutralMode(NeutralModeValue.Brake); } @Override @@ -30,11 +25,6 @@ public void outputTelemetry() { partsNT.putDouble("Current", hopperMotor.getSupplyCurrent().getValueAsDouble()); } - @Override - protected void setSpeed(double speed) { - hopperMotor.set(speed); - } - @Override public void periodic() { super.periodic(); @@ -45,4 +35,9 @@ public void log() { partsLogger.logDouble("Output", hopperMotor.getStatorCurrent().getValueAsDouble()); partsLogger.logDouble("Current", hopperMotor.getSupplyCurrent().getValueAsDouble()); } + + @Override + protected void setSpeed(double speed) { + hopperMotor.set(speed); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 42da017..3a674b9 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -2,14 +2,22 @@ import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; +import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.IntakeConstants; import frc.robot.constants.RobotConstants; import frc.robot.states.IntakeState; public abstract class Intake extends PARTsSubsystem { - IntakeState state = IntakeState.IDLE; + IntakeState intakeState = IntakeState.IDLE; + + PIDController intakePIDController; + SimpleMotorFeedforward intakeFeedForward; public Intake() { super("Intake"); @@ -18,23 +26,28 @@ public Intake() { partsNT.putDouble("Intake Speed", 0); partsNT.putDouble("Pivot Speed", 0); } - } + intakePIDController = new PIDController(IntakeConstants.P, IntakeConstants.I, IntakeConstants.D); + intakeFeedForward = new SimpleMotorFeedforward(IntakeConstants.S, IntakeConstants.V, IntakeConstants.A); + intakePIDController.setTolerance(IntakeConstants.PID_THRESHOLD); + + } + //region Generic Subsystem Functions @Override public void outputTelemetry() { - partsNT.putDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsNT.putDouble("Pivot Position", getPivotAngle().to(PARTsUnitType.Angle)); partsNT.putDouble("Current Intake Speed", getIntakeSpeed()); - partsNT.putString("Intake State", state.toString()); + partsNT.putString("Intake State", intakeState.toString()); } @Override public void stop() { - state = IntakeState.DISABLED; + intakeState = IntakeState.DISABLED; } @Override public void reset() { - state = IntakeState.IDLE; + intakeState = IntakeState.IDLE; } @Override @@ -42,23 +55,77 @@ public void periodic() { if (RobotConstants.DEBUGGING) { setIntakeSpeed(partsNT.getDouble("Intake Speed")); setPivotSpeed(partsNT.getDouble("Pivot Speed")); + } else { + switch (intakeState) { + case DISABLED: + case IDLE: + setIntakeSpeed(intakeState.getSpeed()); + setPivotSpeed(0); + break; + case INTAKING: + case OUTTAKING: + case SHOOTING: + case HOME: + setIntakeSpeed(intakeState.getSpeed()); + intakePIDController.setSetpoint(intakeState.getAngle()); + double pidCalc = intakePIDController.calculate(getPivotAngle().to(PARTsUnitType.Angle), intakeState.getAngle()); + double ffCalc = intakeFeedForward.calculate(intakePIDController.getSetpoint()); + + partsNT.putBoolean("At goal", intakePIDController.atSetpoint()); + + setPivotVoltage(pidCalc); + break; + default: + setIntakeSpeed(0); + setPivotSpeed(0); + break; + } } } @Override public void log() { - partsLogger.logDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle)); + partsLogger.logDouble("Pivot Position", getPivotAngle().to(PARTsUnitType.Angle)); partsLogger.logDouble("Intake Speed", getIntakeSpeed()); - partsLogger.logString("Intake State", state.toString()); + partsLogger.logString("Intake State", intakeState.toString()); } + //endregion + //region Custom Public Functions public abstract void setIntakeSpeed(double speed); public abstract void setPivotSpeed(double speed); - public abstract void setPivotPosition(PARTsUnit position); - public abstract double getIntakeSpeed(); - public abstract PARTsUnit getPivotPosition(); + public abstract PARTsUnit getPivotAngle(); + + public abstract void setPivotVoltage(double voltage); + + public abstract double getPivotRotationSpeed(); + + public Command intake() { + return PARTsCommandUtils.setCommandName("Intake.intake", this.runOnce(() -> { + intakeState = IntakeState.INTAKING; + })); + } + + public Command intakeShooting() { + return PARTsCommandUtils.setCommandName("Intake.intakeShooting", this.runOnce(() -> { + intakeState = IntakeState.SHOOTING; + })); + } + + public Command intakeIdle() { + return PARTsCommandUtils.setCommandName("Intake.intakeIdle", this.runOnce(() -> { + intakeState = IntakeState.IDLE; + })); + } + + public Command home() { + return PARTsCommandUtils.setCommandName("Intake.home", this.runOnce(() -> { + intakeState = IntakeState.HOME; + })); + } + //endregion } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java index 9827ede..b1a34b0 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakePhys.java @@ -1,50 +1,37 @@ package frc.robot.subsystems.Intake; -import static edu.wpi.first.units.Units.Rotations; - import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; -import org.parts3492.partslib.network.PARTsNT; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; - -import edu.wpi.first.wpilibj.motorcontrol.Talon; +import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.IntakeConstants; +/** + * The physical intake subsystem.

+ * WARNING: The pivot arm MUST be in home position when the robot starts. + */ public class IntakePhys extends Intake { - TalonFX intakeMotor; - TalonFX pivotMotor; + protected final TalonFX intakeMotor; + protected final TalonFX pivotMotor; public IntakePhys() { super(); TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); - intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); + intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; intakeMotor.getConfigurator().apply(intakeConfig); - pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID); - } - - @Override - public void setIntakeSpeed(double speed) { - intakeMotor.set(speed); - } - - @Override - public void setPivotPosition(PARTsUnit position) { - pivotMotor.setPosition(position.to(PARTsUnitType.Rotations)); - } - - @Override - public double getIntakeSpeed() { - return intakeMotor.get(); - } - - @Override - public PARTsUnit getPivotPosition() { - return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble(), PARTsUnitType.Rotations); + intakeMotor.setNeutralMode(NeutralModeValue.Brake); + + pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID, IntakeConstants.CAN_BUS_NAME); + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotMotor.getConfigurator().apply(pivotConfig); + pivotMotor.getConfigurator().setPosition(0); + pivotMotor.setNeutralMode(NeutralModeValue.Brake); + } @Override @@ -83,4 +70,29 @@ public void log() { public void setPivotSpeed(double speed) { pivotMotor.set(speed); } + + @Override + public void setPivotVoltage(double voltage) { + pivotMotor.setVoltage(voltage); + } + + @Override + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public double getIntakeSpeed() { + return intakeMotor.get(); + } + + @Override + public double getPivotRotationSpeed() { + return pivotMotor.getVelocity().getValueAsDouble(); + } + + @Override + public PARTsUnit getPivotAngle() { + return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble() / IntakeConstants.PIVOT_GEAR_RATIO, PARTsUnitType.Rotations); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java index 2f7ae9f..3c31b3e 100644 --- a/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeSim.java @@ -13,18 +13,22 @@ public void setIntakeSpeed(double speed) { public void setPivotSpeed(double speed) { } - @Override - public void setPivotPosition(PARTsUnit position) { - } - @Override public double getIntakeSpeed() { return 0; } @Override - public PARTsUnit getPivotPosition() { + public PARTsUnit getPivotAngle() { return new PARTsUnit(0, PARTsUnitType.Angle); } - + + @Override + public void setPivotVoltage(double voltage) { + } + + @Override + public double getPivotRotationSpeed() { + return 0; + } } diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java b/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java new file mode 100644 index 0000000..4719fc2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeSysid.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.Intake; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import org.parts3492.partslib.PARTsUnit.PARTsUnitType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class IntakeSysid extends IntakePhys { + + private MutVoltage appliedVoltage; + + private MutAngle pivotAngle; + + private MutAngularVelocity pivotVelocity; + + private SysIdRoutine routine; + + public IntakeSysid() { + super(); + + appliedVoltage = Volts.mutable(0); + + pivotAngle = Units.Radian.mutable(0); + + pivotVelocity = Units.RadiansPerSecond.mutable(0); + + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setPivotVoltage(voltage.in(Volts)), + + log -> { + // Record a frame for the shooter motor. + log.motor("pivotarm") + .voltage( + appliedVoltage.mut_replace( + super.pivotMotor.getStatorCurrent().getValueAsDouble(), Volts)) + .angularPosition(pivotAngle.mut_replace( + getPivotAngle().toPARTsUnit(PARTsUnitType.Angle).to(PARTsUnitType.Radian), Radians)) + .angularVelocity( + pivotVelocity.mut_replace(getPivotRotationSpeed(), RotationsPerSecond)); + }, + this)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} diff --git a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java index c3f9e7b..f76f4b6 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker/Kicker.java @@ -1,12 +1,10 @@ package frc.robot.subsystems.Kicker; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; -import frc.robot.constants.ShooterConstants; import frc.robot.states.KickerState; public abstract class Kicker extends PARTsSubsystem { @@ -41,7 +39,6 @@ public void reset() { public void log() { partsLogger.logString("Kicker State", kickerState.toString()); } - // endregion @Override public void periodic() { @@ -50,11 +47,7 @@ public void periodic() { } else { switch (kickerState) { case ROLLING: - setSpeed(kickerState.getSpeed()); - break; case DISABLED: - setSpeed(kickerState.getSpeed()); - break; case IDLE: setSpeed(kickerState.getSpeed()); break; @@ -64,6 +57,7 @@ public void periodic() { } } } + //endregion // region Custom Public Functions /** @@ -78,15 +72,15 @@ public KickerState getState() { } public Command roll() { - return PARTsCommandUtils.setCommandName("Command Roll", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.roll", this.runOnce(() -> { kickerState = KickerState.ROLLING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.idle", this.runOnce(() -> { kickerState = KickerState.IDLE; })); } + //endregion } -// endregion diff --git a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java index 501b4b7..c2916f0 100644 --- a/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java +++ b/src/main/java/frc/robot/subsystems/Kicker/KickerPhys.java @@ -3,13 +3,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.signals.NeutralModeValue; import frc.robot.constants.KickerConstants; public class KickerPhys extends Kicker { @@ -19,8 +13,9 @@ public KickerPhys() { super(); TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerMotor = new TalonFX(KickerConstants.KICKER_MOTOR_ID); + kickerMotor = new TalonFX(KickerConstants.KICKER_MOTOR_ID, KickerConstants.CAN_BUS_NAME); kickerMotor.getConfigurator().apply(config); + kickerMotor.setNeutralMode(NeutralModeValue.Coast); } @Override @@ -30,11 +25,6 @@ public void outputTelemetry() { partsNT.putDouble("KickerOutput", kickerMotor.getStatorCurrent().getValueAsDouble()); } - @Override - protected void setSpeed(double speed) { - kickerMotor.set(speed); - } - @Override public void periodic() { super.periodic(); @@ -46,4 +36,9 @@ public void log() { partsLogger.logDouble("KickerOutput", kickerMotor.getStatorCurrent().getValueAsDouble()); } + + @Override + protected void setSpeed(double speed) { + kickerMotor.set(speed); + } } diff --git a/src/main/java/frc/robot/subsystems/LimelightVision.java b/src/main/java/frc/robot/subsystems/LimelightVision.java index 84eeea7..abfa730 100644 --- a/src/main/java/frc/robot/subsystems/LimelightVision.java +++ b/src/main/java/frc/robot/subsystems/LimelightVision.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import java.util.function.BiConsumer; import java.util.function.BiFunction; import java.util.function.Consumer; import java.util.function.Supplier; @@ -101,7 +100,7 @@ public void setMegaTagMode(MegaTagMode mode) { } public Command commandMegaTagMode(MegaTagMode mode) { - Command c = PARTsCommandUtils.setCommandName("commandMegaTagMode", this.runOnce(() -> setMegaTagMode(mode))); + Command c = PARTsCommandUtils.setCommandName("LimelightVision.commandMegaTagMode", this.runOnce(() -> setMegaTagMode(mode))); c = c.ignoringDisable(true); return c; } @@ -205,6 +204,8 @@ public void periodic() { 0, 0, 0); + double [] hw = LimelightHelpers.getLimelightDoubleArrayEntry("limelight", "hw").get(); + partsNT.putDouble(camera.getName() + "/temp", hw.length > 0 ? hw [0]: -1); if (camera.isEnabled()) { PoseEstimate poseEstimate = (megaTagMode == MegaTagMode.MEGATAG2) ? getMegaTag2PoseEstimate(camera.getName()) @@ -255,7 +256,7 @@ public void log() { // throw new UnsupportedOperationException("Unimplemented method 'log'"); } - public void resetRobotPose() { + /*public void resetRobotPose() { setMegaTagMode(MegaTagMode.MEGATAG1); for (Camera camera : CameraConstants.LimelightCameras) { LimelightHelpers.SetRobotOrientation( @@ -283,7 +284,7 @@ public void resetRobotPose() { } } setMegaTagMode(MegaTagMode.MEGATAG2); - } + }*/ public void setPipelineIndex(Pipelines pipeline) { partsNT.putString("Pipeline name", pipeline.name()); diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index f1e30f1..6df7da8 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -2,24 +2,30 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; import frc.robot.constants.ShooterConstants; import frc.robot.states.ShooterState; +import frc.robot.util.Hub; +import frc.robot.util.Hub.Targets; + +import java.util.function.Supplier; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; -import frc.robot.states.ShooterState; import org.parts3492.partslib.command.PARTsSubsystem; -public abstract class Shooter extends PARTsSubsystem{ +public abstract class Shooter extends PARTsSubsystem { private ShooterState shooterState = ShooterState.IDLE; private PIDController shooterPIDController; private SimpleMotorFeedforward shooterFeedforward; + private Pose2d poseSupplier; - public Shooter() { + public Shooter(Supplier poseSupplier) { super("Shooter", RobotConstants.LOGGING); + this.poseSupplier = poseSupplier.get(); if (RobotConstants.DEBUGGING) { partsNT.putDouble("Shooter Speed", 0); } @@ -30,7 +36,7 @@ public Shooter() { shooterPIDController.setTolerance(ShooterConstants.PID_THRESHOLD); } - //region Generic Subsystem Functions + // region Generic Subsystem Functions @Override public void outputTelemetry() { partsNT.putString("Shooter State", shooterState.toString()); @@ -39,6 +45,9 @@ public void outputTelemetry() { partsNT.putDouble("Get Setpoint", shooterPIDController.getSetpoint()); partsNT.putBoolean("At Setpoint", shooterPIDController.atSetpoint()); partsNT.putDouble("Current Error", shooterPIDController.getError()); + + Targets zone = Hub.getZone(poseSupplier); + partsNT.putString("Zone", zone == null ? "No zone" : zone.toString()); } @Override @@ -56,31 +65,44 @@ public void log() { partsLogger.logString("Shooter State", shooterState.toString()); } - - @Override public void periodic() { if (RobotConstants.DEBUGGING) { setSpeed(partsNT.getDouble("Shooter Speed")); - } - else { - double voltage = 0; - shooterPIDController.setSetpoint(shooterState.getRPM()); - - double pidCalc = shooterPIDController.calculate(getRPM(), shooterState.getRPM()); - double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); - - voltage = pidCalc + ffCalc; - - setVoltage(voltage); + } else { + switch (shooterState) { + case CHARGING: + case DISABLED: + case IDLE: + case SHOOTING: + double voltage = 0; + Targets zone = Hub.getZone(poseSupplier); + double zoneRPM = shooterState.getZoneRPM(zone); + double shooterRPM = shooterState.getRPM(); + shooterPIDController.setSetpoint(shooterRPM); + + double pidCalc = shooterPIDController.calculate(getRPM(), shooterRPM); + double ffCalc = shooterFeedforward.calculate((shooterPIDController.getSetpoint() * Math.PI + * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Meter) * 2) / 60); + + voltage = pidCalc + ffCalc; + + setVoltage(voltage); + break; + default: + setSpeed(0); + break; + } } } - //endregion + // endregion - //region Custom Public Functions - /** Sets the speed of the Shooter. + // region Custom Public Functions + /** + * Sets the speed of the Shooter. + * * @param speed The speed between -1.0 and 1.0. - */ + */ protected abstract void setSpeed(double speed); protected abstract void setVoltage(double voltage); @@ -89,18 +111,20 @@ public void periodic() { protected abstract double getRPM(); - public ShooterState getState() { return shooterState; } + public ShooterState getState() { + return shooterState; + } public Command shoot() { - return PARTsCommandUtils.setCommandName("Command Shoot", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.shoot", this.runOnce(() -> { shooterState = ShooterState.SHOOTING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Shooter Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Kicker.idle", this.runOnce(() -> { shooterState = ShooterState.IDLE; })); } - //endregion + // endregion } diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java index e57bcf9..a3e5e96 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterPhys.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.Shooter; +import java.util.function.Supplier; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; @@ -7,34 +9,50 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Pose2d; import frc.robot.constants.ShooterConstants; public class ShooterPhys extends Shooter { protected final TalonFX leftMotor; - //protected final TalonFX rightMotor; + protected final TalonFX rightMotor; - public ShooterPhys() { - super(); + public ShooterPhys(Supplier poseSupplier) { + super(poseSupplier); TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID); + leftMotor = new TalonFX(ShooterConstants.LEFT_MOTOR_ID, ShooterConstants.CAN_BUS_NAME); leftMotor.getConfigurator().apply(config); - //rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID); + rightMotor = new TalonFX(ShooterConstants.RIGHT_MOTOR_ID, ShooterConstants.CAN_BUS_NAME); - //rightMotor.setControl(new Follower(ShooterConstants.LEFT_MOTOR_ID, MotorAlignmentValue.Opposed)); + rightMotor.setControl(new Follower(ShooterConstants.LEFT_MOTOR_ID, MotorAlignmentValue.Opposed)); leftMotor.setNeutralMode(NeutralModeValue.Coast); - //rightMotor.setNeutralMode(NeutralModeValue.Coast); + rightMotor.setNeutralMode(NeutralModeValue.Coast); } @Override public void outputTelemetry() { super.outputTelemetry(); partsNT.putDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); - //partsNT.putDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); + partsNT.putDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); partsNT.putDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); - //partsNT.putDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); + partsNT.putDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); + } + + @Override + public void periodic() { + super.periodic(); + } + + @Override + public void log() { + super.log(); + partsLogger.logDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); + partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); + + partsLogger.logDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); + partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); } @Override @@ -44,7 +62,7 @@ protected void setSpeed(double speed) { @Override protected double getRPM() { - return leftMotor.getVelocity().getValueAsDouble() / 60; + return leftMotor.getVelocity().getValueAsDouble() * 60; } @Override @@ -56,19 +74,4 @@ protected void setVoltage(double voltage) { protected double getVoltage() { return leftMotor.getSupplyVoltage().getValueAsDouble(); } - - @Override - public void periodic() { - super.periodic(); - } - - @Override - public void log() { - super.log(); - partsLogger.logDouble("Current/Left", leftMotor.getSupplyCurrent().getValueAsDouble()); - //partsLogger.logDouble("Current/Right", rightMotor.getSupplyCurrent().getValueAsDouble()); - - partsLogger.logDouble("Output/Left", leftMotor.getStatorCurrent().getValueAsDouble()); - //partsLogger.logDouble("Output/Right", rightMotor.getStatorCurrent().getValueAsDouble()); - } } diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java index b3b2683..2e2b441 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSim.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.Shooter; +import java.util.function.Supplier; + import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import com.revrobotics.PersistMode; @@ -11,6 +13,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -29,8 +32,8 @@ public class ShooterSim extends Shooter { SparkRelativeEncoderSim motorEncoder; FlywheelSim shooterSim; - public ShooterSim() { - super(); + public ShooterSim(Supplier poseSupplier) { + super(poseSupplier); maxGearbox = DCMotor.getNEO(2); SparkMaxConfig shooterConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java index efa977e..a3afbb7 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterSysid.java @@ -2,12 +2,13 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; -import edu.wpi.first.units.measure.MutVelocity; import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; @@ -26,8 +27,8 @@ public class ShooterSysid extends ShooterPhys { private SysIdRoutine routine; - public ShooterSysid() { - super(); + public ShooterSysid(Supplier poseSupplier) { + super(poseSupplier); appliedVoltage = Volts.mutable(0); @@ -42,7 +43,7 @@ public ShooterSysid() { (log) -> { log.motor("shootermotor1") .voltage(appliedVoltage.mut_replace( - super.leftMotor.getStatorCurrent().getValueAsDouble(), Volts)) + super.leftMotor.getMotorVoltage().getValueAsDouble(), Volts)) .linearPosition(shooterPosition.mut_replace( super.leftMotor.getPosition().getValueAsDouble() * Math.PI * ShooterConstants.SHOOTER_WHEEL_RADIUS.to(PARTsUnitType.Inch) * 2, Inches)) .linearVelocity(shooterVelocity.mut_replace( diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..2ad0536 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems; + +import java.util.function.BooleanSupplier; + +import org.parts3492.partslib.command.PARTsCommandUtils; +import org.parts3492.partslib.command.PARTsSubsystem; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import frc.robot.subsystems.Hopper.Hopper; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Kicker.Kicker; +import frc.robot.subsystems.Shooter.Shooter; +import frc.robot.subsystems.Turret.Turret; + +public class Superstructure extends PARTsSubsystem { + private final Hopper hopper; + private final Intake intake; + private final Kicker kicker; + private final Shooter shooter; + private final Turret turret; + + public Superstructure(Hopper hopper, Intake intake, Kicker kicker, Shooter shooter, Turret turret) { + this.hopper = hopper; + this.intake = intake; + this.kicker = kicker; + this.shooter = shooter; + this.turret = turret; + } + + /** + * lift up pivot arm, roll hopper, roll kicker, shoot. Only happens if turret has valid angle + */ + public Command shoot(BooleanSupplier end) { + return PARTsCommandUtils + .setCommandName("Superstructure.shoot", + Commands.parallel(turret.track(), intake.intakeShooting(), hopper.roll(), kicker.roll(), + shooter.shoot()).onlyIf(turret::isValidAngle)) + .andThen(new WaitUntilCommand(() -> !turret.isValidAngle() || end.getAsBoolean())) + .andThen(Commands.runOnce(() -> { + turret.reset(); + intake.reset(); + hopper.reset(); + kicker.reset(); + shooter.reset(); + }, hopper, intake, kicker, shooter, turret)); + } + + @Override + public void outputTelemetry() { + } + + @Override + public void stop() { + } + + @Override + public void reset() { + } + + @Override + public void log() { + } +} diff --git a/src/main/java/frc/robot/subsystems/Turret/Turret.java b/src/main/java/frc/robot/subsystems/Turret/Turret.java index d5cc1a3..ac7560f 100644 --- a/src/main/java/frc/robot/subsystems/Turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret/Turret.java @@ -3,18 +3,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.RobotConstants; import frc.robot.constants.TurretConstants; import frc.robot.states.TurretState; import frc.robot.util.Field; -import frc.robot.util.Hub; import java.util.function.Supplier; -import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.command.PARTsCommandUtils; import org.parts3492.partslib.command.PARTsSubsystem; @@ -27,8 +23,8 @@ public abstract class Turret extends PARTsSubsystem { public Turret(Supplier robotPoseSupplier) { super("Turret", RobotConstants.LOGGING); - if (RobotConstants.DEBUGGING) { - partsNT.putDouble("Turret Speed", 0); + if (RobotConstants.DEBUGGING) { + partsNT.putDouble("Turret Speed", 0); } this.robotPoseSupplier = robotPoseSupplier; @@ -65,7 +61,6 @@ public void reset() { public void log() { partsLogger.logString("Turret State", turretState.toString()); } - // endregion @Override public void periodic() { @@ -73,16 +68,36 @@ public void periodic() { setSpeed(partsNT.getDouble("Turret Speed")); } else { double voltage = 0; - turretPIDController.setSetpoint(turretState.getAngle()); - - double pidCalc = turretPIDController.calculate(getAngle(), turretState.getAngle()); - double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); - voltage = pidCalc + ffCalc; - - setVoltage(voltage); + switch (turretState) { + case DISABLED: + case IDLE: + setSpeed(0); + break; + case TRACKING: + if (isValidAngle()) { + turretPIDController.setSetpoint(getAngleToTarget()); + double pidCalc = turretPIDController.calculate(getAngle(), getAngleToTarget()); + //double ffCalc = turretFeedforward.calculate(turretPIDController.getSetpoint()); + + partsNT.putDouble("Turret voltage", voltage); + partsNT.putBoolean("Turret at setpoint", turretPIDController.atSetpoint()); + + voltage = pidCalc; //+ ffCalc; + + setVoltage(voltage); + } + else { + setSpeed(0); + } + break; + default: + setSpeed(0); + break; + } } } + // endregion // region Custom Public Functions /** @@ -98,18 +113,22 @@ public void periodic() { protected abstract double getAngle(); + public boolean isValidAngle() { + return Math.abs(getAngleToTarget()) <= 90; + } + public TurretState getState() { return turretState; } public Command track() { - return PARTsCommandUtils.setCommandName("Command Track", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Turret.track", this.runOnce(() -> { turretState = TurretState.TRACKING; })); } public Command idle() { - return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> { + return PARTsCommandUtils.setCommandName("Turret.idle", this.runOnce(() -> { turretState = TurretState.IDLE; })); } @@ -117,25 +136,11 @@ public Command idle() { // region private functions private double getAngleToTarget() { - /* - * Pose2d targetPose = Field.getAllianceHubPose(); - * Pose2d robotPose = robotPoseSupplier.get(); - * Rotation2d angleToTarget = - * targetPose.getTranslation().minus(robotPose.getTranslation()).getAngle(); - */ double angleToTarget = edu.wpi.first.math.MathUtil .inputModulus(robotPoseSupplier.get().getRotation().getDegrees(), -180, 180) - (Math.atan2(Field.getAllianceHubPose().getY() - robotPoseSupplier.get().getY(), Field.getAllianceHubPose().getX() - robotPoseSupplier.get().getX()) * 180 / Math.PI); return angleToTarget; - - /*if (angleToTarget > 180) { - return 180 - angleToTarget; - } else if (angleToTarget < -180) { - return 360 + angleToTarget; - } else { - return angleToTarget; - }*/ } // endregion private functions } diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java index d864cdc..13aa0c9 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretPhys.java @@ -2,16 +2,11 @@ import java.util.function.Supplier; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import frc.robot.constants.TurretConstants; public class TurretPhys extends Turret { @@ -20,11 +15,11 @@ public class TurretPhys extends Turret { public TurretPhys(Supplier robotPoseSupplier) { super(robotPoseSupplier); - SparkMaxConfig turretConfig = new SparkMaxConfig(); - turretConfig.idleMode(IdleMode.kCoast); - turretConfig.inverted(true); - - turretMotor = new TalonFX(TurretConstants.TURRET_MOTOR_ID); + turretMotor = new TalonFX(TurretConstants.TURRET_MOTOR_ID, TurretConstants.CAN_BUS_NAME); + TalonFXConfiguration turretConfig = new TalonFXConfiguration(); + turretMotor.getConfigurator().apply(turretConfig); + turretMotor.getConfigurator().setPosition(0); + turretMotor.setNeutralMode(NeutralModeValue.Brake); } @Override @@ -36,35 +31,35 @@ public void outputTelemetry() { } @Override - protected void setSpeed(double speed) { - turretMotor.set(speed); + public void periodic() { + super.periodic(); } @Override - protected void setVoltage(double voltage) { - turretMotor.setVoltage(voltage); + public void log() { + super.log(); + partsLogger.logDouble("Current/Turret", turretMotor.getSupplyCurrent().getValueAsDouble()); + + partsLogger.logDouble("Output/Turret", turretMotor.getStatorCurrent().getValueAsDouble()); } @Override - protected double getVoltage() { - return turretMotor.getSupplyCurrent().getValueAsDouble(); + protected double getAngle() { + return turretMotor.getPosition().getValueAsDouble() * 360 / TurretConstants.TURRET_GEAR_RATIO; } @Override - public void periodic() { - super.periodic(); + protected void setSpeed(double speed) { + turretMotor.set(speed); } @Override - public void log() { - super.log(); - partsLogger.logDouble("Current/Turret", turretMotor.getSupplyCurrent().getValueAsDouble()); - - partsLogger.logDouble("Output/Turret", turretMotor.getStatorCurrent().getValueAsDouble()); + protected void setVoltage(double voltage) { + turretMotor.setVoltage(voltage); } @Override - protected double getAngle() { - return turretMotor.getPosition().getValueAsDouble() * 360 % 360; + protected double getVoltage() { + return turretMotor.getSupplyCurrent().getValueAsDouble(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java index fe27453..a842ef5 100644 --- a/src/main/java/frc/robot/subsystems/Turret/TurretSim.java +++ b/src/main/java/frc/robot/subsystems/Turret/TurretSim.java @@ -5,10 +5,11 @@ import edu.wpi.first.math.geometry.Pose2d; public class TurretSim extends Turret { + //protected final TalonFX turretMotor; public TurretSim(Supplier robotPoseSupplier) { super(robotPoseSupplier); -} + } @Override protected void setSpeed(double speed) { diff --git a/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java b/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java new file mode 100644 index 0000000..e8fb480 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Turret/TurretSysid.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.Turret; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class TurretSysid extends TurretPhys { + + private MutVoltage appliedVoltage; + + private MutAngle pivotAngle; + + private MutAngularVelocity pivotVelocity; + + private SysIdRoutine routine; + + public TurretSysid(Supplier robotPoseSupplier) { + super(robotPoseSupplier); + + appliedVoltage = Volts.mutable(0); + + pivotAngle = Units.Radian.mutable(0); + + pivotVelocity = Units.RadiansPerSecond.mutable(0); + + routine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setVoltage(voltage.in(Volts)), + + log -> { + // Record a frame for the shooter motor. + log.motor("turret") + .voltage( + appliedVoltage.mut_replace( + super.turretMotor.getStatorCurrent().getValueAsDouble(), Volts)) + .angularPosition(pivotAngle.mut_replace( + getAngle() * Math.PI / 180, Radians)) + .angularVelocity( + pivotVelocity.mut_replace(super.turretMotor.getVelocity().getValueAsDouble(), RotationsPerSecond)); + }, + this)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return routine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return routine.dynamic(direction); + } +} diff --git a/src/main/java/frc/robot/util/Field.java b/src/main/java/frc/robot/util/Field.java index 0ac3bf2..5ac7c9b 100644 --- a/src/main/java/frc/robot/util/Field.java +++ b/src/main/java/frc/robot/util/Field.java @@ -14,7 +14,6 @@ import java.util.List; import org.parts3492.partslib.game.AprilTag; -import org.parts3492.partslib.RobotUtils; /** This interface stores information about the field elements. */ public interface Field { @@ -146,6 +145,10 @@ public static Pose2d getAllianceHubPose() { return RobotContainer.isBlue() ? blueHubCenter : transformToOppositeAlliance(blueHubCenter); } + public static Pose2d [] getAllianceTrenchPoses() { + return RobotContainer.isBlue() ? new Pose2d [] {getTag(23).getLocation().toPose2d(), getTag(38).getLocation().toPose2d()}: new Pose2d [] {getTag(12).getLocation().toPose2d(), getTag(7).getLocation().toPose2d()}; + } + public static int[] getAllTagIDs() { int[] ids = new int[APRILTAGS.length]; diff --git a/src/main/java/frc/robot/util/Hub.java b/src/main/java/frc/robot/util/Hub.java index 3b3427a..0ecc800 100644 --- a/src/main/java/frc/robot/util/Hub.java +++ b/src/main/java/frc/robot/util/Hub.java @@ -2,6 +2,7 @@ import java.util.Optional; +import org.parts3492.partslib.PARTsUnit; import org.parts3492.partslib.PARTsUnit.PARTsUnitType; import org.parts3492.partslib.network.PARTsNT; @@ -9,7 +10,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.FieldConstants; public class Hub { private static Pose2d hubPose2d = Field.getAllianceHubPose(); @@ -18,10 +18,10 @@ public class Hub { private static PARTsNT partsNT = new PARTsNT("Hub"); public static enum Targets { - ZONE1(FieldConstants.ZONE1_RADIUS.to(PARTsUnitType.Meter)), - ZONE2(FieldConstants.ZONE2_RADIUS.to(PARTsUnitType.Meter)), - ZONE3(FieldConstants.ZONE3_RADIUS.to(PARTsUnitType.Meter)), - ZONE4(FieldConstants.ZONE4_RADIUS.to(PARTsUnitType.Meter)); + ZONE1(new PARTsUnit(4, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE2(new PARTsUnit(6, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE3(new PARTsUnit(8, PARTsUnitType.Foot).to(PARTsUnitType.Meter)), + ZONE4(new PARTsUnit(10, PARTsUnitType.Foot).to(PARTsUnitType.Meter)); private double radius; @@ -34,6 +34,12 @@ public double getRadius() { } } + public static void outputTelemetry() { + partsNT.putBoolean("Hub Active", Hub.isHubActive()); + partsNT.putDouble("Time Left", timer.get() <=25 ? 25 - timer.get() : 0); + checkHubActivity(); + } + public static boolean isInRadius(Pose2d center, Pose2d point, double radius) { double centerPoseX = center.getX(); double centerPoseY = center.getY(); @@ -42,7 +48,6 @@ public static boolean isInRadius(Pose2d center, Pose2d point, double radius) { double pointPoseY = point.getY(); double distanceSquared = (Math.pow(centerPoseX - pointPoseX, 2) + Math.pow(centerPoseY - pointPoseY, 2)); - // System.out.println(Math.sqrt(distanceSquared)); return distanceSquared <= Math.pow(radius, 2); } @@ -132,12 +137,6 @@ public static void startHubActiveTimer() { timer.start(); } - public static void outputTelemetry() { - partsNT.putBoolean("Hub Active", Hub.isHubActive()); - partsNT.putDouble("Time Left", timer.get() <=25 ? 25 - timer.get() : 0); - checkHubActivity(); - } - public static void checkHubActivity() { if (previousHubActive != Hub.isHubActive()) { timer.restart(); diff --git a/src/main/java/frc/robot/util/Trench.java b/src/main/java/frc/robot/util/Trench.java new file mode 100644 index 0000000..5c18ca5 --- /dev/null +++ b/src/main/java/frc/robot/util/Trench.java @@ -0,0 +1,35 @@ +package frc.robot.util; + +import org.parts3492.partslib.command.PARTsCommandUtils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Drivetrain.PARTsDrivetrain; + +public class Trench { + private static Pose2d goal; + + public static Command parkUnderTrench(PARTsDrivetrain drivetrain) { + return PARTsCommandUtils.setCommandName("Trench.parkUnderTrench", + Commands.runOnce(() -> goal = getNearestTrench(drivetrain.getPose(), Field.getAllianceTrenchPoses())) + .andThen(drivetrain.commandPathFindToPose(goal))); + } + + private static Pose2d getNearestTrench(Pose2d current, Pose2d[] poses) { + int index = 0; + double distance = getDistance(current, poses[0]); + for (int i = 0; i < poses.length; i++) { + double localDistance = getDistance(current, poses[i]); + if (localDistance < distance) { + distance = localDistance; + index = i; + } + } + return poses[index]; + } + + private static double getDistance(Pose2d current, Pose2d goal) { + return Math.sqrt(Math.pow(current.getX() - goal.getX(), 2) + Math.pow(current.getY() - goal.getY(), 2)); + } +}