final 612 shuffleboard/telemetry stuff

Co-authored-by: Penguinator472 <[email protected]> Co-authored-by: DragonRida <[email protected]> Co-authored-by: ParagonCrafter <[email protected]>
Team612 · Feb 24, 2024 · fbb059d · fbb059d
1 parent b45d610
commit fbb059d
Show file tree

Hide file tree

Showing 3 changed files with 283 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/ShuffleBoardButtons.java b/src/main/java/frc/robot/ShuffleBoardButtons.java
@@ -0,0 +1,264 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+package frc.robot;
+import org.photonvision.PhotonCamera;
+
+import com.ctre.phoenix6.StatusSignal;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+// import edu.wpi.first.math.estimator.PoseEstimator;
+// import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.PoseEstimator;
+// import frc.robot.subsystems.Climb;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Vision;
+import frc.robot.SwerveModule;
+// import frc.robot.subsystems.Graboloid Jr. XIV
+public class ShuffleBoardButtons {
+   PhotonCamera cam = new PhotonCamera("camName1"); // Whatever the camera 1 name is plug it in here
+   PhotonCamera camo = new PhotonCamera("camName2"); // Whatever the camera 2 name is plug it in here
+   Vision m_vision = new Vision(cam,camo);
+   SwerveModule m_swerve = new SwerveModule(0, null);
+   Drivetrain m_drivetrain = new Drivetrain();
+   Intake m_intake = new Intake();
+  //  Climb m_climb = new Climb();
+   PoseEstimator m_poseEstimator = new PoseEstimator();
+   Shooter m_shooter = new Shooter();
+   ShuffleboardTab m_driverTab;
+   ShuffleboardTab m_encoderTab;
+   ShuffleboardTab m_graphTab;
+   ShuffleboardTab m_debugTab;
+   ShuffleboardTab m_limitSwitchTab;
+
+   GenericEntry NavxAngle;
+   GenericEntry PoseEstimatorAngle;
+   GenericEntry PoseEstimatorX;
+   GenericEntry PoseEstimatorY;
+   GenericEntry fodState;
+   // GenericEntry grabberCurrentGraph;
+   GenericEntry telescopeCurrentGraph;
+   GenericEntry BoreEncoders;
+   GenericEntry pivotEntry;
+   GenericEntry telescopeEntry;
+   GenericEntry telescopeEncoderRate;
+   GenericEntry PivPos;
+   GenericEntry AprilDet;
+   GenericEntry NoteDet;
+   GenericEntry voltTurn;
+   GenericEntry voltDrive;
+   // GenericEntry isReleasing;
+
+
+   GenericEntry pivotBLS;
+   GenericEntry telescopeLimitSwitch;
+
+
+   GenericEntry isBlueAlliance;
+
+
+   public static GenericEntry toggleAlliance;
+
+
+
+   //accessable entires
+   public static GenericEntry grabberSpikeTresh;
+   public static GenericEntry teleEncoderRateThresh;
+
+
+   public static GenericEntry teleSpikeThresh;
+
+
+   interface StringFunction {
+       String run();
+   }
+
+
+   interface IntFunction {
+       int run();
+   }
+   interface DoubleFunction {
+       Double run();
+   }
+
+   interface DoubleArrayFunction {
+    double[] run();
+}
+
+   interface BooleanFunction {
+       Boolean run();
+   }
+
+   interface Pose2DFunction {
+       Pose2d run();
+   }
+
+
+   interface Rotation2dDFunction {
+    Rotation2d run();
+  }
+
+   interface DSVFunction {
+       DoubleSolenoid.Value run();
+   }
+
+   interface StatusSignalFunction {
+    StatusSignal<Double> run();
+}
+
+
+   // //debugging for arm thresholds
+   public static GenericEntry lowGeneral;
+   public static GenericEntry noteIr;
+   public static GenericEntry climbPos;
+   public static GenericEntry shoot;
+   public static GenericEntry grab;
+   // public static GenericEntry NoteDet;
+   // public static GenericEntry voltTurn;
+   // public static GenericEntry voltDrive;
+   public static GenericEntry humanStation; //telescope debug
+
+
+   public void initButtons(){
+       m_driverTab = Shuffleboard.getTab("DriverTab");
+       m_encoderTab = Shuffleboard.getTab("Encoder");
+       m_graphTab = Shuffleboard.getTab("Graphs");
+       m_debugTab = Shuffleboard.getTab("Debug Tab");
+       m_limitSwitchTab = Shuffleboard.getTab("Limit Switch Tab");
+
+ /* m_driverTab.add("Drive Forward", new InstantCommand(() -> m_drivetrain.drive(0.5, 0, 0, false)))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(0, 0)
+        .withSize(1, 1);
+    mm_driverTab.add("Drive Backward", new InstantCommand(() -> m_drivetrain.drive(-0.5, 0, 0, false)))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(1, 0)
+        .withSize(1, 1);
+    m_driverTab.add("Turn Left", new InstantCommand(() -> m_drivetrain.drive(0, 0, -0.5, false)))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(0, 1)
+        .withSize(1, 1);
+    m_driverTab.add("Turn Right", new InstantCommand(() -> m_drivetrain.drive(0, 0, 0.5, false)))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(1, 1)
+        .withSize(1, 1);
+    m_driverTab.add("Intake In", new InstantCommand(() -> m_intake.intakeIn()))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(0, 2)
+        .withSize(1, 1);
+    m_driverTab.add("Intake Out", new InstantCommand(() -> m_intake.intakeOut()))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(1, 2)
+        .withSize(1, 1);
+    m_driverTab.add("Intake Stop", new InstantCommand(() -> m_intake.intakeStop()))
+        .withWidget(BuiltInWidgets.kCommand)
+        .withPosition(0, 3)
+        .withSize(1, 1);*/
+
+
+       // Srikar
+       Rotation2dDFunction navXReturnO = () -> m_drivetrain.getNavxAngle();
+       Pose2DFunction poseEstimatorAngleO = () -> m_poseEstimator.getCurrentPose();
+       BooleanFunction isAprilTagDetectedO = () -> m_vision.hasTarget(); // Joel
+      //  m_vision.setPipeline(1); // set the vision pipeline to whatever the note pipeline is
+       BooleanFunction isNoteDetectedO = () -> m_vision.hasTarget();; // Srikar
+       StatusSignalFunction voltAngle1 = () -> m_swerve.getAngleVoltage(); // Achyut
+       StatusSignalFunction voltDrive1 = () -> m_swerve.getDriveVoltage(); // Achyut
+
+
+       AprilDet = m_debugTab.add("climbPos", isAprilTagDetectedO.run()).getEntry();
+       NoteDet =  m_debugTab.add("isShooterShooting", isNoteDetectedO.run()).getEntry();
+       voltTurn = m_debugTab.add("isGrabberGrabbing", voltAngle1.run()).getEntry();
+       voltDrive = m_debugTab.add("isGrabberGrabbing", voltDrive1.run()).getEntry();
+
+
+
+       // IntFunction PoseEstimatorXO = () -> m_poseEstimator.getCurrentPose();
+       // IntFunction PoseEstimatorYO = () -> m_poseEstimator.getCurrentPose();
+
+
+       // Joel
+       BooleanFunction isBlueAllianceO = () -> true;
+       BooleanFunction noteIrO = () -> m_intake.getIRSensor();
+      //  DSVFunction climbPosO = () -> m_climb.get();
+       DoubleArrayFunction shootO = () -> m_shooter.get();
+
+
+       // Isaac and Achyut
+       DoubleFunction grabO = () -> m_intake.getIntake();
+       // IntFunction humanStationO = () -> 1;
+       DoubleFunction PivPosO = () -> m_intake.getArm();
+       // IntFunction fodState0 = () -> 1;
+       // IntFunction pivotEntry0 = () -> 1;
+       // IntFunction boreEncodersO = () -> 1;
+
+       System.out.println(navXReturnO.run());
+       System.out.println(poseEstimatorAngleO.run());
+
+
+       //debug entries
+
+
+       NavxAngle = m_debugTab.add("NavX angle", navXReturnO.run()).getEntry();
+       PoseEstimatorAngle = m_debugTab.add("PoseEstimator Angle", poseEstimatorAngleO.run()).getEntry();
+       // PoseEstimatorX = m_debugTab.add("PoseEstimator X", PoseEstimatorXO.run()).getEntry();
+       // PoseEstimatorY = m_debugTab.add("PoseEstimator Y", PoseEstimatorYO.run()).getEntry();
+       isBlueAlliance = m_debugTab.add("isBlueAlliance", isBlueAllianceO.run()).getEntry();
+
+
+       // Game entries
+       lowGeneral = m_debugTab.add("low General", 0.0).getEntry();
+       noteIr = m_debugTab.add("noteir", noteIrO.run()).getEntry();
+       StringFunction climbPosO;
+	// climbPos = m_debugTab.add("climbPos", climbPosO.run()).getEntry();
+       shoot =  m_debugTab.add("isShooterShooting", shootO.run()).getEntry();
+       grab = m_debugTab.add("isGrabberGrabbing", grabO.run()).getEntry();
+
+       // humanStation = m_debugTab.add("humanStation", humanStationO.run()).getEntry();
+
+
+       // smartdashboard entries
+
+       // fodState = m_driverTab.add("FOD state?", fodState0.run()).getEntry();
+       PivPos = m_driverTab.add("isPivotInIntake", PivPosO.run()).getEntry();
+
+
+
+
+       // encoder entries
+       // pivotEntry = m_encoderTab.add("Pivot Encoder", pivotEntry0.run()).getEntry();
+       // BoreEncoders = m_encoderTab.add("Bore Encoder", boreEncodersO.run()).getEntry();
+
+
+       //graphing entries
+       // grabberSpikeTresh = m_graphTab.add("GrabberSpikeTresh", 0.0).getEntry();
+       // grabberCurrentGraph = m_graphTab.add("Grabber Current vs Time", 0.0).withWidget(BuiltInWidgets.kGraph).getEntry();
+       // telescopeCurrentGraph = m_graphTab.add("Telescope Current vs Time", 0.0).withWidget(BuiltInWidgets.kGraph).getEntry();
+       // telescopeEncoderRate = m_graphTab.add("Telescope Encoder Rate", 0.0).withWidget(BuiltInWidgets.kGraph).getEntry();
+       // telescopeEntry = m_encoderTab.add("Telescope Encoder", 0.0).getEntry();
+       // teleEncoderRateThresh = m_graphTab.add("TeleEncoderThresh", 0.0).getEntry();
+       // teleSpikeThresh = m_graphTab.add("TeleSpikeThresh", 0.0).getEntry();
+       // pivotBLS = m_limitSwitchTab.add("pivotLimitSwitchState", false).getEntry();
+
+
+       // telescopeLimitSwitch = m_limitSwitchTab.add("telescopeLimitSwitch", false).getEntry();
+       // toggleAlliance = m_driverTab.add("Is Red Alliance?",false).withWidget(BuiltInWidgets.kToggleButton).getEntry();
+   }
+
+
+   // @Override
+   public void periodoic() {
+     // This method will be called once per scheduler run
+   }
+
+}
+
diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java
@@ -1,4 +1,5 @@
 package frc.robot;
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
@@ -170,4 +171,12 @@ public void setDriveVoltage(double volts) {
   public void setTurnVoltage(double volts) {
     angleMotor.setVoltage(volts);
   }
+
+  public StatusSignal<Double> getAngleVoltage() {
+    return driveMotor.getMotorVoltage();
+  }
+
+  public StatusSignal<Double> getDriveVoltage() {
+    return angleMotor.getMotorVoltage();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -35,12 +35,22 @@ public static Intake getInstance(){
     }
     return instance;
   }
+  // public double {
 
+  // } 
+  // public doublle
   // return limit switch states
   public boolean getIntakeLimitState(){
     return m_IntakePivotMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen).isPressed();
   }
+
+  public double getIntake(){
+    return m_IntakeRollerMotor.getActiveTrajectoryPosition();
+  }
 
+  public double getArm(){
+    return m_IntakePivotMotor.get();
+  }
   // move intake pivot
   public void rotateIntake(double rotate){
     if(Math.abs(rotate) < DEADZONE) rotate = 0;