Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: Team3309/FRC2023
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: master
Choose a base ref
...
head repository: Team3309/FRC2023
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: oldSwerveMethod
Choose a head ref
Able to merge. These branches can be automatically merged.
  • 4 commits
  • 8 files changed
  • 2 contributors

Commits on Nov 1, 2023

  1. old swerve method

    RoboChicken46 committed Nov 1, 2023
    Copy the full SHA
    07eb88b View commit details

Commits on Dec 8, 2023

  1. good

    RoboChicken46 committed Dec 8, 2023
    Copy the full SHA
    cb4db9a View commit details

Commits on Dec 30, 2023

  1. Distance Estimator

    RoboChicken46 committed Dec 30, 2023
    Copy the full SHA
    b6baee5 View commit details
  2. - Classifier Pipeline working

    - Testing Command for Classifier Pipeline working
    RoboChicken46 committed Dec 30, 2023
    Copy the full SHA
    33d8fd2 View commit details
3 changes: 3 additions & 0 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
{
"java.configuration.updateBuildConfiguration": "interactive"
}
2 changes: 1 addition & 1 deletion .wpilib/wpilib_preferences.json
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
{
"currentLanguage": "none",
"currentLanguage": "java",
"enableCppIntellisense": false,
"projectYear": "2023",
"teamNumber": 3309
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
@@ -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
26 changes: 14 additions & 12 deletions src/main/java/frc/robot/LimelightVision.java
Original file line number Diff line number Diff line change
@@ -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;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
@@ -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)
103 changes: 22 additions & 81 deletions src/main/java/frc/robot/Swerve/SwerveModule3309.java
Original file line number Diff line number Diff line change
@@ -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);
}
}
}




}
16 changes: 16 additions & 0 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.java
Original file line number Diff line number Diff line change
@@ -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
// -------------------------------------------------------------------------------------------------------------------------------------
17 changes: 17 additions & 0 deletions src/main/java/friarLib2/vision/ClassifierCamera.java
Original file line number Diff line number Diff line change
@@ -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");
}



}