Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Aarav Shah
2023-01-07 13:36:56 -07:00
committed by GitHub
commit 65c403586c
40 changed files with 2608 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/utility/LEDPatterns.java
+45
View File
@@ -0,0 +1,45 @@
package frc4388.utility;
/**
* Add your docs here.
*/
public enum LEDPatterns {
/* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* COLOR 1 PATTERNS */
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
/* COLOR 2 PATTERNS */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
/* COLOR 1 AND 2 PATTERNS */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
/* SOLID COLORS */
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
/* GETTERS/SETTERS */
private final float id;
LEDPatterns(float id) {
this.id = id;
}
public float getValue() {
return id;
}
}
src/main/java/frc4388/utility/RobotGyro.java
+180
View File
@@ -0,0 +1,180 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpiutil.math.MathUtil;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro extends GyroBase {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro){
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>NavX:
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements. It's important to
* make sure that the robot is not moving while the centering calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*
* <p>Pigeon:
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
* make sure that the robot is not moving while the tempurature calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
} else {
m_navX.calibrate();
}
}
@Override
public void reset() {
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
m_navX.reset();
}
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon(){
return m_pigeon;
}
public AHRS getNavX(){
return m_navX;
}
@Override
public void close() throws Exception {
}
}
src/main/java/frc4388/utility/RobotTime.java
+79
View File
@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
/**
* <p>Keeps track of Robot times like time passed, delta time, etc
* <p>All times are in milliseconds
*/
public class RobotTime {
private long m_currTime = System.currentTimeMillis();
public long m_deltaTime = 0;
private long m_startRobotTime = m_currTime;
public long m_robotTime = 0;
public long m_lastRobotTime = 0;
private long m_startMatchTime = 0;
public long m_matchTime = 0;
public long m_lastMatchTime = 0;
public long m_frameNumber = 0;
/**
* Private constructor prevents other classes from instantiating
*/
private RobotTime(){}
private static RobotTime instance = null;
/**
* Gets the instance of Robot Time. If there is no instance running one will be created.
* @return instance of Robot Time
*/
public static RobotTime getInstance() {
if (instance == null) {
instance = new RobotTime();
}
return instance;
}
/**
* Call this once per periodic loop.
*/
public void updateTimes() {
m_lastRobotTime = m_robotTime;
m_lastMatchTime = m_matchTime;
m_currTime = System.currentTimeMillis();
m_robotTime = m_currTime - m_startRobotTime;
m_deltaTime = m_robotTime - m_lastRobotTime;
m_frameNumber++;
if (m_startMatchTime != 0) {
m_matchTime = m_currTime - m_startMatchTime;
}
}
/**
* Call this in both the auto and periodic inits
*/
public void startMatchTime() {
if (m_startMatchTime == 0) {
m_startMatchTime = m_currTime;
}
}
/**
* Call this in disabled init
*/
public void endMatchTime() {
m_startMatchTime = 0;
m_matchTime = 0;
}
}
src/main/java/frc4388/utility/RobotUnits.java
+27
View File
@@ -0,0 +1,27 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.utility;
/** Aarav's good units class (better than WPILib)
* @author Aarav Shah */
public class RobotUnits {
// constants
// angle conversions
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
// falcon conversions
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
// distance conversions
public static double metersToFeet(final double meters) {return meters * 3.28084;}
public static double feetToMeters(final double feet) {return feet / 3.28084;}
}
src/main/java/frc4388/utility/controller/IHandController.java
+21
View File
@@ -0,0 +1,21 @@
package frc4388.utility.controller;
/**
* Add your docs here.
*/
public interface IHandController {
public double getLeftXAxis();
public double getLeftYAxis();
public double getRightXAxis();
public double getRightYAxis();
public double getLeftTriggerAxis();
public double getRightTriggerAxis();
public int getDpadAngle();
}
src/main/java/frc4388/utility/controller/XboxController.java
+218
View File
@@ -0,0 +1,218 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj.Joystick;
/**
* This is a wrapper for the WPILib Joystick class that represents an XBox
* controller.
* @author frc1675
*/
public class XboxController implements IHandController
{
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double DEADZONE = 0.1;
private Joystick m_stick;
/**
* Add your docs here.
*/
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
/**
* Add your docs here.
*/
public Joystick getJoyStick() {
return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
}
public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
/**
* Get the angle input from the dpad.
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
}
public boolean getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
}
public boolean getDPadTop(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
}
public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+69
View File
@@ -0,0 +1,69 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
import frc4388.utility.controller.XboxController;
/**
* Mapping for the Xbox controller triggers to allow triggers to be defined as
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
* exceeds a tolerance defined in {@link XboxController}.
*/
public class XboxTriggerButton extends Button {
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller;
private int m_trigger;
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
}
/** {@inheritDoc} */
@Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
else if (m_trigger == LEFT_TRIGGER) {
return m_controller.getLeftTrigger();
}
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
}
}