Feature/turret gear ratio

.github/workflows/ci.sh

@@ -3,6 +3,7 @@
 # Make sure that Gradlew is executable.
 chmod +x gradlew
 
+
 # Start simulation.
 ./gradlew simulateJavaRelease &
 sim_pid=$!

.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

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;

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<IPARTsSubsystem> subsystems = new ArrayList<IPARTsSubsystem>(
-            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,37 +192,78 @@ 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() {
         autoChooser = AutoBuilder.buildAutoChooser();
         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()) {

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";
 }

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

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";
 }

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;
 }

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";
 }

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;
-}
+}

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;
 }

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;

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<N3> MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0);
     public static final edu.wpi.first.math.Vector<N3> MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 3492);
 }

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,