processor placement, advantagekit, and chirp

build.gradle

@@ -10,6 +10,11 @@ java {
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
 deploy {
@@ -72,6 +77,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4559075342465753,
+        "y": 3.123458904109589
+      },
+      "prevControl": {
+        "x": 2.4559075342465753,
+        "y": 3.123458904109589
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 57.80426606528677
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -4,7 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -14,11 +22,31 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
+
+
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+  @SuppressWarnings("resource")
+public Robot() {
+    Logger.recordMetadata("ProjectName", "2025_Robot_Code"); // Set a metadata value
+
+    if (isReal()) {
+        Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+    } else {
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user)
+        Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log
+    }
+
+    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added.
+    }
+
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.

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

@@ -7,6 +7,7 @@ package frc.robot;
 import frc.robot.constants.ManipulatorPivotConstants;
 import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.ManipulatorPivot;
 import frc.robot.subsystems.Vision;
@@ -64,6 +65,12 @@ public class RobotContainer {
 
 		manipulatorPivot = new ManipulatorPivot();
 
+		//commands for pathplanner
+		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
+		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
+		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -143,14 +150,6 @@ public class RobotContainer {
 			manipulator.runUntilCollected(() -> 0.35)
 		);
 
-		driver.leftBumper().whileTrue(
-			manipulator.runUntilCollected(() -> 0.5)
-		);
-
-		driver.rightBumper().whileTrue(
-			manipulator.runManipulator(() -> 1, false)
-		);
-
 		driver.start().and(driver.back()).onTrue(
 			startingConfig()
 		);
@@ -205,6 +204,8 @@ public class RobotContainer {
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
 		);
 
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition));
+
 	}
 
 	private void configureNamedCommands() {
@@ -418,6 +419,15 @@ public class RobotContainer {
 		.raceWith(elevator.maintainPosition()));
 	}
 
+	private Command moveWithAlgae(double elevatorPosition, double armPosition) {
+		/*return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true)
+			.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
+				 elevator.maintainPosition());*/
+		return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kProcessorPosition)
+		.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
+		.raceWith(elevator.maintainPosition()));
+	}
+
 	@SuppressWarnings("unused")
 	private Command startingConfig() {
 		return moveManipulatorUtil(0, 0, false, true)

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -43,7 +43,7 @@ public class ManipulatorPivotConstants {
     public static final double kL4Position = Units.degreesToRadians(45.0);
     public static final double kL2AlgaePosition = Units.degreesToRadians(175.0);
     public static final double kL3AlgaePosition = Units.degreesToRadians(175.0);
-    public static final double kProcesserPosition = Units.degreesToRadians(175.0);
+    public static final double kProcessorPosition = Units.degreesToRadians(175.0);
     public static final double kNetPosition = Units.degreesToRadians(175.0);
     /**The closest position to the elevator brace without hitting it */
     public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -4,10 +4,12 @@
 
 package frc.robot.subsystems;
 
+import java.io.File;
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.ctre.phoenix6.Orchestra;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
@@ -22,6 +24,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
@@ -43,6 +47,9 @@ public class Drivetrain extends SubsystemBase {
 
     private Vision vision;
 
+    public Orchestra m_orchestra = new Orchestra();
+    private Timer musicTimer = new Timer();
+
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
         m_frontLeft = new MAXSwerveModule(
@@ -73,7 +80,7 @@ public class Drivetrain extends SubsystemBase {
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(gyro.getAngle()),
+            Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -99,6 +106,21 @@ public class Drivetrain extends SubsystemBase {
             }, 
             this
         );
+
+        m_orchestra.loadMusic(Filesystem.getDeployDirectory()
+        .toPath()
+        .resolve("Orchestra" + File.separator + "doomE1M1.chrp")
+        .toString());
+
+        // Add a single device to the orchestra
+        m_orchestra.addInstrument(m_frontLeft.getDrivingMotor(), 0);
+        m_orchestra.addInstrument(m_frontRight.getDrivingMotor(), 1);
+        m_orchestra.addInstrument(m_rearLeft.getDrivingMotor(), 2);
+        m_orchestra.addInstrument(m_rearRight.getDrivingMotor(), 3);
+
+        m_orchestra.play();
+        musicTimer.reset();
+        musicTimer.start();
     }
 
     @Override
@@ -130,6 +152,13 @@ public class Drivetrain extends SubsystemBase {
         }
         */
         
+        if(musicTimer.get()>8){
+            if (m_orchestra.isPlaying()) {
+                m_orchestra.stop();
+            }
+            musicTimer.stop();
+            musicTimer.reset();
+        }
         
     }
 

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -107,9 +107,9 @@ public class MAXSwerveModule {
         // Command driving and turning SPARKS towards their respective setpoints.
         m_drive.setControl(
         driveVelocityRequest.withVelocity(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         ).withFeedForward(
-            correctedDesiredState.speedMetersPerSecond
+            correctedDesiredState.speedMetersPerSecond / ModuleConstants.kWheelCircumferenceMeters
         )
         );
 
@@ -134,6 +134,10 @@ public class MAXSwerveModule {
         return m_turningSpark.get() * RobotController.getBatteryVoltage();
     }
 
+    public TalonFX getDrivingMotor(){
+        return m_drive;
+    }
+
     /** Zeroes all the SwerveModule encoders. */
     public void resetEncoders() {
         m_drive.setPosition(0);

src/main/java/frc/robot/subsystems/ManipulatorPivot.java

@@ -47,7 +47,7 @@ public class ManipulatorPivot extends SubsystemBase {
         );
         pidController.setSetpoint(0);
 
-        pidController.disableContinuousInput();
+        pidController.enableContinuousInput(0, 180);
         
         feedForward = new ArmFeedforward(
             ManipulatorPivotConstants.kFeedForwardS,

vendordeps/AdvantageKit.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.1",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+    ],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.1",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}