so teleopInit wasn't in robot.java...

Author: Ishan1522

.gitignore

@@ -1,6 +1,6 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
-*src/main/java/frc/robot/BuildConstants.java
+# *src/main/java/frc/robot/BuildConstants.java
 simgui-ds.json
 simgui.json
 networktables.json

build.gradle

@@ -151,15 +151,15 @@ tasks.withType(JavaCompile) {
 }
 
 // Create version file
-// project.compileJava.dependsOn(createVersionFile)
-// gversion {
-//     srcDir       = "src/main/java/"
-//     classPackage = "frc.robot"
-//     className    = "BuildConstants"
-//     dateFormat   = "yyyy-MM-dd HH:mm:ss z"
-//     timeZone     = "America/New_York"
-//     indent       = "  "
-// }
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc.robot"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/New_York"
+    indent       = "  "
+}
 
 // TODO: set this shit up
 // Create commit with working changes on event branches

src/main/java/frc/robot/BuildConstants.java

@@ -0,0 +1,17 @@
+package frc.robot;
+
+/** Automatically generated file containing build version information. */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "offseason-robot-code-2024-4";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 460;
+  public static final String GIT_SHA = "407af199c2fcc39f00d180058fa4bb5040ba980a";
+  public static final String GIT_DATE = "2024-12-16 16:34:08 EST";
+  public static final String GIT_BRANCH = "shooter-pivot-sim";
+  public static final String BUILD_DATE = "2024-12-20 17:10:49 EST";
+  public static final long BUILD_UNIX_TIME = 1734732649398L;
+  public static final int DIRTY = 1;
+
+  private BuildConstants() {}
+}

src/main/java/frc/robot/Constants.java

@@ -14,7 +14,7 @@ public static final class LogPaths {
     public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/";
   }
 
-  public static final Mode currentMode = Mode.SIM;
+  public static final Mode CURRENT_MODE = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */

src/main/java/frc/robot/Robot.java

@@ -40,7 +40,7 @@ public Robot() {
     }
 
     // Set up data receivers & replay source
-    switch (Constants.currentMode) {
+    switch (Constants.CURRENT_MODE) {
       case REAL:
         // Running on a real robot, log to a USB stick ("/U/logs")
         Logger.addDataReceiver(new WPILOGWriter());
@@ -113,6 +113,7 @@ public void autonomousPeriodic() {}
   /** This function is called once when teleop is enabled. */
   @Override
   public void teleopInit() {
+    m_robotContainer.teleopInit();
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove

src/main/java/frc/robot/RobotContainer.java

@@ -9,12 +9,16 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.drive.DriveCommand;
+import frc.robot.commands.pivot.ManualPivot;
 import frc.robot.extras.simulation.field.SimulatedField;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
 import frc.robot.extras.util.JoystickUtil;
+import frc.robot.subsystems.pivot.Pivot;
+import frc.robot.subsystems.pivot.PivotIOSim;
+import frc.robot.subsystems.pivot.PivotIOTalonFX;
 import frc.robot.subsystems.swerve.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
@@ -36,9 +40,10 @@ public class RobotContainer {
   private final Vision visionSubsystem;
   private final SwerveDrive swerveDrive;
   private final CommandXboxController operatorController = new CommandXboxController(1);
+  // private final Pivot pivot;
   // private final Indexer indexer = new Indexer(new IndexerIOTalonFX());
   // private final Intake intake = new Intake(new IntakeIOTalonFX());
-  // private final Pivot pivot = new Pivot(new PivotIOTalonFX());
+  private final Pivot pivot;
   // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX());
   private final CommandXboxController driverController = new CommandXboxController(0);
 
@@ -51,7 +56,7 @@ public class RobotContainer {
   // private final XboxController driverController = new XboxController(0);
 
   public RobotContainer() {
-    switch (Constants.currentMode) {
+    switch (Constants.CURRENT_MODE) {
       case REAL -> {
         /* Real robot, instantiate hardware IO implementations */
 
@@ -68,12 +73,14 @@ public RobotContainer() {
                 new PhysicalModule(SwerveConstants.moduleConfigs[2]),
                 new PhysicalModule(SwerveConstants.moduleConfigs[3]));
         visionSubsystem = new Vision(new VisionIOReal());
+        pivot = new Pivot(new PivotIOTalonFX());
       }
 
       case SIM -> {
         /* Sim robot, instantiate physics sim IO implementations */
 
         /* create simulations */
+        pivot = new Pivot(new PivotIOSim());
         /* create simulation for pigeon2 IMU (different IMUs have different measurement erros) */
         this.gyroSimulation = GyroSimulation.createNavX2();
         /* create a swerve drive simulation */
@@ -128,6 +135,7 @@ public RobotContainer() {
                 new ModuleInterface() {},
                 new ModuleInterface() {},
                 new ModuleInterface() {});
+        pivot = null;
       }
     }
   }
@@ -190,7 +198,7 @@ private void configureButtonBindings() {
     // Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2);
 
     // // manual pivot and intake rollers
-    // Trigger operatorAButton = new Trigger(operatorController::getAButton);
+    Trigger operatorAButton = new Trigger(operatorController.a());
     // Trigger operatorXButton = new Trigger(operatorController::getXButton);
     // Trigger operatorYButton = new Trigger(operatorController::getYButton);
     // DoubleSupplier operatorRightStickY = operatorController::getRightY;
@@ -202,7 +210,7 @@ private void configureButtonBindings() {
     Trigger driverLeftBumper = new Trigger(driverController.leftBumper());
     // Trigger driverBButton = new Trigger(driverController::getBButton);
     // Trigger driverYButton = new Trigger(driverController::getYButton);
-    // DoubleSupplier operatorLeftStickY = operatorController::getLeftY;
+    DoubleSupplier operatorLeftStickY = operatorController::getLeftY;
 
     // //DRIVER BUTTONS
 
@@ -285,8 +293,8 @@ private void configureButtonBindings() {
     // operatorLeftBumper.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem,
     // shooterSubsystem, true, ledSubsystem, this::intakeCallback));
     // // manual pivot (possible climb, unlikely)
-    // operatorAButton.whileTrue(new ManualPivot(pivotSubsystem,
-    // ()->modifyAxisCubed(operatorRightStickY)));
+    operatorAButton.whileTrue(
+        new ManualPivot(pivot, () -> JoystickUtil.modifyAxisCubed(operatorLeftStickY)));
     // operatorDownDirectionPad.whileTrue(new ManualPivot(pivotSubsystem, ()->-0.2));
     // // manual rollers
     // operatorYButton.whileTrue(new ManualIntake(intakeSubsystem, true));

src/main/java/frc/robot/commands/pivot/ManualPivot.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands.pivot;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.pivot.*;
 import java.util.function.DoubleSupplier;
@@ -29,6 +30,7 @@ public void initialize() {}
   @Override
   public void execute() {
     pivot.setPivotSpeed(speed.getAsDouble());
+    SmartDashboard.putNumber("command speed for pivot", speed.getAsDouble());
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/pivot/PivotConstants.java

@@ -3,8 +3,8 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 
 public class PivotConstants {
-  public static final double PIVOT_MASS = 0 - 9;
-  public static final double PIVOT_LENGTH = 0 - 9;
+  public static final double PIVOT_MASS = 5;
+  public static final double PIVOT_LENGTH = 5;
 
   public static final double PIVOT_GEAR_RATIO = 8.0;
 

src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java

@@ -4,12 +4,16 @@
 
 package frc.robot.subsystems.pivot;
 
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.Rotations;
+
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.HardwareConstants;
 
 public class PivotIOSim implements PivotIO {
@@ -22,9 +26,16 @@ public class PivotIOSim implements PivotIO {
 
   private SingleJointedArmSim pivotSim =
       new SingleJointedArmSim(
-          DCMotor.getKrakenX60(2), pivotGearing, pivotMass, pivotLength, 0, 0, true, 0);
+          DCMotor.getKrakenX60(2),
+          pivotGearing,
+          0.01,
+          pivotLength,
+          Rotations.of(0).in(Radians),
+          Rotations.of(1).in(Radians),
+          true,
+          Rotations.of(0).in(Radians));
 
-  private final Constraints pivotConstraints = new Constraints(0, 0);
+  private final Constraints pivotConstraints = new Constraints(0.8, 0.8);
   private final ArmFeedforward armFeedforward = new ArmFeedforward(armkS, armkG, armkV);
   private final ProfiledPIDController pivotController =
       new ProfiledPIDController(0, 0, 0, pivotConstraints);
@@ -49,6 +60,8 @@ public void updateInputs(PivotIOInputs inputs) {
     inputs.followerVelocity = Units.radiansToRotations(pivotSim.getVelocityRadPerSec());
     inputs.followerAppliedVolts = followerAppliedVolts;
     inputs.followerSupplyCurrentAmps = pivotSim.getCurrentDrawAmps();
+    SmartDashboard.putNumber("Pivot Angle Rads", pivotSim.getAngleRads());
+    SmartDashboard.putNumber("Pivot Speed Rads Per Sec", pivotSim.getVelocityRadPerSec());
   }
 
   /**
@@ -72,6 +85,11 @@ public void setVoltage(double volts) {
   public void setPivotAngle(double angleRots) {
     double currentPivotAngleRots = Units.radiansToRotations(pivotSim.getAngleRads());
     double armFF = armFeedforward.calculate(angleRots, pivotController.getSetpoint().velocity);
-    setVoltage(pivotController.calculate(angleRots, currentPivotAngleRots) + armFF);
+    setVoltage(pivotController.calculate(currentPivotAngleRots, angleRots) + armFF);
+  }
+
+  @Override
+  public void setPivotSpeed(double output) {
+    pivotSim.setInput(output);
   }
 }

src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java

@@ -48,7 +48,7 @@ static Queue<Angle> registerInput(Supplier<Angle> supplier) {
   }
 
   static OdometryThread createInstance(DeviceCANBus canBus) {
-    return switch (Constants.currentMode) {
+    return switch (Constants.CURRENT_MODE) {
       case REAL ->
           new OdometryThreadReal(
               canBus,