diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index b43033d..caadf59 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -13,9 +13,33 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; public class ArmSubsystem extends SubsystemBase { + //nasty limelight slop + NetworkTable limelightBack; // table for the limelight + NetworkTableEntry tid; // Table for the ID of target + NetworkTableEntry ty; // Table for the skew of the robot on a vertical axis +/* + + + + +double TAG_ID = getID(); +double DISTANCE = DISTANCE_CALCULATIONS(); + @Override + public void periodic(){ + //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high +System.out.println(DISTANCE_CALCULATIONS()); +System.out.println(getID()); +if(getID() == 1.0){ + setArmPose(55); + } +} +*/ // Declare a variable for the motor you want to control private final TalonFX m_arm; @@ -24,6 +48,12 @@ public class ArmSubsystem extends SubsystemBase { public ArmSubsystem() { + //more yummy slop + limelightBack = NetworkTableInstance.getDefault().getTable("limelight"); + tid = limelightBack.getEntry("tid"); + ty = limelightBack.getEntry("ty"); + + // Initialize the motor in the constructor with the motor ID and optional canbus // ID m_arm = new TalonFX(Constants.CANIDs.ARM_ID); @@ -104,6 +134,8 @@ public ArmSubsystem() { } /* End of the class-method */ + + public StatusSignal getArmPos() { /* Reusing from drivetrain subsystem */ return m_arm.getPosition(); @@ -121,6 +153,36 @@ public void setArmPose(double armPose) { } + //MMMMMMMMMMMMMMMMMMMMMM YUMMY SLOP slorp slorp slorp slorp + //gets the ID of the selected apriltag + public double getID(){ + return tid.getDouble(-1.0); + } + + // distance from the bot to the april tag in centimeters using trig + //CHANGE 170 to 122 after TEST!!!! !! ! !!!! + public double DISTANCE_CALCULATIONS(){ + double angleToGoalRadians = (ty.getDouble(0.0)) * (3.14159/180.0); + return (170 - CAMERA_HEIGHT) / Math.tan(angleToGoalRadians); + } + + double CAMERA_HEIGHT = 156; + //crescendo bot height from floor to middle of LL camera is 56 + //CHANGE POST TEST + double CAMERA_ANGLE = 28.61; + + double TAG_ID = getID(); +double DISTANCE = DISTANCE_CALCULATIONS(); + @Override + public void periodic(){ + //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high +System.out.println(DISTANCE_CALCULATIONS()); +System.out.println(getID()); +// proof of concept, please work +if(getID() == 1.0){ + setArmPose(55); + } +} /* * Currently only being called in subsystem-command; @@ -133,20 +195,8 @@ public void showArmTelemetry() { SmartDashboard.putNumber("Arm Voltage", m_arm.getMotorVoltage().getValue()); } - VisionSubsystem vision = new VisionSubsystem(); - @Override - public void periodic(){ - double TAG_ID = vision.getID(); - double DISTANCE = vision.DISTANCE_CALCULATIONS(); - //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high - if((TAG_ID == 1.0) - //&&(DISTANCE <= 100) - ){ - setArmPose(55); - } -} } // end of ArmSubsystem method /*************************** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index f2deec2..b2b2cba 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -233,17 +233,15 @@ public Pose2d getTargetPose(){ } } */ -/*ArmSubsystem arm = new ArmSubsystem(); +ArmSubsystem arm = new ArmSubsystem(); @Override public void periodic(){ - double TAG_ID = getID(); - double DISTANCE = DISTANCE_CALCULATIONS(); //if tag ID = 6 or 5 and the tag is a meter or less away, set arm pose to high - if((TAG_ID == 1.0)&&(DISTANCE <= 100)){ + if((getID() == 1.0)&&(DISTANCE_CALCULATIONS() <= 100)){ arm.setArmPose(55); } -}*/ +} } // end of class \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json index 15fc17c..c8c5149 100644 --- a/vendordeps/playingwithfusion2024.json +++ b/vendordeps/playingwithfusion2024.json @@ -1,7 +1,7 @@ { "fileName": "playingwithfusion2024.json", "name": "PlayingWithFusion", - "version": "2024.01.28", + "version": "2024.03.09", "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", "frcYear": "2024", "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", @@ -12,14 +12,14 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-java", - "version": "2024.01.28" + "version": "2024.03.09" } ], "jniDependencies": [ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-cpp", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": false, "libName": "PlayingWithFusion", @@ -53,7 +53,7 @@ { "groupId": "com.playingwithfusion.frc", "artifactId": "PlayingWithFusion-driver", - "version": "2024.01.28", + "version": "2024.03.09", "headerClassifier": "headers", "sharedLibrary": true, "libName": "PlayingWithFusionDriver",