116 lines
4.5 KiB
Java
116 lines
4.5 KiB
Java
package frc.robot.utilities;
|
|
|
|
import java.util.Optional;
|
|
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
import frc.robot.constants.CompetitionConstants;
|
|
|
|
public class Utilities {
|
|
public static final double kG = -9.81;
|
|
|
|
/**
|
|
* Returns the Alliance enumeration that indicates who will have the first
|
|
* shift. Returns null if the data is not available.
|
|
*
|
|
* @return The Alliance that will have the first shift, or null if game specific data
|
|
* is not present
|
|
*/
|
|
public static Alliance whoHasFirstShift() {
|
|
String gameData = DriverStation.getGameSpecificMessage();
|
|
|
|
if(gameData.length() > 0) {
|
|
switch (gameData.charAt(0)) {
|
|
case 'B':
|
|
return Alliance.Red;
|
|
case 'R':
|
|
return Alliance.Blue;
|
|
default:
|
|
return null;
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
/**
|
|
* Returns the pose of the hub for the given alliance assigned to our robot.
|
|
* If no alliance is assigned (which is unlikely) this method returns
|
|
* the Blue hub pose, which is the closet to the field origin
|
|
*
|
|
* @return The Pose2d object which represents the appropriate pose of the Hub
|
|
*/
|
|
public static Pose2d getHubPose() {
|
|
Optional<Alliance> alliance = DriverStation.getAlliance();
|
|
|
|
if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
|
|
return CompetitionConstants.kBlueHubLocation;
|
|
} else {
|
|
return CompetitionConstants.kRedHubLocation;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Returns the AprilTag ID of the tag that is in the center of the hub
|
|
* for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
|
|
* the Blue hub's center tag is returned.
|
|
*
|
|
* @return The tag ID that is in the center of the assigned alliance's hub
|
|
*/
|
|
public static int getHubCenterAprilTagID() {
|
|
Optional<Alliance> alliance = DriverStation.getAlliance();
|
|
|
|
if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
|
|
return 26;
|
|
} else {
|
|
return 10;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* A ChatGPT possible hallucination related to calcuating whether a shot is possible
|
|
* for a given speed and change in X and Y position
|
|
*
|
|
* Note that X in this scenario is the physical distance from the shooter exit to
|
|
* target. Y is the change in height from the shooter exit to the target height
|
|
*
|
|
* @param targetVMPS The target velocity of the shooter in Meters per Second
|
|
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
|
|
* @param deltaYM The height difference between the shooter exit and the target
|
|
* @return A true or false value indicating whether the shot is possible
|
|
*/
|
|
public static boolean shotPossible(double targetVMPS, double deltaXM, double deltaYM) {
|
|
return Math.pow(targetVMPS, 4) >=
|
|
-9.81 * (-9.81 * Math.pow(deltaXM, 2) + 2 * deltaYM * Math.pow(targetVMPS, 2));
|
|
}
|
|
|
|
/**
|
|
* A ChatGPT possible hallucination that calculates the angle required to make a shot for
|
|
* a given speed and change in X and Y position
|
|
*
|
|
* Note that X in this scenario is the physical disatance from the shooter exit to
|
|
* target. Y is the change in height from the shooter exit to the target height
|
|
*
|
|
* Setting softerShot to true changes the angle of attack to a soft, long range shot. False
|
|
* makes the shot more of a lob
|
|
*
|
|
* @param targetVMPS The target velocity of the shooter in Meters per Second
|
|
* @param deltaXM The "as the crow flies" distance between the shooter exit and the target
|
|
* @param deltaYM The height difference between the shooter exit and the target
|
|
* @param softerShot True for a long range shot, false for a short range lob
|
|
* @return The angle required of the shooter to make the shot described by the input parameters
|
|
*/
|
|
public static double shotAngle(double targetVMPS, double deltaXM, double deltaYM, boolean softerShot) {
|
|
double vPow2 = Math.pow(targetVMPS, 2);
|
|
double vPow4 = Math.pow(targetVMPS, 4);
|
|
double sqrtPiece = Math.sqrt(vPow4 - kG * (kG * Math.pow(deltaXM, 2) + 2 * deltaYM * vPow2));
|
|
|
|
if(softerShot) {
|
|
return Math.atan((vPow2 - sqrtPiece) / (kG * deltaXM));
|
|
} else {
|
|
return Math.atan((vPow2 + sqrtPiece) / (kG * deltaXM));
|
|
}
|
|
}
|
|
}
|