added telemetry check for updating smartdashboard values

created sendable chooser to change competition mode + vision

Author: ProgrammingSR

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

@@ -17,6 +17,8 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
@@ -88,6 +90,9 @@ public static final class DriveConstants {
     // TODO: Tune this PID before running on a robot on the ground
     public static final double kPModuleTurningController = 0.3;
 
+    // TODO: Set value
+    public static final boolean kUseTelemetry = false;
+
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
         new Translation2d(kWheelBase / 2, kTrackWidth / 2),
         new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
@@ -114,7 +119,13 @@ public static final class DriveConstants {
     public static final double kMaxAccelerationUnitsPerSecond = 100;
     public static final double kMaxAngularAccelerationUnitsPerSecond = 100;
   }
-
+  public static final class competitionMode {
+    public static final boolean kCompetitionMode = false;
+    
+  } {
+  
+    
+  }
   public static final class VisionConstants {
     // TODO: Update cam pose relative to center of bot
     public static final Pose3d kCamPos = new Pose3d(
@@ -206,4 +217,11 @@ public static final class EndEffectorConstants{
             Map.entry(0.0, Pair.of(0.0, Math.PI / 2)),
             Map.entry(1.0, Pair.of(Math.PI / 2, Math.PI)))); // TODO: find safe pivot position
   }
-}
+  public static final class SendableChooserConstants {
+    public static final String kcompetitionMode = "competition mode";      
+    public static final String kpracticeMode = "practice mode";
+    public static final String kUseVision = "use vision";
+    public static final String kNotUseVision = "don't use vision";
+  }
+}  
+

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -23,6 +24,22 @@ public class Robot extends TimedRobot {
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
    */
+
+  //creating sendable chooser to set competition mode
+  public final class sendableChooserCompMode {
+    public String m_modeSelected;
+    private final SendableChooser<String> m_sendableChooserCompetition = new SendableChooser<>();
+  }
+
+  //creating a sendable chooser to set vision
+  public final class sendableChooserVision {
+    public String m_usingVision;
+    private final SendableChooser<String> m_sendableChooserVision = new SendableChooser<>();
+  }
+
+  private sendableChooserCompMode sendableComp = new sendableChooserCompMode();
+  private sendableChooserVision sendableVision = new sendableChooserVision();
+
   @Override
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
@@ -78,6 +95,12 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+    sendableComp.m_modeSelected = sendableComp.m_sendableChooserCompetition.getSelected();
+  	boolean m_competitionMode = sendableComp.m_modeSelected.equals("competition mode");
+
+    sendableVision.m_usingVision = sendableVision.m_sendableChooserVision.getSelected();
+  	boolean m_usingVision = sendableVision.m_usingVision.equals("use vision");
+
   }
 
   /** This function is called periodically during operator control. */

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

@@ -162,8 +162,10 @@ public void periodic() {
       m_desiredStates[3].angle.getDegrees(), m_desiredStates[3].speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond,
     };
 
-    SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logDataDesired);
-    SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
+    if (Constants.DriveConstants.kUseTelemetry) {
+      SmartDashboard.putNumberArray("AdvantageScope Swerve Desired States", logDataDesired);
+      SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
+    }
   }
 
   /**

src/main/java/frc/robot/utils/AllianceFlipUtil.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 
 /** Add your docs here. */
 public class AllianceFlipUtil {