Merge branch 'testing' of https://github.com/Creekside7688/FRC-2024 i…

…nto testing

.vscode/settings.json

@@ -31,7 +31,10 @@
     "gradle": true,
     ".wpilib": true,
     ".vscode": true,
-    ".github": true
+    ".github": true,
+    ".DataLogTool": true,
+    ".pathplanner": true,
+    ".SysId": true
   },
   "java.test.config": [
     {

elastic-layout.json

@@ -0,0 +1 @@
+{"version":1.0,"grid_size":120,"tabs":[{"name":"Info","grid_layout":{"layouts":[],"containers":[{"title":"Limelight","x":0.0,"y":0.0,"width":600.0,"height":480.0,"type":"Camera Stream","properties":{"topic":"/CameraPublisher/limelight-processed","period":0.06}},{"title":"Swerve","x":960.0,"y":360.0,"width":360.0,"height":360.0,"type":"SwerveDrive","properties":{"topic":"/SmartDashboard/Swerve","period":0.06,"show_robot_rotation":true,"rotation_unit":"Radians"}},{"title":"Heading","x":600.0,"y":360.0,"width":360.0,"height":360.0,"type":"Radial Gauge","properties":{"topic":"/SmartDashboard/Heading","period":0.033,"data_type":"double","start_angle":0.0,"end_angle":360.0,"min_value":0.0,"max_value":360.0,"number_of_labels":8,"wrap_value":false,"show_pointer":true,"show_ticks":true}},{"title":"Battery Voltage","x":0.0,"y":480.0,"width":360.0,"height":240.0,"type":"Voltage View","properties":{"topic":"/SmartDashboard/Battery Voltage","period":0.033,"data_type":"double","min_value":4.0,"max_value":13.0,"divisions":5,"inverted":false,"orientation":"horizontal"}},{"title":"Battery Amperage","x":360.0,"y":480.0,"width":240.0,"height":240.0,"type":"Graph","properties":{"topic":"/SmartDashboard/Battery Amperage","period":0.033,"data_type":"double","time_displayed":5.0,"min_value":null,"max_value":null,"color":4278238420,"line_width":2.0}},{"title":"Field","x":600.0,"y":0.0,"width":720.0,"height":360.0,"type":"Field","properties":{"topic":"/SmartDashboard/Field","period":0.06,"field_game":"Crescendo","robot_width":0.85,"robot_length":0.85,"show_other_objects":true,"show_trajectories":true}},{"title":"Has Note","x":1440.0,"y":120.0,"width":120.0,"height":120.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/Has Note","period":0.06,"data_type":"boolean","true_color":4294929920,"false_color":4278190080}},{"title":"Elevator","x":1320.0,"y":0.0,"width":120.0,"height":240.0,"type":"Number Bar","properties":{"topic":"/SmartDashboard/Elevator Height (Inches)","period":0.06,"data_type":"double","min_value":0.0,"max_value":33.0,"divisions":4,"inverted":false,"orientation":"vertical"}},{"title":"Intaking","x":1440.0,"y":0.0,"width":120.0,"height":120.0,"type":"Boolean Box","properties":{"topic":"/SmartDashboard/Intaking","period":0.06,"data_type":"boolean","true_color":4278255360,"false_color":4294901760}},{"title":"FMSInfo","x":1320.0,"y":360.0,"width":480.0,"height":240.0,"type":"FMSInfo","properties":{"topic":"/FMSInfo","period":0.06}},{"title":"Alliance","x":1800.0,"y":360.0,"width":120.0,"height":240.0,"type":"Boolean Box","properties":{"topic":"/FMSInfo/IsRedAlliance","period":0.06,"data_type":"boolean","true_color":4294901760,"false_color":4278190335}},{"title":"Scheduler","x":1560.0,"y":0.0,"width":360.0,"height":360.0,"type":"Scheduler","properties":{"topic":"/SmartDashboard/Scheduler","period":0.06}},{"title":"Time Remaining","x":1320.0,"y":600.0,"width":240.0,"height":120.0,"type":"Match Time","properties":{"topic":"/SmartDashboard/Time Remaining","period":0.06,"data_type":"double","time_display_mode":"Minutes and Seconds","red_start_time":15,"yellow_start_time":30}},{"title":"Autonomous Path","x":1320.0,"y":240.0,"width":240.0,"height":120.0,"type":"ComboBox Chooser","properties":{"topic":"/SmartDashboard/Autonomous Path","period":0.06,"sort_options":false}},{"title":"Team","x":1560.0,"y":600.0,"width":360.0,"height":120.0,"type":"Match Time","properties":{"topic":"/SmartDashboard/Team","period":0.06,"data_type":"double","time_display_mode":"Seconds Only","red_start_time":0,"yellow_start_time":0}}]}}]}

simgui.json

@@ -1,9 +1,23 @@
 {
+  "HALProvider": {
+    "DIO": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Info/AUTONOMOUS SELECTOR": "String Chooser",
+      "/Shuffleboard/Info/Autonomous Path": "String Chooser",
+      "/Shuffleboard/Info/Field": "Field2d",
+      "/Shuffleboard/Info/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/Auto Selector": "String Chooser",
-      "/SmartDashboard/Field": "Field2d"
+      "/SmartDashboard/Autonomous Path": "String Chooser",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/Scheduler": "Scheduler",
+      "/SmartDashboard/VisionSystemSim-Limelight/Sim Field": "Field2d"
     }
   },
   "NetworkTables Info": {

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

@@ -1,8 +1,10 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -22,6 +24,15 @@ public void robotInit() {
     public void robotPeriodic() {
         checkDriverStationUpdate();
         CommandScheduler.getInstance().run();
+
+        SmartDashboard.putNumber("Time Remaining", DriverStation.getMatchTime());
+
+        SmartDashboard.putData(CommandScheduler.getInstance());
+        // Replace with values from PDH
+        SmartDashboard.putNumber("Battery Voltage", RobotController.getBatteryVoltage());
+        SmartDashboard.putNumber("Battery Amperage", 60);
+
+        SmartDashboard.putNumber("Team", 7688);
     }
 
     @Override
@@ -68,6 +79,10 @@ public void testPeriodic() {
     }
 
     private void checkDriverStationUpdate() {
+        if(!DriverStation.getAlliance().isPresent()) {
+            return;
+        }
+
         Alliance currentAlliance = DriverStation.getAlliance().get();
 
         // If we have data, and have a new alliance from last time

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.lib.zylve.Controller;
 import frc.robot.auto.PhotonRunnable;
-import frc.robot.auto.commands.FollowAprilTag;
 import frc.robot.constants.OperatorConstants;
 import frc.robot.elevator.Elevator;
 import frc.robot.elevator.commands.ElevatorDown;
@@ -29,7 +28,7 @@
 import frc.robot.swerve.SwerveDrive;
 
 public class RobotContainer {
-    Controller controller = new Controller(OperatorConstants.CONTROLLER_PORT);
+    private final Controller controller = new Controller(OperatorConstants.CONTROLLER_PORT);
 
     private final PhotonCamera photonCamera = new PhotonCamera("limelight");
     private final PhotonRunnable photonRunnable = new PhotonRunnable(photonCamera);
@@ -39,8 +38,6 @@ public class RobotContainer {
     private final Intake intake = new Intake();
     private final Shooter shooter = new Shooter();
 
-    private final FollowAprilTag followAprilTag = new FollowAprilTag(swerveDrive, photonCamera, swerveDrive::getPose);
-
     private final Command elevatorUp = new ElevatorUp(elevator);
     private final Command elevatorDown = new ElevatorDown(elevator);
     private final Command ElevatorSmallUp = new ElevatorSmallUp(elevator);
@@ -75,7 +72,7 @@ public RobotContainer() {
                     -MathUtil.applyDeadband(controller.getLeftY(), OperatorConstants.DEADBAND),
                     -MathUtil.applyDeadband(controller.getLeftX(), OperatorConstants.DEADBAND),
                     -MathUtil.applyDeadband(controller.getRightX(), OperatorConstants.DEADBAND),
-                    controller.getLeftBumper().getAsBoolean(),
+                    controller.getLeftTrigger().getAsBoolean(),
                     true, true
                 ),
                 swerveDrive
@@ -84,25 +81,16 @@ public RobotContainer() {
     }
 
     private void configureSubsystemCommands() {
+        controller.getLeftBumper().onTrue(intakeEject);
         controller.getRightBumper().whileTrue(intakePickup);
-        controller.getLeftTrigger().onTrue(intakeShooterFeed);
 
-
         controller.getA().onTrue(ampScore);
         controller.getB().onTrue(speakerScore);
 
-        controller.getX().onTrue(ElevatorSmallUp);
+        controller.getRightTrigger().onTrue(ElevatorSmallUp);
     }
 
     private void configureSwerveDriveCommands() {
-        controller.getLeftStick()
-            .whileTrue(
-                new RunCommand(
-                    () -> swerveDrive.lockPosition(),
-                    swerveDrive
-                )
-            );
-
         controller.getRightStick()
             .whileTrue(
                 new RunCommand(
@@ -113,14 +101,14 @@ private void configureSwerveDriveCommands() {
     }
 
     private void configureAutonomous() {
-        autoSelector.addOption("Leave", new PathPlannerAuto("Leave"));
-        autoSelector.addOption("Left Amp", new PathPlannerAuto("Left Amp"));
-        autoSelector.setDefaultOption("Right Roundhouse Amp", new PathPlannerAuto("Right Roundhouse Amp"));
-
         NamedCommands.registerCommand("PickupNote", intakePickup);
         NamedCommands.registerCommand("AmpNote", ampScore);
 
-        SmartDashboard.putData("Auto Selector", autoSelector);
+        autoSelector.addOption("Right Roundhouse Amp", new PathPlannerAuto("Right Roundhouse Amp"));
+        autoSelector.addOption("Left Amp", new PathPlannerAuto("Left Amp"));
+        autoSelector.setDefaultOption("Leave", new PathPlannerAuto("Leave"));
+
+        SmartDashboard.putData("Autonomous Path", autoSelector);
     }
 
     public Command getAutonomousCommand() {

src/main/java/frc/robot/auto/PhotonRunnable.java

@@ -95,4 +95,8 @@ public PhotonPipelineResult getLatestResult() {
         return photonCamera.getLatestResult();
     }
 
+    public PhotonCamera getCamera() {
+        return photonCamera;
+    }
+
 }

src/main/java/frc/robot/auto/commands/AmpAlign.java

@@ -70,6 +70,7 @@ public void initialize() {
 
     @Override
     public void execute() {
+
         Pose2d robotPose2d = poseProvider.get();
 
         // Convert the 2d robot pose to a 3d one.

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

@@ -5,6 +5,8 @@
 import edu.wpi.first.math.util.Units;
 
 public class DriveConstants {
+    public static final String SHUFFLEBOARD_TABLE = "Info";
+
     // Maximum allowed speeds.
     public static final double MAXIMUM_SPEED_METRES_PER_SECOND = 4;
     public static final double MAXIMUM_ANGULAR_SPEED_RADIANS_PER_SECOND = 2 * Math.PI;

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

@@ -4,7 +4,7 @@
 
 public class ShooterConstants {
     public static final int MOTOR_ID = 10;
-    public static final int CURRENT_LIMIT = 50;
+    public static final int CURRENT_LIMIT = 90;
     public static final IdleMode IDLE_MODE = IdleMode.kCoast;
 
     public static final double SHOOTER_SPINUP_DELAY = 10;

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

@@ -12,8 +12,8 @@ public class VisionConstants {
 
     /** Physical location of the apriltag camera on the robot, relative to the center of the robot. */
     public static final Transform3d APRILTAG_CAMERA_TO_ROBOT = new Transform3d(
-        new Translation3d(0, 0.3048, -0.3302),
-        new Rotation3d(0.0, 15.0 * Math.PI / 180, 0)
+        new Translation3d(0, 0, 0),
+        new Rotation3d(0, 0, 0)
     );
 
     public static final String LIMELIGHT_NAME = "limelight";

src/main/java/frc/robot/elevator/Elevator.java

@@ -3,6 +3,8 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ElevatorConstants;
 
@@ -21,12 +23,16 @@ public Elevator() {
         encoder.setPosition(0);
     }
 
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Elevator Height (Inches)", this.getEncoderPosition() * 210);
+    }
 
     public void run(double speed) {
         motor.set(speed);
     }
 
-    public double encoderPosition() {
+    public double getEncoderPosition() {
         return encoder.getPosition() / 210;
     }
 

src/main/java/frc/robot/elevator/commands/ElevatorDown.java

@@ -21,7 +21,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        double maxSteps = elevator.encoderPosition();
+        double maxSteps = elevator.getEncoderPosition();
         SmartDashboard.putNumber("EncoderSteps", maxSteps);
     }
 
@@ -34,6 +34,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return elevator.encoderPosition() < ElevatorConstants.MOTOR_MIN_STEPS;
+        return elevator.getEncoderPosition() < ElevatorConstants.MOTOR_MIN_STEPS;
     }
 }

src/main/java/frc/robot/elevator/commands/ElevatorSmallUp.java

@@ -22,7 +22,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        double maxSteps = elevator.encoderPosition();
+        double maxSteps = elevator.getEncoderPosition();
         SmartDashboard.putNumber("EncoderSteps", maxSteps);
 
     }
@@ -37,6 +37,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return elevator.encoderPosition() > ElevatorConstants.MOTOR_TEMP_STEPS;
+        return elevator.getEncoderPosition() > ElevatorConstants.MOTOR_TEMP_STEPS;
     }
 }

src/main/java/frc/robot/elevator/commands/ElevatorUp.java

@@ -21,7 +21,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        double maxSteps = elevator.encoderPosition();
+        double maxSteps = elevator.getEncoderPosition();
         SmartDashboard.putNumber("EncoderSteps", maxSteps);
 
     }
@@ -34,6 +34,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return elevator.encoderPosition() > ElevatorConstants.MOTOR_MAX_STEPS;
+        return elevator.getEncoderPosition() > ElevatorConstants.MOTOR_MAX_STEPS;
     }
 }

src/main/java/frc/robot/intake/Intake.java

@@ -6,35 +6,36 @@
 import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeConstants;
 
 public class Intake extends SubsystemBase {
-    private final CANSparkMax motor = new CANSparkMax(IntakeConstants.MOTOR_ID, MotorType.kBrushless);
-
-    private final DigitalInput sensor = new DigitalInput(IntakeConstants.SENSOR_CHANNEL);
-    private final RelativeEncoder encoder = motor.getEncoder();
-
-    private final SparkPIDController rotationController = motor.getPIDController();
+    private final CANSparkMax motor;
+    private final DigitalInput sensor;
+    private final RelativeEncoder encoder;
+    private final SparkPIDController rotationController;
 
     public Intake() {
+        motor = new CANSparkMax(IntakeConstants.MOTOR_ID, MotorType.kBrushless);
         motor.setSmartCurrentLimit(IntakeConstants.CURRENT_LIMIT);
         motor.setIdleMode(IntakeConstants.IDLE_MODE);
 
+        sensor = new DigitalInput(IntakeConstants.SENSOR_CHANNEL);
+
+        encoder = motor.getEncoder();
+        rotationController = motor.getPIDController();
         rotationController.setP(IntakeConstants.P);
     }
 
     @Override
     public void periodic() {
-
-    }
-
-    public boolean getSensor() {
-        return sensor.get();
+        SmartDashboard.putBoolean("Has Note", this.hasNote());
+        SmartDashboard.putBoolean("Intaking", encoder.getVelocity() > 10);
     }
 
-    public boolean isNotePresent() {
-        return !getSensor();
+    public boolean hasNote() {
+        return !sensor.get();
     }
 
     public double getRPM() {

src/main/java/frc/robot/intake/commands/IntakePickup.java

@@ -20,7 +20,7 @@ public void initialize() {
 
     @Override
     public void execute() {
-        SmartDashboard.putBoolean("sensorSub", intake.getSensor());
+        SmartDashboard.putBoolean("sensorSub", intake.hasNote());
 
     }
 
@@ -31,6 +31,6 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        return !intake.getSensor();
+        return intake.hasNote();
     }
 }

src/main/java/frc/robot/shooter/Shooter.java

@@ -1,21 +1,29 @@
 package frc.robot.shooter;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
 
 public class Shooter extends SubsystemBase {
     private final CANSparkMax motor = new CANSparkMax(ShooterConstants.MOTOR_ID, MotorType.kBrushless);
+    private final RelativeEncoder encoder;
 
     public Shooter() {
         motor.setSmartCurrentLimit(ShooterConstants.CURRENT_LIMIT);
         motor.setIdleMode(ShooterConstants.IDLE_MODE);
+        motor.burnFlash();
+
+        encoder = motor.getEncoder();
+        encoder.setPositionConversionFactor(1);
     }
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(getName(), encoder.getVelocity());
     }
 
     public void run(double speed) {