Deploy vision

.vscode/settings.json

@@ -26,5 +26,7 @@
     },
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m",
+  "java.checkstyle.configuration": "${workspaceFolder}\\buildSrc\\checkstyle.xml",
+  "java.checkstyle.version": "10.8.0"
 }

scripteditor/src/main/java/gui/MainWindow.java

@@ -164,8 +164,8 @@ public void onClick() {
     try {
       logoImage = ImageIO.read(new File("scripteditor/src/resources/logo.png"));
       frame.setIconImage(new ImageIcon("scripteditor/src/resources/Icon.png").getImage());
-      setFieldImage(ImageIO.read(new File("scripteditor/src/resources/47.350364343 FieldImage.png")),
-          "47.350364343 FieldImage");
+      setFieldImage(ImageIO.read(new File("scripteditor/src/resources/21.1215992958 FieldImage.png")),
+          "21.1215992958 FieldImage.png");
     } catch (IOException e) {
       e.printStackTrace();
     }

src/main/java/team1403/lib/core/CougarRobot.java

@@ -97,6 +97,13 @@ public final void changeMode(Mode mode) {
     startMode(mode);
   }
 
+  /**
+   * Adds functationality to the robot init method found in WpiLibRobotAdapter.
+   */
+  public void robotInit() {
+
+  }
+
   /**
    * Adds functionality to the teleop init method found in WpiLibRobotAdapter
    */

src/main/java/team1403/lib/core/WpiLibRobotAdapter.java

@@ -105,6 +105,8 @@ public void robotInit() {
     m_cougarRobot = m_cougarRobotFactory.apply(parameters);
     m_schedulerTimer = new BaseTimer("SchedulerTimer", clock);
     m_periodTimer = new BaseTimer("LoopPeriodTimer", clock);
+
+    m_cougarRobot.robotInit();
   }
 
 

src/main/java/team1403/lib/device/DeviceFactory.java

@@ -8,6 +8,7 @@
  *
  * <p>Subsystems create their devices through this factory.
  */
+
 public interface DeviceFactory {
   /**
    * Creates a PowerDistributor for monitoring the power panel.
@@ -76,12 +77,13 @@ public AdvancedMotorController makeTalonSrx(
    *
    * @param name    The name of the new device instance.
    * @param deviceNumber  The CAN bus channel the motor controller is on.
-   * @param mode  The control mode for the TalonFX
    * @param logger  The logger to use with the new instance.
    *
    * @return a new MotorController for a TalonFX.
    */
-  public AdvancedMotorController makeCougarTalonFx(String name, int deviceNumber, CougarLogger logger);
+
+  public AdvancedMotorController makeCougarTalonFx(String name, int deviceNumber, 
+                                                CougarLogger logger);
 
   /**
    * Creates a VictorSpPwm MotorController.

src/main/java/team1403/lib/device/GyroscopeDevice.java

@@ -67,4 +67,6 @@ public default double getHeading() {
   public default Rotation2d getRotation2d() {
     return Rotation2d.fromDegrees(getHeading());
   }
+
+
 }

src/main/java/team1403/lib/subsystems/BuiltinSubsystem.java

@@ -37,10 +37,10 @@ public BuiltinSubsystem(CougarLibInjectedParameters injectedParameters,
 
   @Override
   public void periodic() {
-    SmartDashboard.putNumber("BuiltinDevices.Pdp.Temp", m_pdp.getCelsius());
-    SmartDashboard.putNumber("BuiltinDevices.Pdp.Amps", m_pdp.getAmps());
-    SmartDashboard.putNumber("BuiltinDevices.Pdp.Volts", m_pdp.getVoltage());
-    SmartDashboard.putNumber("BuiltinDevices.Pdp.Joules", m_pdp.getTotalJoules());
+    // SmartDashboard.putNumber("BuiltinDevices.Pdp.Temp", m_pdp.getCelsius());
+    // SmartDashboard.putNumber("BuiltinDevices.Pdp.Amps", m_pdp.getAmps());
+    // SmartDashboard.putNumber("BuiltinDevices.Pdp.Volts", m_pdp.getVoltage());
+    // SmartDashboard.putNumber("BuiltinDevices.Pdp.Joules", m_pdp.getTotalJoules());
 
     // Let's show memory usage (in MB) for curiosity.
     // There doesnt seem to be a wawy to see in-use so we will show free.

src/main/java/team1403/robot/chargedup/AutoManager.java

@@ -2,21 +2,20 @@
 
 import java.util.List;
 
-import com.revrobotics.CANSparkMax.IdleMode;
-
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 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.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import team1403.robot.chargedup.StateManager.GamePiece;
 import team1403.robot.chargedup.arm.ArmStateGroup;
 import team1403.robot.chargedup.arm.ArmSubsystem;
 import team1403.robot.chargedup.arm.RunIntake;
@@ -28,120 +27,140 @@
 
 public class AutoManager {
   static private AutoManager m_instance;
-  static private final TrajectoryConfig m_trajectoryConfig = new TrajectoryConfig(RobotConfig.Swerve.kMaxSpeed,
-      3).setKinematics(RobotConfig.Swerve.kDriveKinematics);
 
-  private AutoManager() {}
+  private final TrajectoryConfig m_trajectoryConfig = new TrajectoryConfig(4,
+      2).setKinematics(RobotConfig.Swerve.kDriveKinematics);
+
+  private final PIDController xController = new PIDController(
+      RobotConfig.Swerve.kPTranslation,
+      RobotConfig.Swerve.kITranslation,
+      RobotConfig.Swerve.kDTranslation);
+
+  private final PIDController yController = new PIDController(
+      RobotConfig.Swerve.kPTranslation,
+      RobotConfig.Swerve.kITranslation,
+      RobotConfig.Swerve.kDTranslation);
 
+  private final ProfiledPIDController thetaController = new ProfiledPIDController(
+      4,
+      RobotConfig.Swerve.kIAutoTurning,
+      RobotConfig.Swerve.kDAutoTurning,
+      RobotConfig.Swerve.kThetaControllerConstraints);
+
+  private SwerveControllerCommand grid3Trajectory1;
+  private SwerveControllerCommand grid3Trajectory2;
+  private SwerveControllerCommand grid3Trajectory3;
+
+  private AutoManager() {
+    thetaController.enableContinuousInput(-Math.PI, Math.PI);
+  }
 
   public static AutoManager getInstance() {
-    if(m_instance == null) {
+    if (m_instance == null) {
       m_instance = new AutoManager();
     }
     return m_instance;
   }
-  /**
-   * Command for grids 1 and 3.
-   * @param swerve
-   * @param arm
-   * @return the command
-   */
-  Command getSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) {
-    //Config navx
-    swerve.zeroGyroscope();
-    swerve.setGyroscopeOffset(199);
-
-    Rotation2d rotation = swerve.getGyroscopeRotation();
-
-    // 2. Generate trajectory
-    final Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(0, 0, rotation),
-        List.of(
-            new Translation2d(0.2, 0),
-            new Translation2d(0.4, 0),
-            new Translation2d(0.6, 0),
-            new Translation2d(0.8, 0),
-            new Translation2d(0.9, 0)),
-        new Pose2d(1, 0, rotation),
-        m_trajectoryConfig);
-
-    Trajectory trajectoryTwo = TrajectoryGenerator.generateTrajectory(
-      new Pose2d(1, 0, rotation),
-      List.of(
-          new Translation2d(1.5, 0),
-          new Translation2d(2, 0),
-          new Translation2d(2.5, 0),
-          new Translation2d(3, 0)),
-      new Pose2d(3.5, 0, rotation),
-      m_trajectoryConfig);
-
-    // 3. Define PID controllers for tracking trajectory
-    PIDController xController = new PIDController(
-      RobotConfig.Swerve.kPTranslation, 
-      RobotConfig.Swerve.kITranslation, 
-      RobotConfig.Swerve.kDTranslation);
 
-    PIDController yController = new PIDController(
-      RobotConfig.Swerve.kPTranslation, 
-      RobotConfig.Swerve.kITranslation, 
-      RobotConfig.Swerve.kDTranslation);
 
-    ProfiledPIDController thetaController = new ProfiledPIDController(
-      0, 
-      RobotConfig.Swerve.kIAutoTurning, 
-      RobotConfig.Swerve.kDAutoTurning, 
-      RobotConfig.Swerve.kThetaControllerConstraints);
+  public void init(SwerveSubsystem swerve) {
+    grid3Trajectory1 = new SwerveControllerCommand(
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            List.of(
+                new Translation2d(-1, -0.3),
+                new Translation2d(-2, -0.3)),
+            new Pose2d(-3, 0, Rotation2d.fromDegrees(180)),
+            m_trajectoryConfig),
+        swerve::getPose,
+        xController,
+        yController,
+        thetaController,
+        swerve);
 
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    SwerveControllerCommand swerveControllerCommandOne = new SwerveControllerCommand(
-        trajectory,
+    grid3Trajectory2 = new SwerveControllerCommand(
+        TrajectoryGenerator.generateTrajectory( 
+            new Pose2d(-3, 0, Rotation2d.fromDegrees(180)),
+            List.of(
+                new Translation2d(-4.8, 0.7),
+                new Translation2d(-4.9, -0.7),
+                new Translation2d(-4.7, -0.2)),
+            new Pose2d(-4, 0.05, Rotation2d.fromDegrees(225)),
+            m_trajectoryConfig),
         swerve::getPose,
         xController,
         yController,
         thetaController,
         swerve);
 
-    SwerveControllerCommand swerveControllerCommandTwo = new SwerveControllerCommand(
-      trajectoryTwo,
-      swerve::getPose,
-      xController,
-      yController,
-      thetaController,
-      swerve);
-
-    return new SequentialCommandGroup(
-      // new SequentialMoveArmCommand(m_arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false),
-      new RunIntake(arm, 1),
-      swerveControllerCommandOne,
-      // new SetpointArmCommand(m_arm, ArmStateGroup.getTuck(), false),
-      swerveControllerCommandTwo
-      );
+    grid3Trajectory3 = new SwerveControllerCommand(
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(-4, -0.3, Rotation2d.fromDegrees(225)),
+            List.of(
+                new Translation2d(-2.5, -0.5),
+                new Translation2d(-0.5, -0.5)),
+            new Pose2d(0.1, -1.05, Rotation2d.fromDegrees(4)),
+            m_trajectoryConfig),
+        swerve::getPose,
+        xController,
+        yController,
+        thetaController,
+        swerve);
   }
 
+  /**
+   * Command for grids 3.
+   * 
+   * @param swerve
+   * @param arm
+   * @return the command
+   */
+  public Command getRightGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) {
+    swerve.setSpeedLimiter(1);
+
 
-  public Command getAlternateSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) {
+    return new SequentialCommandGroup(
+        new SequentialMoveArmCommand(arm,
+        StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false),
+        new RunIntake(arm, 1),
+        new ParallelCommandGroup(
+            new SequentialCommandGroup(
+                new WaitCommand(0.1),
+                new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false),
+                new WaitCommand(1.8),
+                new InstantCommand(() -> StateManager.getInstance().updateArmState(GamePiece.CUBE)),
+                new SetpointArmCommand(arm, () -> StateManager.getInstance().getCurrentArmGroup().getFloorIntakeState(),
+                    true),
+                new RunIntake(arm, 3, 1.5),
+                new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), true),
+                new WaitCommand(1.2),
+                new SetpointArmCommand(arm, () -> StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false)
+                ),
+            new SequentialCommandGroup(
+                grid3Trajectory1,
+                grid3Trajectory2,
+                grid3Trajectory3)),
+                new RunIntake(arm, -1));
+  }
+
+  public Command getTimedSideGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) {
     swerve.setSpeedLimiter(1);
     return new SequentialCommandGroup(
-      new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false),
-      new RunIntake(arm, 1),
-      new TimedDrive(swerve, 1, new ChassisSpeeds(-2, 0, 0)),
-      new ParallelCommandGroup(
-        new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false),
-        new TimedDrive(swerve,6.5, new ChassisSpeeds(-2, 0, 0))
-      )
-      );
+        new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false),
+        new RunIntake(arm, 1),
+        new TimedDrive(swerve, 1, new ChassisSpeeds(-2, 0, 0)),
+        new ParallelCommandGroup(
+            new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false),
+            new TimedDrive(swerve, 6.5, new ChassisSpeeds(-2, 0, 0))));
   }
 
   public Command getMiddleGridCommand(SwerveSubsystem swerve, ArmSubsystem arm) {
-  swerve.setSpeedLimiter(1);
-  return new SequentialCommandGroup(
-      new SequentialMoveArmCommand(arm, StateManager.getInstance().getCurrentArmGroup().getHighNodeState(), false),
-      new RunIntake(arm, 1),
-      new TimedDrive(swerve, 1, new ChassisSpeeds(-2, -0.5, 0.2)),
-      new SetpointArmCommand(arm, () -> ArmStateGroup.getTuck(), false),
-      new TimedDrive(swerve, 2.5, new ChassisSpeeds(-4.5, 0.1, 0)),
-      new InstantCommand(() -> swerve.setXModeEnabled(true))
-  );
+    swerve.setSpeedLimiter(1);
+    return new SequentialCommandGroup(
+        new TimedDrive(swerve, 3.4, new ChassisSpeeds(-4, 0, 0)),
+        new TimedDrive(swerve, 1.6, new ChassisSpeeds(0, -4, 0)),
+        new TimedDrive(swerve, 1.8, new ChassisSpeeds(4, 0, 0)),
+        new InstantCommand(() -> swerve.setXModeEnabled(true)));
   }
 }