Add autos in MapleSim

src/main/deploy/pathplanner/paths/Mobility.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 7.0,
+        "x": 6.212301136363636,
         "y": 4.0
       },
       "prevControl": {
-        "x": 7.255655952782719,
+        "x": 6.467957089146355,
         "y": 4.003114592795971
       },
       "nextControl": null,

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

@@ -70,6 +70,8 @@ public class RobotContainer extends TimedRobot {
 
     public RobotContainer() {
         NamedCommands.registerCommand("Eject Coral", superstructure.AutoCoral());
+        NamedCommands.registerCommand("Intake Algae", autoCommands.IntakeAlgae());
+        NamedCommands.registerCommand("Eject Algae", autoCommands.EjectAlgae());
 
         autoChooser = AutoBuilder.buildAutoChooser("Tests");
         SmartDashboard.putData("Auto Mode", autoChooser);
@@ -238,6 +240,11 @@ public static double ExponentialConvert(double controllerValue, double exponent)
     }
 
     public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
+        if (RobotBase.isReal()){
+            return autoChooser.getSelected(); // Selected auto on driver station (for real robot)
+        }
+        else{
+            return AutoBuilder.buildAuto("RightAuto (Coral, Algae x2)"); // Change the autoName to the desired auto in pathplanner (for sim)
+        }
     }
 }

src/main/java/frc/robot/Superstructure.java

@@ -15,6 +15,8 @@
 import frc.robot.NetworkTables;
 import frc.robot.NetworkTables.ConstantId;
 
+import edu.wpi.first.wpilibj.RobotBase;
+
 public class Superstructure extends edu.wpi.first.wpilibj2.command.SubsystemBase {
     private final DigitalInput m_coralBeamBreak = new DigitalInput(0);
     private final NetworkTables m_networkTables;
@@ -100,7 +102,7 @@ public void PrintPosition() {
     public Command AutoCoral() {
         return Commands.sequence(
             Commands.runOnce(() -> {
-                System.out.println("============ CoralEjectPressed\nmoving rollers forward//\n");
+                System.out.println("============ CoralEjectPressed\nmoving rollers forward\n");
                 armMotor.set(TalonSRXControlMode.Position,
                     m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition)
                 );
@@ -124,6 +126,7 @@ public Command AutoCoral() {
             }),
             Commands.waitSeconds(0.5),
             Commands.runOnce(() -> {
+                System.out.println("Reset arm and roller");
                 rollerMotor.set(VictorSPXControlMode.PercentOutput, 0);
                 armMotor.set(ControlMode.Position, 
                     m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition)
@@ -137,8 +140,7 @@ public Command AlgaeIntakePressed() {
             Commands.runOnce(() -> {
                 System.out.println("============ AlgaeIntakePressed");
                 System.out.println("lowering arm");
-                System.out.println("Position1: " + armMotor.getSelectedSensorPosition(0));
-
+                if (RobotBase.isReal()) System.out.println("Position1: " + armMotor.getSelectedSensorPosition(0));
                 armMotor.set(ControlMode.Position,
                     m_networkTables.getDoubleValue(ConstantId.ArmIntakePosition)
                 );
@@ -148,7 +150,7 @@ public Command AlgaeIntakePressed() {
             ),
             Commands.runOnce(() -> {
                 System.out.println("stopping the lowering of arm");
-                System.out.println("Position2: " + armMotor.getSelectedSensorPosition());
+                if (RobotBase.isReal()) System.out.println("Position2: " + armMotor.getSelectedSensorPosition());
 
                 rollerMotor.set(VictorSPXControlMode.PercentOutput,
                     m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeIntakeVelocity)
@@ -161,7 +163,7 @@ public Command AlgaeIntakeReleased() {
         return Commands.sequence(
             Commands.runOnce(() -> {
                 System.out.println("============ AlgaeIntakeReleased");
-                System.out.println("Position3:" + armMotor.getSelectedSensorPosition());
+                if (RobotBase.isReal()) System.out.println("Position3:" + armMotor.getSelectedSensorPosition());
 
                 armMotor.set(ControlMode.Position,
                     m_networkTables.getDoubleValue(ConstantId.ArmHoldPosition)
@@ -172,7 +174,7 @@ public Command AlgaeIntakeReleased() {
             ),
             Commands.runOnce(() -> {
                 System.out.println("============ AlgaeIntakeReleased");
-                System.out.println("Position4:" + armMotor.getSelectedSensorPosition(0));
+                if (RobotBase.isReal()) System.out.println("Position4:" + armMotor.getSelectedSensorPosition(0));
                 rollerMotor.set(VictorSPXControlMode.PercentOutput,
                     m_networkTables.getDoubleValue(ConstantId.RollerMovementHoldVelocity));
             })
@@ -222,6 +224,7 @@ public Command AutoIntakeCoral() {
     public Command AlgaeEjectPressed() {
         return Commands.runOnce(() -> {
             System.out.println("============= AlgaeEjectPressed");
+            System.out.println("Set rollers to algae eject velocity");
             rollerMotor.set(VictorSPXControlMode.PercentOutput,
                 m_networkTables.getDoubleValue(ConstantId.RollerMovementAlgaeEjectVelocity)
             );
@@ -231,6 +234,7 @@ public Command AlgaeEjectPressed() {
     public Command AlgaeEjectReleased() {
         return Commands.runOnce(() -> {
             System.out.println("========== AlgaeEjectReleased");
+            System.out.println("Reset rollers");
             armMotor.set(TalonSRXControlMode.Position,
                 m_networkTables.getDoubleValue(ConstantId.ArmDefaultPosition)
             );

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

@@ -33,6 +33,7 @@ public Command IntakeAlgae() {
     public Command EjectAlgae() {
         return Commands.sequence(
             // Implement algae eject
+            superstructure.AlgaeEjectPressed(),
             Commands.waitSeconds(networkTables.getTimeValue(NetworkTables.ConstantId.AutoEjectAlgaeWait).in(edu.wpi.first.units.Units.Seconds)),
             superstructure.AlgaeEjectReleased()
         );

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

@@ -139,9 +139,12 @@ private void startSimThread() {
     /* Run simulation at a faster rate so PID gains behave more reasonably */
     m_simNotifier = new Notifier(mapleSimSwerveDrivetrain::update);
     m_simNotifier.startPeriodic(kSimLoopPeriod);
+
+    // Initialize simulation pose to inside the field on black line for red alliance
+    double redAllianceInitialSimX = 10.2;
+    int redAllianceInitialSimY = 4;
 
-    // Initialize simulation pose to inside the field
-    Pose2d initialSimPose = new Pose2d(16, 5, new Rotation2d(0));
+    Pose2d initialSimPose = new Pose2d(redAllianceInitialSimX, redAllianceInitialSimY, new Rotation2d(0));
     mapleSimSwerveDrivetrain.mapleSimDrive.setSimulationWorldPose(initialSimPose);
     }