Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

added LL1 FOM #136

Merged
merged 2 commits into from
Nov 9, 2024
Merged

added LL1 FOM #136

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
151ce37
added LL1 FOM
trixydevs Nov 9, 2024
000986a
Merge branch 'limelightMerging' into liamsmergingFOMbranch
trixydevs Nov 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import static frc.robot.settings.Constants.DriveConstants.FR_DRIVE_MOTOR_ID;
import static frc.robot.settings.Constants.DriveConstants.FR_STEER_ENCODER_ID;
import static frc.robot.settings.Constants.DriveConstants.FR_STEER_MOTOR_ID;
import static frc.robot.settings.Constants.DriveConstants.MAX_VELOCITY_METERS_PER_SECOND;
import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kD;
import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kI;
import static frc.robot.settings.Constants.ShooterConstants.AUTO_AIM_ROBOT_kP;
Expand All @@ -34,6 +35,7 @@
//import java.util.logging.Logger;

import com.ctre.phoenix6.hardware.Pigeon2;
import com.google.flatbuffers.DoubleVector;
import com.pathplanner.lib.util.PathPlannerLogging;

import edu.wpi.first.math.Matrix;
Expand Down Expand Up @@ -64,6 +66,7 @@
import frc.robot.commands.AngleShooter;
import frc.robot.commands.RotateRobot;
import frc.robot.helpers.MotorLogger;
import frc.robot.helpers.MythicalMath;
import frc.robot.settings.Constants;
import frc.robot.settings.Constants.CTREConfigs;
import frc.robot.settings.Constants.DriveConstants;
Expand Down Expand Up @@ -620,4 +623,17 @@ private Pose3d robotToFieldCoordinates(Pose3d robotCoordinates)
return new Pose3d(robotCoordinatesX+odometrPoseX,robotCoordinatesY+odometrPosey,robotCoordinatesZ, robotCoordinatesAngle);
}

public double getLL1FOM() //larger fom is BAD, and is less trustworthy.
{
int maximumNumbOfTags = 2;
double maxTagDist = 10.0;

double VelocityContributer = (MythicalMath.DistanceFromOrigin3d(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond,0.0)/MAX_VELOCITY_METERS_PER_SECOND);
double centeredContributer = (limelight.getAprilValues(APRILTAG_LIMELIGHT2_NAME).tx); //I think this only gets up to 1???
double numTagsContributer = maximumNumbOfTags/limelight.getLLTagCount(APRILTAG_LIMELIGHT2_NAME);
double distanceContributer = (LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME).avgTagDist/maxTagDist);

return VelocityContributer*centeredContributer*numTagsContributer*distanceContributer;
}

}
18 changes: 18 additions & 0 deletions src/main/java/frc/robot/subsystems/Limelight.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,7 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
double limelight2y = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getY();
double limelight2x = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getX();
double limelight2z = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getZ();

// use those coordinates to find the distance to the closest apriltag for each limelight
double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1x, limelight1y, limelight1z);
double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2x, limelight2y, limelight2z);
Expand All @@ -136,6 +137,7 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
Pose2d scaledPose2 = MythicalMath.multiplyOnlyPos(pose2.pose, confidenceSource2);
Pose2d newPose = MythicalMath.divideOnlyPos((MythicalMath.addOnlyPosTogether(scaledPose1, scaledPose2)), (confidenceSource1+confidenceSource2));
//Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);

pose1.pose = newPose;
return pose1;
}
Expand Down Expand Up @@ -210,6 +212,22 @@ public LimelightDetectorData getNeuralDetectorValues() {
LimelightHelpers.getTV(OBJ_DETECTION_LIMELIGHT_NAME));
}

public LimelightDetectorData getAprilValues(String cameraname) {
return new LimelightDetectorData(
LimelightHelpers.getTX(cameraname),
LimelightHelpers.getTY(cameraname),
LimelightHelpers.getTA(cameraname),
LimelightHelpers.getNeuralClassID(cameraname),
LimelightHelpers.getTV(cameraname));
}

public int getLLTagCount(String cameraname)
{
return LimelightHelpers.getLatestResults(cameraname).targetingResults.targets_Fiducials.length;
}



private boolean isValid(String limelightName, PoseEstimate estimate) {
Boolean valid = (
estimate.pose.getX() < FIELD_CORNER.getX() &&
Expand Down