.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "java.configuration.updateBuildConfiguration": "interactive"
+}

.wpilib/wpilib_preferences.json

@@ -1,5 +1,5 @@
 {
-    "currentLanguage": "none",
+    "currentLanguage": "java",
     "enableCppIntellisense": false,
     "projectYear": "2023",
     "teamNumber": 3309

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/LimelightVision.java

@@ -1,6 +1,9 @@
 package frc.robot;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import friarLib2.vision.LimelightCamera;
@@ -28,29 +31,28 @@ public static Command SetPipelineCommand(int index)
     public static IVisionCamera shooterCamera = new LimelightCamera();
 
     private static final PixelToAngle ANGLE_CONVERTER = new PixelToAngle(320, 240, 54, 41); // Constants for the limelight 2
-    private static final double HEIGHT_OF_CAMERA = 0.7747; // Meters
-    private static final double ANGLE_OF_CAMERA = 34.8; // Degrees
+    private static final double HEIGHT_OF_CAMERA = 49.61; // inches
+    private static final double ANGLE_OF_CAMERA = 160; // Degrees
 
     private static double lastDistance = 0; // Return this if the robot does not have a target
 
     /**
      * @return the distance in meters from the target
      */
-    public static double getMetersFromTarget () {
+    public static double getInchesFromTarget () {
         try {
-            double a2;
-            if (shooterCamera instanceof PhotonCameraWrapper) {
-                a2 = shooterCamera.getBestTarget().getY();
-            } else {
-                a2 = ANGLE_CONVERTER.calculateYAngle(shooterCamera.getBestTarget());
-            }
+            NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+            NetworkTableEntry ty = table.getEntry("ty");
+            double a2 = ty.getDouble(0.0);
             double a1 = ANGLE_OF_CAMERA;
             double h1 = HEIGHT_OF_CAMERA;
-            double h2 = Units.inchesToMeters(12*8 + 8); // Height of target
+            double h2 = 26; // Place holder Height of target
 
-            lastDistance = (h2 - h1) / Math.tan(Math.toRadians(a1 + a2));
-        } catch (IndexOutOfBoundsException e) {} // If the camera has no target
+            double angleToGoal = (a1 + a2);
+            double angleToGoalRadian = Math.toRadians(angleToGoal);
+            lastDistance = (h2-h1) / Math.tan(angleToGoalRadian);
 
+        } catch (IndexOutOfBoundsException e) {} // If the camera has no target the target has no 
         return lastDistance;
     }
 }

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

@@ -146,6 +146,7 @@ private void ConfigureBindings()
     private void SetDefaultCommands()
     {
         _Drive.setDefaultCommand(new DriveTeleop(_Drive));
+        // _Arm.setDefaultCommand(_Arm.Command_testClassy());
 
 //        Arm.setDefaultCommand(
 //             Arm.Command_SetPosition(ArmSubsystem.ArmPosition.Stowed)

src/main/java/frc/robot/Swerve/SwerveModule3309.java

@@ -84,11 +84,11 @@ public class SwerveModule3309 implements SwerveModule {
      * @param encoderID CAN ID for the module's CANCoder
      * @param name The module's name (used when outputting to SmartDashboard)
      */
-    public SwerveModule3309 (double steeringOffset, int driveMotorID, int steeringMotorID, int encoderID, String name)
-    {
+    public SwerveModule3309 (double steeringOffset, int driveMotorID, int steeringMotorID, int encoderID, String name) {
         this.name = name;
-
-        ConfigMotors(driveMotorID, steeringMotorID);
+        driveMotor = new WPI_TalonFX(driveMotorID);
+        steeringMotor = new WPI_TalonFX(steeringMotorID);
+        configMotors();
 
         this.steeringOffset = steeringOffset;
 
@@ -112,79 +112,24 @@ public SwerveModule3309 (double steeringOffset, int driveMotorID, int steeringMo
      * @param IDs The collection of CAN ID's for the module
      * @param name The module's name (used when outputting to SmartDashboard)
      */
-    public SwerveModule3309 (double steeringOffset, SwerveCANIDs IDs, String name)
-    {
+    public SwerveModule3309 (double steeringOffset, SwerveCANIDs IDs, String name) {
         this(steeringOffset, IDs.driveMotorID, IDs.steeringMotorID, IDs.CANCoderID, name);
     }
 
     /**
      * Sets the motor PID values to those which will make the robot move the way we want.
      */
-    public void ConfigMotors(int driveMotorID, int steeringMotorID) {
-        driveMotor = ConfigureMotor(driveMotorID,
-                DRIVE_PID_GAINS,
-                1,
-                10000,
-                10000);
+    public void configMotors () {
+        driveMotor.configFactoryDefault();
+        DRIVE_PID_GAINS.configureMotorPID(driveMotor);
+        driveMotor.config_IntegralZone(0, 500);
+        driveMotor.setNeutralMode(NeutralMode.Brake);
         driveMotor.configSupplyCurrentLimit(DRIVE_MOTOR_CURRENT_LIMIT);
 
-        steeringMotor = ConfigureMotor(steeringMotorID,
-                STEERING_PID_GAINS,
-                1,
-                10000,
-                10000);
-    }
-
-    private WPI_TalonFX ConfigureMotor(
-              int motorId
-            , PIDParameters pidConstants
-            , double peakOutput
-            , double acceleration
-            , double cruise)
-    {
-        WPI_TalonFX motor = new WPI_TalonFX(motorId);
-
-        // --Factory default hardware to prevent unexpected behavior
-        motor.configFactoryDefault();
-
-        // -- Configure Sensor Source for Pirmary PID
-        motor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, Constants.TIMEOUT_MS);
-
-        // -- set deadband to super small 0.001 (0.1 %).
-        //    The default deadband is 0.04 (4 %)
-        motor.configNeutralDeadband(0.001, Constants.TIMEOUT_MS);
-        motor.setNeutralMode(NeutralMode.Brake);
-
-        //motor.setSensorPhase(false);
-        //motor.setInverted(true);
-
-        // -- Set relevant frame periods to be at least as fast as periodic rate
-        motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, Constants.TIMEOUT_MS);
-        motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, Constants.TIMEOUT_MS);
-
-        // -- Set the peak and nominal outputs
-        motor.configNominalOutputForward(0, Constants.TIMEOUT_MS);
-        motor.configNominalOutputReverse(0, Constants.TIMEOUT_MS);
-        motor.configPeakOutputForward(peakOutput, Constants.TIMEOUT_MS);
-        motor.configPeakOutputReverse(-peakOutput, Constants.TIMEOUT_MS);
-
-        // -- Motion Magic
-        motor.selectProfileSlot(pidConstants.GetSlotIndex(), 0);
-        motor.config_kP(pidConstants.GetSlotIndex(), pidConstants.GetP(), Constants.TIMEOUT_MS);
-        motor.config_kI(pidConstants.GetSlotIndex(), pidConstants.GetI(), Constants.TIMEOUT_MS);
-        motor.config_kD(pidConstants.GetSlotIndex(), pidConstants.GetD(), Constants.TIMEOUT_MS);
-        motor.config_kF(pidConstants.GetSlotIndex(), pidConstants.GetF(), Constants.TIMEOUT_MS);
-        motor.config_IntegralZone(pidConstants.GetSlotIndex(), pidConstants.GetIZone(), Constants.TIMEOUT_MS);
-
-        // -- Ramp speeds
-        motor.configMotionCruiseVelocity(cruise, Constants.TIMEOUT_MS);
-        motor.configMotionAcceleration(acceleration, Constants.TIMEOUT_MS);
-        motor.configMotionSCurveStrength(4);
-
-        // -- Zero the sensor once on robot boot up
-        motor.setSelectedSensorPosition(0, 0, Constants.TIMEOUT_MS);
-
-        return motor;
+        steeringMotor.configFactoryDefault();
+        STEERING_PID_GAINS.configureMotorPID(steeringMotor);
+        steeringMotor.config_IntegralZone(0, 500);
+        steeringMotor.setNeutralMode(NeutralMode.Brake);
     }
 
     /**
@@ -218,7 +163,7 @@ public SwerveModuleState getState () {
 
     public SwerveModulePosition getPosition () {
         return new SwerveModulePosition(
-            Conversions.encoderTicksToMeters(driveMotor.getSelectedSensorPosition()), 
+            Conversions.encoderTicksPer100msToMps(driveMotor.getSelectedSensorPosition()), 
             Rotation2d.fromDegrees(getSteeringDegreesFromFalcon())
         );
     }
@@ -232,6 +177,7 @@ public void zeroPosition () {
         driveMotor.setSelectedSensorPosition(0);
     }
 
+
     /**
      * @return If the belts for the steering axis have slipped
      */
@@ -295,14 +241,14 @@ private double getSteeringDegreesFromEncoder () {
         return Conversions.encoderTicksToDegreesCANCoder(steeringEncoder.getPosition());
     }
 
+
     @Override
     public void outputToDashboard () {
         SmartDashboard.putNumber(name + " CANCoder absolute value", steeringEncoder.getAbsolutePosition());
         SmartDashboard.putNumber(name + " CANCoder raw value", steeringEncoder.getPosition());
         SmartDashboard.putNumber(name + " CANCoder degrees", getSteeringDegreesFromEncoder());
         SmartDashboard.putNumber(name + " Falcon degrees", getSteeringDegreesFromFalcon());
         SmartDashboard.putNumber(name + " Falcon raw value", steeringMotor.getSelectedSensorPosition());
-        SmartDashboard.putNumber(name + " Position", getPosition().distanceMeters);
         SmartDashboard.putBoolean(name + " has slipped", steeringHasSlipped());
     }
 
@@ -319,15 +265,6 @@ public static double encoderTicksPer100msToMps (double encoderTicksPer100ms) {
             return encoderTicksPer100ms / mpsToEncoderTicksPer100ms(1);
         }
 
-        public static double metersToEncoderTicks (double meters) {
-            double wheelDiameterMeters = Units.inchesToMeters(WHEEL_DIAMETER_INCHES);
-            return meters * (1.0/(wheelDiameterMeters * Math.PI)) * DRIVE_GEAR_RATIO * (2048.0/1.0);
-        }
-
-        public static double encoderTicksToMeters (double encoderTicks) {
-            return encoderTicks / metersToEncoderTicks(1);
-        }
-
         public static double degreesToEncoderTicksFalcon (double degrees) {
             return degrees * (2048.0 / 360.0) * STEERING_GEAR_RATIO_FALCON;
         }
@@ -344,4 +281,8 @@ public static double encoderTicksToDegreesCANCoder (double encoderTicks) {
             return encoderTicks / degreesToEncoderTicksCANCoder(1);
         }
     }
-}
+
+
+
+
+}

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

@@ -16,6 +16,7 @@
 import frc.robot.OI;
 import friarLib2.math.FriarMath;
 import friarLib2.utility.PIDParameters;
+import friarLib2.vision.ClassifierCamera;
 
 import java.util.Map;
 import java.util.concurrent.atomic.AtomicInteger;
@@ -513,6 +514,21 @@ public CommandBase Command_TEST_MOVE_ARM(Joint joint, double pct)
         });
     }
 
+   public Command Command_testClassy()
+   {
+    return runOnce(() ->
+    {
+        if (ClassifierCamera.ClassifierClass().equals("1 No Aidan"))
+        {
+            ClampSolenoid.set(DoubleSolenoid.Value.kForward);
+        }
+        else if (ClassifierCamera.ClassifierClass().equals("0 Aidan"))
+        {
+            ClampSolenoid.set(DoubleSolenoid.Value.kReverse);
+        }
+   });
+   }
+
     // -------------------------------------------------------------------------------------------------------------------------------------
     // -- Internal Commands
     // -------------------------------------------------------------------------------------------------------------------------------------

src/main/java/friarLib2/vision/ClassifierCamera.java

@@ -0,0 +1,17 @@
+package friarLib2.vision;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+//wrapper for the JSON Dump Specification of the Neural Classifier Results from the Networks
+
+public class ClassifierCamera
+{
+
+    public static String ClassifierClass()
+    {
+        return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tclass").getString("null");
+    }
+
+
+
+}