diff --git a/2022 Season/src/main/java/frc/robot/Lib/InterpolationTable.java b/2022 Season/src/main/java/frc/robot/Lib/InterpolationTable.java
index 22723b4..7d21c7f 100644
--- a/2022 Season/src/main/java/frc/robot/Lib/InterpolationTable.java
+++ b/2022 Season/src/main/java/frc/robot/Lib/InterpolationTable.java
@@ -10,12 +10,21 @@ public class InterpolationTable {
private final double[][] referencePoints;
private final int propertyCount;
+ /**
+ * Constructor for the InterpolationTable
+ * @param referencePoints - The reference points to put into the interpolation table
+ */
public InterpolationTable(double[][] referencePoints) {
Arrays.sort(referencePoints, INPUT_SORTER);
this.referencePoints = referencePoints;
propertyCount = referencePoints[0].length - 1;
}
+ /**
+ * Loops through the reference points and gets the desired list based on the input
+ * @param input - Input used to determine the desired list
+ * @return - Desired list of reference points
+ */
public double[] sample(double input) {
int leftBoundIndex = 0;
int rightBoundIndex = referencePoints.length - 1;
@@ -54,6 +63,14 @@ public double[] sample(double input) {
return outputs;
}
+ /**
+ * Returns a new output by calculating the interpolation between the two points
+ * @param input - The input used to determine the interpolation
+ * @param outputProperty - Index used to calculate the slope of point1 and point2
+ * @param point1 - List of the first point
+ * @param point2 - List of the second point
+ * @return - new interpolated value.
+ */
private double interpolate(double input, int outputProperty, double[] point1, double[] point2) {
outputProperty++;
double slope = (point2[outputProperty] - point1[outputProperty]) / (point2[0] - point1[0]);
diff --git a/2022 Season/src/main/java/frc/robot/Lib/Photoeye.java b/2022 Season/src/main/java/frc/robot/Lib/Photoeye.java
index ab55f5f..805f1b6 100644
--- a/2022 Season/src/main/java/frc/robot/Lib/Photoeye.java
+++ b/2022 Season/src/main/java/frc/robot/Lib/Photoeye.java
@@ -3,12 +3,19 @@
import edu.wpi.first.wpilibj.DigitalInput;
public class Photoeye extends DigitalInput {
-
+ /**
+ * Constructor for the Photoeye
+ * @param port - Port to read the photoeye digital input from
+ */
public Photoeye(int port)
{
super(port);
}
+ /**
+ * Get the state of the photoeye
+ * @return true | false
+ */
public boolean isBlocked()
{
return !this.get();
diff --git a/2022 Season/src/main/java/frc/robot/Subsystems/Climber.java b/2022 Season/src/main/java/frc/robot/Subsystems/Climber.java
index 13507c3..5d1a665 100644
--- a/2022 Season/src/main/java/frc/robot/Subsystems/Climber.java
+++ b/2022 Season/src/main/java/frc/robot/Subsystems/Climber.java
@@ -62,6 +62,9 @@ public Climber(){
_climbTarget = Constants.kClimbPullUpPosition;
}
+ /**
+ * Resets the climber encoder only if the limitSwitch is pressed
+ */
public void resetClimbEncoder()
{
if (isLimitSwitchPressed())
@@ -70,22 +73,36 @@ public void resetClimbEncoder()
}
}
+ /**
+ * Checks if climber limit switch is pressed
+ * @return - true if limit switch is pressed
+ */
public boolean isLimitSwitchPressed()
{
return _climbLimitSwitch.isPressed();
}
+ /**
+ * Sets a new climberTarget based on the position provided
+ * @param position - Position to set climberTarget to
+ */
public void setClimbPosition(double position)
{
_climbTarget = position;
}
+ /**
+ * Sets the left and right climber motor to -1
+ */
public void extend()
{
_leftClimbMotor.set(-1);
_rightClimbMotor.set(-1);
}
+ /**
+ * Moves both the left and right climber motors respectively based on the value of the climberEncoder
+ */
public void climb()
{
if (_climbEncoder.get() <= 1) {
@@ -97,53 +114,85 @@ public void climb()
}
}
+ /**
+ * Stops moving the climber motors
+ */
public void stopClimb()
{
_leftClimbMotor.set(0);
_rightClimbMotor.set(0);
}
+ /**
+ * Checks if the climberEncoder value is greater than or equals to the climberTarget values
+ * @return - true if the climberTarger has been reached or passed
+ */
public boolean isExtendFinished()
{
return _climbEncoder.get() >= _climbTarget;
}
+ /**
+ * Sets the speed of both the left and right climber motors
+ * @param speed - Speed you would like to set the climber motors
+ */
public void setClimberMotor(double speed)
{
_leftClimbMotor.set(speed);
_rightClimbMotor.set(speed);
}
+ /**
+ * Sets the climber forward
+ */
public void setClimberForward()
{
_climber.set(Value.kForward);
}
+ /**
+ * Sets the climber in reverse
+ */
public void setClimberReverse()
{
_climber.set(Value.kReverse);
}
+ /**
+ * Sets the climber off
+ */
public void setClimberOff()
{
_climber.set(Value.kOff);
}
+ /**
+ * Sets the ratchet in reverse
+ */
public void engageRatchet()
{
_ratchet.set(Value.kReverse);
}
+ /**
+ * Sets the ratchet forward
+ */
public void disengageRatchet()
{
_ratchet.set(Value.kForward);
}
+ /**
+ * Sets the ratchet off
+ */
public void setRatchetOff()
{
_climber.set(Value.kOff);
}
+ /**
+ * Logs the data of the climber to the SmartDashboard
+ */
public void LogTelemetry() {
// TODO Auto-generated method stub
@@ -153,6 +202,9 @@ public void LogTelemetry() {
SmartDashboard.putNumber("rightClimbCurrent", _rightClimbMotor.getOutputCurrent());
}
+ /**
+ * Reads the data of the climber from the SmartDashboard... This function is not implemented yet
+ */
public void ReadDashboardData() {
// TODO Auto-generated method stub
diff --git a/2022 Season/src/main/java/frc/robot/Subsystems/Collector.java b/2022 Season/src/main/java/frc/robot/Subsystems/Collector.java
index 536eb1c..fd58128 100644
--- a/2022 Season/src/main/java/frc/robot/Subsystems/Collector.java
+++ b/2022 Season/src/main/java/frc/robot/Subsystems/Collector.java
@@ -30,6 +30,10 @@ public class Collector implements ISubsystem {
private boolean _bottomChamberState = false;
private boolean _topChamberState = false;
+ /**
+ * Get the Collector instance
+ * @return - Instance of collector
+ */
public static Collector GetInstance()
{
if (_instance == null)
@@ -39,6 +43,9 @@ public static Collector GetInstance()
return _instance;
}
+ /**
+ * Constructor for the Collector
+ */
public Collector()
{
_collectorMotor = new CANSparkMax(Constants.kCollectMotorDeviceId, MotorType.kBrushless);
@@ -72,11 +79,17 @@ public void stopCollect() {
_collectorMotor.set(0.0);
}
+ /**
+ * Stops the collector chamber back and front motors
+ */
public void stopChamber() {
_frontChamberMotor.set(0.0);
_backChamberMotor.set(0.0);
}
+ /**
+ * Moves the collector chamber back and front motors, to collect the cargo
+ */
public void intake() {
_collectorSolenoid.set(true);
_collectorMotor.set(-_collectorSpeed);
@@ -86,10 +99,17 @@ public void intake() {
_backChamberMotor.set(-_chamberSpeed);
}
}
+
+ /**
+ * Moves the collector motors, to spit the cargo
+ */
public void spit() {
_collectorMotor.set(_collectorSpeed);
}
+ /**
+ * Moves the collector chamber back and front motors, to eject the cargo
+ */
public void eject() {
_collectorSolenoid.set(true);
_collectorMotor.set(_collectorSpeed);
@@ -97,6 +117,9 @@ public void eject() {
_backChamberMotor.set(-_chamberSpeed);
}
+ /**
+ * Moves the cargo using the front and back chamber motors, and increase ball count accordingly
+ */
public void feed() {
_frontChamberMotor.set(-_chamberSpeed);
_backChamberMotor.set(_chamberSpeed);
@@ -119,11 +142,18 @@ public void feed() {
_bottomChamberState = _chamberBottomSensor.isBlocked();
}
+ /**
+ * Sets the ballCount in the collector based on the {@param ballCount}
+ * @param ballCount - The number of balls in the collector
+ */
public void setBallCount(int ballCount)
{
_ballCount = ballCount;
}
+ /**
+ * Runs mutiple if conditions that determine the ball count and whether or not the chamber will stopped
+ */
public void index()
{
if (_chamberTopSensor.isBlocked())
@@ -156,19 +186,31 @@ public void index()
_bottomChamberState = _chamberBottomSensor.isBlocked();
}
+ /**
+ * Moves the collected front and back motors in the negative direction of the chamber speed.
+ */
public void chamberUp() {
_frontChamberMotor.set(-_chamberSpeed);
_backChamberMotor.set(-_chamberSpeed);
}
+ /**
+ * Sets the collector solenoid output to true
+ */
public void collectorDown() {
_collectorSolenoid.set(true);
}
+ /**
+ * Sets the collector solenoid output to false
+ */
public void collectorUp() {
_collectorSolenoid.set(false);
}
+ /**
+ * Logs collector data into the SmartDashboard
+ */
public void LogTelemetry() {
// TODO Auto-generated method stub
@@ -181,6 +223,9 @@ public void LogTelemetry() {
SmartDashboard.putNumber("ballCount", _ballCount);
}
+ /**
+ * Reads collector data from SmartDashboard... At the moment this function is not implemented
+ */
public void ReadDashboardData() {
// TODO Auto-generated method stub
diff --git a/2022 Season/src/main/java/frc/robot/Subsystems/Drivetrain.java b/2022 Season/src/main/java/frc/robot/Subsystems/Drivetrain.java
index 4698474..78ecab7 100644
--- a/2022 Season/src/main/java/frc/robot/Subsystems/Drivetrain.java
+++ b/2022 Season/src/main/java/frc/robot/Subsystems/Drivetrain.java
@@ -49,6 +49,10 @@ public class Drivetrain implements ISubsystem {
private double _targetAngle = 7769;
+ /**
+ * Returns the singleton instance of the Drivetrain
+ * @return - Instance of the DriveTrain
+ */
public static Drivetrain GetInstance()
{
if (_instance == null)
@@ -58,6 +62,9 @@ public static Drivetrain GetInstance()
return _instance;
}
+ /**
+ * Constructor for the Drivetrain
+ */
public Drivetrain()
{
_leftFrontMotor = new CANSparkMax(Constants.kLeftFrontDriveDeviceId, MotorType.kBrushless);
@@ -74,12 +81,12 @@ public Drivetrain()
if (Constants.kCompetitionRobot) {
- _leftMiddleMotor = new CANSparkMax(Constants.kLeftMiddleDriveDeviceId, MotorType.kBrushless);
- _rightMiddleMotor = new CANSparkMax(Constants.kRightMiddleDriveDeviceId, MotorType.kBrushless);
- _rightMiddleMotor.setInverted(true);
+ _leftMiddleMotor = new CANSparkMax(Constants.kLeftMiddleDriveDeviceId, MotorType.kBrushless);
+ _rightMiddleMotor = new CANSparkMax(Constants.kRightMiddleDriveDeviceId, MotorType.kBrushless);
+ _rightMiddleMotor.setInverted(true);
- _leftMiddleMotor.follow(_leftFrontMotor);
- _rightMiddleMotor.follow(_rightFrontMotor);
+ _leftMiddleMotor.follow(_leftFrontMotor);
+ _rightMiddleMotor.follow(_rightFrontMotor);
}
_leftFrontMotor.setSmartCurrentLimit(60);
@@ -135,6 +142,9 @@ public Pose2d getPose() {
return _odometry.getPoseMeters();
}
+ /**
+ * Updates the odometry Pose
+ */
public void updatePose() {
_odometry.update(_gyro.getRotation2d(), _leftDriveEncoder.getDistance(), _rightDriveEncoder.getDistance());
}
@@ -148,11 +158,19 @@ public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(_leftDriveEncoder.getRate(), _rightDriveEncoder.getRate());
}
+ /**
+ * Resets the odometry to the specified pose.
+ *
+ * @param pose The pose to which to set the odometry.
+ */
public void resetOdometry(Pose2d pose) {
resetEncoders();
_odometry.resetPosition(pose, _gyro.getRotation2d());
}
+ /**
+ * Resets the odometry.
+ */
public void resetOdometry()
{
resetEncoders();
@@ -167,10 +185,10 @@ public void resetOdometry()
*/
public void tankDriveVolts(double leftVolts, double rightVolts) {
_rightFrontMotor.setIdleMode(IdleMode.kBrake);
- _rightMiddleMotor.setIdleMode(IdleMode.kBrake);
+ //_rightMiddleMotor.setIdleMode(IdleMode.kBrake);
_rightRearMotor.setIdleMode(IdleMode.kBrake);
_leftFrontMotor.setIdleMode(IdleMode.kBrake);
- _leftMiddleMotor.setIdleMode(IdleMode.kBrake);
+ //_leftMiddleMotor.setIdleMode(IdleMode.kBrake);
_leftRearMotor.setIdleMode(IdleMode.kBrake);
_leftFrontMotor.setVoltage(leftVolts);
@@ -178,13 +196,19 @@ public void tankDriveVolts(double leftVolts, double rightVolts) {
_robotDrive.feed();
}
+ /**
+ * Controls the throttle and turn of the drive directly with voltages. Drives using IdleMode.kCoast
+ *
+ * @param throttle - Robot speed along the X axis. Value between 1 and -1; Forward is positive
+ * @param turn - Robot rotation rate along the Z axis. Value between 1 and -1; Clockwise is positive
+ */
public void drive(double throttle, double turn)
{
_rightFrontMotor.setIdleMode(IdleMode.kCoast);
- _rightMiddleMotor.setIdleMode(IdleMode.kCoast);
+ //_rightMiddleMotor.setIdleMode(IdleMode.kCoast);
_rightRearMotor.setIdleMode(IdleMode.kCoast);
_leftFrontMotor.setIdleMode(IdleMode.kCoast);
- _leftMiddleMotor.setIdleMode(IdleMode.kCoast);
+ //_leftMiddleMotor.setIdleMode(IdleMode.kCoast);
_leftRearMotor.setIdleMode(IdleMode.kCoast);
if (Math.abs(throttle) <= .1) {
@@ -192,19 +216,31 @@ public void drive(double throttle, double turn)
} else {
setRampRate(.25);
}
+
_robotDrive.arcadeDrive(throttle, turn);
}
+ /**
+ * Sets the left and right motor ramp rate
+ *
+ * @param rate - The rate you would like to set the left and right motor
+ */
public void setRampRate(double rate)
{
_leftFrontMotor.setOpenLoopRampRate(rate);
- _leftMiddleMotor.setOpenLoopRampRate(rate);
+ //_leftMiddleMotor.setOpenLoopRampRate(rate);
_leftRearMotor.setOpenLoopRampRate(rate);
_rightFrontMotor.setOpenLoopRampRate(rate);
- _rightMiddleMotor.setOpenLoopRampRate(rate);
+ //_rightMiddleMotor.setOpenLoopRampRate(rate);
_rightRearMotor.setOpenLoopRampRate(rate);
}
+ /**
+ * Controls the left and right sides of the drive directly with voltages. Drives using IdleMode.kBrake
+ *
+ * @param leftVolts the commanded left output
+ * @param rightVolts the commanded right output
+ */
public void driveBrake(double throttle, double turn)
{
_rightFrontMotor.setIdleMode(IdleMode.kBrake);
@@ -220,6 +256,9 @@ public void resetEncoders() {
_rightDriveEncoder.reset();
}
+ /**
+ * Resets the Drivertrain gyro
+ */
public void resetGyro() {
_gyro.reset();
}
@@ -288,6 +327,13 @@ public double getTurnRate() {
return -_gyro.getRate();
}
+ /**
+ * Returns a new trajectory config
+ *
+ * @param isReverse - Whether or not to return the config in reverse
+ *
+ * @return The trajectory config
+ */
public TrajectoryConfig getTrajectoryConfig(boolean isReverse)
{
var autoVoltageConstraint =
@@ -307,58 +353,92 @@ public TrajectoryConfig getTrajectoryConfig(boolean isReverse)
config.setReversed(isReverse);
return config;
}
+
+ /**
+ * Sets path follower current path as Test Path
+ */
public void setTestPath()
{
_pathFollower.setTestPath(getTrajectoryConfig(false));
}
+
+ /**
+ * Sets path follower current path as Drive Forward And Shoot Path
+ */
public void setDriveForwardAndShootPath()
{
_pathFollower.setDriveForwardAndShootPath(getTrajectoryConfig(false));
}
+ /**
+ * Sets path follower current path as Collect wo From Terminal Path
+ */
public void setCollectTwoFromTerminalPath()
{
_pathFollower.setCollectTwoFromTerminalPath(getTrajectoryConfig(false), getPose());
}
+ /**
+ * Sets path follower current path as Drive Back From Terminal Path
+ */
public void setDriveBackFromTerminalPath()
{
_pathFollower.setDriveBackFromTerminalPath(getTrajectoryConfig(true), getPose());
}
- public void setFifthBallPath()
- {
- _pathFollower.setFifthBallPath(getTrajectoryConfig(false), getPose());
- }
-
+ /**
+ * Sets path follower current path as Five Ball Part One Path
+ */
public void setFiveBallPartOnePath()
{
_pathFollower.setFiveBallPartOnePath(getTrajectoryConfig(false));
}
+ /**
+ * Sets path follower current path as Five Ball Part Two To Terminal Path
+ */
public void setFiveBallPartTwoToTerminalPath()
{
_pathFollower.setFiveBallPartTwoToTerminalPath(getTrajectoryConfig(false), getPose().getRotation());
}
+ /**
+ * Sets path follower current path as Five Ball Part Two From Terminal Path
+ */
public void setFiveBallPartTwoFromTerminalPath()
{
_pathFollower.setFiveBallPartTwoFromTerminalPath(getTrajectoryConfig(true), getPose().getRotation());
}
+
+ /**
+ * Sets path follower current path as Four Ball Far Part One Path
+ */
public void setFourBallFarPartOnePath()
{
_pathFollower.setFourBallFarPartOneTrajectory(getTrajectoryConfig(false));
}
+
+ /**
+ * Sets path follower current as Four Ball Far Part Two Out Path
+ */
public void setFourBallFarPartTwoOutPath()
{
_pathFollower.setFourBallFarPartTwoOutPath(getTrajectoryConfig(false), getPose());
}
+ /**
+ * Sets path follower current path as Four Ball Far Part Two Path
+ */
public void setFourBallFarPartTwoBackPath()
{
_pathFollower.setFourBallFarPartTwoBackPath(getTrajectoryConfig(true), getPose());
}
+ /**
+ * Starts the path that has already been set
+ *
+ * METHOD SHOULD BE CALLED ONLY AFTER A PATH HAS BEEN SET
+ */
public void startPath()
{
_leftDriveVelocityPID.reset();
@@ -366,6 +446,10 @@ public void startPath()
resetOdometry(_pathFollower.getStartingPose());
_pathFollower.startPath();
}
+
+ /**
+ * Uses the path follower current path to drive to the target location
+ */
public void followPath()
{
var target = _pathFollower.getPathTarget(getPose());
@@ -383,11 +467,20 @@ public void followPath()
tankDriveVolts(leftOutputTarget + leftFeedForward, rightOutputTarget + rightFeedForward);
}
+
+ /**
+ * Returns whether or not the started path is finished.
+ * @return if path finished
+ */
public boolean isPathFinished()
{
return _pathFollower.isPathFinished();
}
+ /**
+ * If the limilight has a selected target it will return the output needed to get to the target
+ * @return the output needed to get to the limilight target. If no target exist returns 0.
+ */
public double followTarget()
{
if (!_limelight.hasTarget())
@@ -419,6 +512,11 @@ public double followTarget()
return -output;
}
+ /**
+ * Uses the visionTargetState to return the output needed to reach target
+ * @param visionTargetState - The visionTargetState to use in calculations
+ * @return - output needed to reach target
+ */
public double followTarget(VisionTargetState visionTargetState)
{
if (_targetAngle == 7769) {
@@ -439,6 +537,10 @@ public double followTarget(VisionTargetState visionTargetState)
return -output;
}
+ /**
+ * Turns the robot to the desired angle using Drivetrain.driveBrake
+ * @param angle - Angle you wish to turn to.
+ */
public void turnToAngle(double angle)
{
var output = _turnPID.calculate(getHeading(), angle);
@@ -455,20 +557,34 @@ public void turnToAngle(double angle)
driveBrake(0, -output);
}
+ /**
+ * Whether or not if the robot has finished its turn
+ * @return - has robot finished its turn
+ */
public boolean isTurnFinished()
{
return _turnPID.atSetpoint();
}
+ /**
+ * Resets the turn pid of the driveTrain
+ */
public void resetPID()
{
_turnPID.reset();
}
+
+ /**
+ * Sets the targetAngle of driveTrain back to 7769
+ */
public void resetTargetAngle()
{
_targetAngle = 7769;
}
+ /**
+ * Logs driveTrain data to SmartDashboard
+ */
public void LogTelemetry() {
// TODO Auto-generated method stub
SmartDashboard.putNumber("leftDriveDistance", _leftDriveEncoder.getDistance());
@@ -478,6 +594,9 @@ public void LogTelemetry() {
SmartDashboard.putNumber("gyroHeading", getHeading());
}
+ /**
+ * Reads the data from the SmartDashboard... But at the moment this function is empty so it does nothing.
+ */
public void ReadDashboardData() {
// TODO Auto-generated method stub
diff --git a/2022 Season/src/main/java/frc/robot/Subsystems/Shooter.java b/2022 Season/src/main/java/frc/robot/Subsystems/Shooter.java
index 9ce18e4..0355ee1 100644
--- a/2022 Season/src/main/java/frc/robot/Subsystems/Shooter.java
+++ b/2022 Season/src/main/java/frc/robot/Subsystems/Shooter.java
@@ -51,7 +51,8 @@ public class Shooter implements ISubsystem {
{10, 14500, 0.165}, // Tarmac
{11, 15000, 0.165}, // Tarmac
{12, 16250, 0.185}, // Tarmac
- });
+ });
+
/**
* Gets the shooter instance
*/
@@ -103,6 +104,9 @@ public Shooter() {
_limelight = Limelight.getInstance();
}
+ /**
+ * Resets the hood encoder
+ */
public void zeroHood()
{
// if (_limitSwitch.isPressed())
@@ -112,17 +116,27 @@ public void zeroHood()
_hoodEncoder.reset();
}
+ /**
+ * Sets the speed of the hoodMotor
+ * @param speed - The speed you would like to set to
+ */
public void manualHood(double speed)
{
_hoodMotor.set(speed);
}
+ /**
+ * Reset the shot of the hoodTarget and shooterTarget
+ */
public void resetShot()
{
_hoodTarget = 0;
_shooterTarget = 0;
}
+ /**
+ * Sets the hoodPosition and the Speed of the Robot based on the Limilight Distance to Target
+ */
public void readyShot()
{
var distance = _limelight.getDistanceToTarget();
@@ -145,6 +159,10 @@ public void readyShot()
_previousDistance = distance;
}
+ /**
+ * Uses the vision state to set the hood position and shooter speed
+ * @param visionTargetState - The visionTargetState that is being used in the calculations
+ */
public void readyShot(VisionTargetState visionTargetState)
{
var distance = visionTargetState.getDistance();
@@ -163,6 +181,10 @@ public void readyShot(VisionTargetState visionTargetState)
_previousDistance = distance;
}
+ /**
+ * Checks if the Shooter is at the needed speed to shoot && if the hoodPID is at the needed SetPoint
+ * @return - Whether or not the conditions are met
+ */
public boolean goShoot()
{
boolean shooterAtSpeed = false;
@@ -177,6 +199,10 @@ public boolean goShoot()
return shooterAtSpeed && _hoodPID.atSetpoint();
}
+
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the custom shot
+ */
public void setCustomShot()
{
_hoodTarget = _customHoodPositon;
@@ -184,6 +210,9 @@ public void setCustomShot()
_targetName = "Custom";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the puke shot
+ */
public void setPukeShot()
{
_shooterTarget = Constants.kPukeShotSpeed;
@@ -191,6 +220,9 @@ public void setPukeShot()
_targetName = "Puke Shot";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the zone shot
+ */
public void setZoneShot()
{
_shooterTarget = Constants.kZoneShotSpeed;
@@ -198,6 +230,9 @@ public void setZoneShot()
_targetName = "Zone Shot";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the close shot
+ */
public void setCloseShot()
{
_shooterTarget = Constants.kCloseShotSpeed;
@@ -205,6 +240,9 @@ public void setCloseShot()
_targetName = "Close Shot";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the Far shot
+ */
public void setFarShot()
{
_shooterTarget = Constants.kFarShotSpeed;
@@ -212,6 +250,9 @@ public void setFarShot()
_targetName = "Far Shot";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the two ball shot
+ */
public void setTwoBallShot()
{
_shooterTarget = 13850;
@@ -219,22 +260,35 @@ public void setTwoBallShot()
_targetName = "Two Ball Shot";
}
+ /**
+ * Sets the hoodTarget, shooterTarget and targetName to the auto shot
+ */
public void setAutoShot()
{
_targetName = "Auto Shot";
}
+ /**
+ * Sets the speed of the leftMotor
+ */
private void setSpeed(double speed)
{
_leftMotor.set(TalonFXControlMode.Velocity, speed);
}
+ /**
+ * Stops the leftMotor from moving
+ */
public void stop()
{
_leftMotor.set(TalonFXControlMode.PercentOutput, 0);
_hoodMotor.set(0);
}
+ /**
+ * Moves the hood to the needed position
+ * @param position - Position you would like the hood to be at
+ */
private void setHoodPosition(double position)
{
if (position != _hoodPID.getSetpoint())
@@ -258,6 +312,9 @@ private void setHoodPosition(double position)
_hoodMotor.set(-output);
}
+ /**
+ * Updates Shooter information on the SmartDashboard
+ */
public void LogTelemetry() {
// TODO Auto-generated method stub
SmartDashboard.putNumber("hoodPosition", _hoodEncoder.get());
@@ -274,10 +331,12 @@ public void LogTelemetry() {
SmartDashboard.putNumber("closedLoopError", _leftMotor.getClosedLoopError());
}
+ /**
+ * Reads the data from the smart dashboard
+ */
public void ReadDashboardData() {
// TODO Auto-generated method stub
_customHoodPositon = SmartDashboard.getNumber("hoodCustomPosition", 0);
_customShooterSpeed = SmartDashboard.getNumber("shooterCustomSpeed", 0);
-
}
}
diff --git a/2022 Season/src/main/java/frc/robot/Utilities/LEDController.java b/2022 Season/src/main/java/frc/robot/Utilities/LEDController.java
index c194557..458a69d 100644
--- a/2022 Season/src/main/java/frc/robot/Utilities/LEDController.java
+++ b/2022 Season/src/main/java/frc/robot/Utilities/LEDController.java
@@ -25,6 +25,7 @@ public LEDController()
_lowerBlinkin = new Spark(1);
_upperBlinkin = new Spark(0);
}
+
public static LEDController GetInstance()
{
if (_instance == null)
@@ -48,10 +49,12 @@ public void setTrackingTargetState()
{
_upperBlinkin.set(kTrackingTarget);
}
+
public void setOnTargetState()
{
_upperBlinkin.set(kOnTarget);
}
+
public void setTeleopIdle()
{
_upperBlinkin.set(sinelonCustom);
diff --git a/2022 Season/src/main/java/frc/robot/Utilities/Limelight.java b/2022 Season/src/main/java/frc/robot/Utilities/Limelight.java
index 57ecd8d..d6af69b 100644
--- a/2022 Season/src/main/java/frc/robot/Utilities/Limelight.java
+++ b/2022 Season/src/main/java/frc/robot/Utilities/Limelight.java
@@ -16,6 +16,10 @@ public class Limelight {
private double _measurementHeight = _goalHeight - _limelightHeight; // Feet
private double _theta = 35; // Degrees
+ /**
+ * Returns the limelight instance
+ * @return - Instance of limelight
+ */
public static Limelight getInstance()
{
if (_instance == null)
@@ -26,6 +30,9 @@ public static Limelight getInstance()
return _instance;
}
+ /**
+ * Limelight Constructor to create a LimeLight Object
+ */
public Limelight()
{
_validTargets = NetworkTableInstance.getDefault().getTable(Constants.kTableName).getEntry(Constants.kValidTargetKey);
@@ -33,11 +40,19 @@ public Limelight()
_camMode = NetworkTableInstance.getDefault().getTable(Constants.kTableName).getEntry(Constants.kCamModeKey);
}
+ /**
+ * Returns the limelight value from {@link NetworkTableInstance}
+ * @return - Angle value to target
+ */
public double getAngleToTarget()
{
return NetworkTableInstance.getDefault().getTable(Constants.kTableName).getEntry(Constants.kTargetAngleXKey).getDouble(0);
}
+ /**
+ * Gets distance from target, if there is no target provided it will return 0.0
+ * @return - Distance from the target
+ */
public double getDistanceToTarget()
{
if (!hasTarget())
@@ -53,6 +68,10 @@ public double getDistanceToTarget()
return distance;
}
+ /**
+ * Returns the limelight visionTargetState
+ * @return - The current VisionTargetState
+ */
public VisionTargetState getVisionTargetState()
{
if (!hasTarget())
@@ -66,22 +85,36 @@ public VisionTargetState getVisionTargetState()
return new VisionTargetState(offset, distance);
}
+ /**
+ * Checks if it has a target
+ * @return - Whether or not there is a target
+ */
public boolean hasTarget()
{
return _validTargets.getDouble(0) >= 1;
}
+ /**
+ * Checks if the bot is aimed
+ * @return - Whether or not the bot is aimed
+ */
public boolean isAimbot()
{
return _camMode.getDouble(0) == 1;
}
+ /**
+ * Sets the aim of the bot
+ */
public void setAimbot()
{
_ledMode.setDouble(Constants.kLEDOn);
_camMode.setDouble(Constants.kCamModeVisionProcessing);
}
+ /**
+ * Sets the DashCame
+ */
public void setDashcam()
{
_ledMode.setDouble(Constants.kLEDOff);
diff --git a/2022 Season/src/main/java/frc/robot/Utilities/PathFollower.java b/2022 Season/src/main/java/frc/robot/Utilities/PathFollower.java
index 74a4a27..14bbe74 100644
--- a/2022 Season/src/main/java/frc/robot/Utilities/PathFollower.java
+++ b/2022 Season/src/main/java/frc/robot/Utilities/PathFollower.java
@@ -19,20 +19,32 @@ public class PathFollower {
private Trajectory _currentPath;
private Timer _timer;
+ /**
+ This is a PathFollower constructor
+ */
public PathFollower()
{
_controller = new RamseteController();
_timer = new Timer();
}
+
+ /**
+ * Reset old timer and starts a new one
+ */
public void startPath()
{
_timer.reset();
_timer.start();
}
+
+ /**
+ * Checks if the current path has been finished
+ */
public boolean isPathFinished()
{
return _timer.get() > _currentPath.getTotalTimeSeconds();
}
+
// public void setLineToTrenchPath(TrajectoryConfig config)
// {
// _currentPath = getLineToTrenchTrajectory(config);
@@ -42,60 +54,136 @@ public boolean isPathFinished()
// _currentPath = getTrenchToLineTrajectory(config);
// }
+ /**
+ * Sets the current Path of the pathFollower to be the path of TestPath
+ * @param config - The trajectory configuration for the new path
+ */
public void setTestPath(TrajectoryConfig config)
{
_currentPath = generateTestTrajectory(config);
}
+ /**
+ * Sets current Path as drive Forward and shoot
+ * @param config - The trajectory configuration for the new path
+ */
public void setDriveForwardAndShootPath(TrajectoryConfig config)
{
_currentPath = getDriveForwardAndShootTrajectory(config);
}
+
+ /**
+ * Sets current Path as Collect Two From Terminal
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setCollectTwoFromTerminalPath(TrajectoryConfig config, Pose2d startingPose)
{
_currentPath = getCollectTwoFromTerminalTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as Drive Back From Terminal
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setDriveBackFromTerminalPath(TrajectoryConfig config, Pose2d startingPose)
{
_currentPath = getDriveBackFromTerminalTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as Fifth Ball
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFifthBallPath(TrajectoryConfig config, Pose2d startingPose)
{
_currentPath = getFifthBallTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as The first Part for Five Ball
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFiveBallPartOnePath(TrajectoryConfig config)
{
_currentPath = getFiveBallPartOneTrajectory(config);
}
+
+ /**
+ * Sets current Path as Part Two TO Terminal
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFiveBallPartTwoToTerminalPath(TrajectoryConfig config, Rotation2d startingPose)
{
_currentPath = getFiveBallPartTwoToTerminalTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as Part Two FROM Terminal
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFiveBallPartTwoFromTerminalPath(TrajectoryConfig config, Rotation2d startingPose)
{
_currentPath = getFiveBallPartTwoFromTerminalTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as Four Ball Path One
+ * @param config - The trajectory configuration for the new path
+ */
public void setFourBallFarPartOneTrajectory(TrajectoryConfig config)
{
_currentPath = getFourBallFarPartOneTarejctory(config);
}
+
+ /**
+ * Sets current Path as Four Ball Part Two Out
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFourBallFarPartTwoOutPath(TrajectoryConfig config, Pose2d startingPose)
{
_currentPath = getFourBallFarPartTwoOutTrajectory(config, startingPose);
}
+
+ /**
+ * Sets current Path as Four Ball Far Part Two Back Path
+ * @param config - The trajectory configuration for the new path
+ * @param startingPose - The Starting position of the robot when the path started
+ */
public void setFourBallFarPartTwoBackPath(TrajectoryConfig config, Pose2d startingPose)
{
_currentPath = getFourBallFarPartTwoBackTrajectory(config, startingPose);
}
+ /**
+ * Gives the current Path of the Path Follower
+ * @return - The Current Path
+ */
public Trajectory getCurrentTrajectory()
{
return _currentPath;
}
+
+ /**
+ * Gives the inital position of the current Path
+ * @return - Current Path Initial Position
+ */
public Pose2d getStartingPose()
{
return _currentPath.getInitialPose();
}
+
+ /**
+ * Gets the path target speed
+ * @param currentPose - Current X, Y Cordinates of the Robot
+ * @return - The differential Drive Wheel Speeds
+ */
public DifferentialDriveWheelSpeeds getPathTarget(Pose2d currentPose)
{
var time = _timer.get();
@@ -111,18 +199,15 @@ public DifferentialDriveWheelSpeeds getPathTarget(Pose2d currentPose)
return Constants.kDriveKinematics.toWheelSpeeds(targetSpeeds);
}
- /*private Trajectory generateTrajectory(TrajectoryConfig config) {
- var start = new Pose2d(3.129, 2.402, Rotation2d.fromDegrees(0));
- var end = new Pose2d(8.106, .75, Rotation2d.fromDegrees(0));
-
- var interiorWaypoints = new ArrayList();
- interiorWaypoints.add(new Translation2d(5.708, .75));
-
- var trajectory = TrajectoryGenerator.generateTrajectory(start, interiorWaypoints, end, config);
- return trajectory;
- }*/
- private Trajectory generateTestTrajectory(TrajectoryConfig config) {
+ // Private Functions --------------------------------
+
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Test Path Cordinates
+ */
+ private Trajectory generateTestTrajectory(TrajectoryConfig config) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
@@ -134,9 +219,14 @@ private Trajectory generateTestTrajectory(TrajectoryConfig config) {
new Pose2d(3, 0, new Rotation2d(0)),
// Pass config
config);
- }
+ }
- private Trajectory getDriveForwardAndShootTrajectory(TrajectoryConfig config) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Drive Forward And Shoot Path Cordinates
+ */
+ private Trajectory getDriveForwardAndShootTrajectory(TrajectoryConfig config) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kDriveForwardStartX, Constants.kDriveForwardStartY, new Rotation2d(Constants.kDriveForwardStartRotation)),
// Pass through these two interior waypoints, making an 's' curve path
@@ -147,9 +237,14 @@ private Trajectory getDriveForwardAndShootTrajectory(TrajectoryConfig config) {
new Pose2d(Constants.kDriveForwardEndX, Constants.kDriveForwardEndY, new Rotation2d(-2.7716865861332294)),
// Pass confighh
config);
- }
+ }
- private Trajectory getCollectTwoFromTerminalTrajectory(TrajectoryConfig config, Pose2d currentPose) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Collect Two From Terminal Path Cordinates
+ */
+ private Trajectory getCollectTwoFromTerminalTrajectory(TrajectoryConfig config, Pose2d currentPose) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.getRotation()),
// Pass through these two interior waypoints, making an 's' curve path
@@ -160,9 +255,14 @@ private Trajectory getCollectTwoFromTerminalTrajectory(TrajectoryConfig config,
new Pose2d(Constants.kCollectTwoEndPointX, Constants.kCollectTwoEndPointY, new Rotation2d(Constants.kCollectTwoEndRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getDriveBackFromTerminalTrajectory(TrajectoryConfig config, Pose2d currentPose) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Drive Back From Terminal Path Cordinates
+ */
+ private Trajectory getDriveBackFromTerminalTrajectory(TrajectoryConfig config, Pose2d currentPose) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.getRotation()),
// Pass through these two interior waypoints, making an 's' curve path
@@ -173,9 +273,14 @@ private Trajectory getDriveBackFromTerminalTrajectory(TrajectoryConfig config, P
new Pose2d(Constants.kDriveBackEndPointX, Constants.kDriveBackEndPointY, new Rotation2d(Constants.kDriveBackEndPointRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFifthBallTrajectory(TrajectoryConfig config, Pose2d currentPose) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Fifth Ball Path Cordinates
+ */
+ private Trajectory getFifthBallTrajectory(TrajectoryConfig config, Pose2d currentPose) {
return TrajectoryGenerator.generateTrajectory(currentPose,
// Pass through these two interior waypoints, making an 's' curve path
@@ -186,9 +291,14 @@ private Trajectory getFifthBallTrajectory(TrajectoryConfig config, Pose2d curren
new Pose2d(Constants.kFifthBallEndPointX, Constants.kFifthBallEndPointY, new Rotation2d(Constants.kFifthBallEndPointRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFiveBallPartOneTrajectory(TrajectoryConfig config) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Five Ball Part One Path Cordinates
+ */
+ private Trajectory getFiveBallPartOneTrajectory(TrajectoryConfig config) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kFiveBallPartOneStartX, Constants.kFiveBallPartOneStartY, new Rotation2d(Constants.kFiveBallPartOneStartRotation)),
// Pass through these two interior waypoints, making an 's' curve path
@@ -200,9 +310,14 @@ private Trajectory getFiveBallPartOneTrajectory(TrajectoryConfig config) {
new Pose2d(Constants.kFiveBallPartOneEndX, Constants.kFiveBallPartOneEndY, new Rotation2d(Constants.kFiveBallPartOneEndRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFiveBallPartTwoToTerminalTrajectory(TrajectoryConfig config, Rotation2d startingPose) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Five Ball Part Two To Terminal Path Cordinates
+ */
+ private Trajectory getFiveBallPartTwoToTerminalTrajectory(TrajectoryConfig config, Rotation2d startingPose) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kFiveBallPartTwoToTerminalStartX, Constants.kFiveBallPartTwoToTerminalStartY, startingPose),
// Pass through these two interior waypoints, making an 's' curve path
@@ -212,9 +327,14 @@ private Trajectory getFiveBallPartTwoToTerminalTrajectory(TrajectoryConfig confi
new Pose2d(Constants.kFiveBallPartTwoToTerminalEndX, Constants.kFiveBallPartTwoToTerminalEndY, new Rotation2d(Constants.kFiveBallPartTwoToTerminalEndRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFiveBallPartTwoFromTerminalTrajectory(TrajectoryConfig config, Rotation2d startingPose) {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Five Ball Part Two From Terminal Path Cordinates
+ */
+ private Trajectory getFiveBallPartTwoFromTerminalTrajectory(TrajectoryConfig config, Rotation2d startingPose) {
return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kFiveBallPartTwoFromTerminalStartX, Constants.kFiveBallPartTwoFromTerminalStartY, startingPose),
// Pass through these two interior waypoints, making an 's' curve path
@@ -224,9 +344,15 @@ private Trajectory getFiveBallPartTwoFromTerminalTrajectory(TrajectoryConfig con
new Pose2d(Constants.kFiveBallPartTwoFromTerminalEndX, Constants.kFiveBallPartTwoFromTerminalEndY, new Rotation2d(Constants.kFiveBallPartTwoFromTerminalEndRotation)),
// Pass config
config);
- }
- private Trajectory getFourBallFarPartOneTarejctory(TrajectoryConfig config)
- {
+ }
+
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Four Ball Far Part One Path Cordinates
+ */
+ private Trajectory getFourBallFarPartOneTarejctory(TrajectoryConfig config)
+ {
return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kFourBallFarPartOneStartX, Constants.kFourBallFarPartOneStartX, new Rotation2d(Constants.kFourBallFarPartOneStartRotation)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(),
@@ -234,10 +360,15 @@ private Trajectory getFourBallFarPartOneTarejctory(TrajectoryConfig config)
new Pose2d(Constants.kFourBallFarPartOneEndX, Constants.kFourBallFarPartOneEndY, new Rotation2d(Constants.kFourBallFarPartOneEndRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFourBallFarPartTwoOutTrajectory(TrajectoryConfig config, Pose2d currentPose)
- {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Four Ball Far Part Two Out Path Cordinates
+ */
+ private Trajectory getFourBallFarPartTwoOutTrajectory(TrajectoryConfig config, Pose2d currentPose)
+ {
return TrajectoryGenerator.generateTrajectory(new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.getRotation()),
// Pass through these two interior waypoints, making an 's' curve path
List.of(
@@ -247,10 +378,15 @@ private Trajectory getFourBallFarPartTwoOutTrajectory(TrajectoryConfig config, P
new Pose2d(Constants.kFourBallFarPartTwoEndPointX, Constants.kFourBallFarPartTwoEndPointY, new Rotation2d(Constants.kFourBallFarPartTwoEndRotation)),
// Pass config
config);
- }
+ }
- private Trajectory getFourBallFarPartTwoBackTrajectory(TrajectoryConfig config, Pose2d currentPose)
- {
+ /**
+ * Returns new Trajectory class
+ * @param config - Configuration for the trajectory
+ * @return - new Trajectory class with the Four Ball Far Part Two Back Path Cordinates
+ */
+ private Trajectory getFourBallFarPartTwoBackTrajectory(TrajectoryConfig config, Pose2d currentPose)
+ {
return TrajectoryGenerator.generateTrajectory(new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.getRotation()),
// Pass through these two interior waypoints, making an 's' curve path
List.of(
@@ -260,7 +396,21 @@ private Trajectory getFourBallFarPartTwoBackTrajectory(TrajectoryConfig config,
new Pose2d(Constants.kFourBallFarPartTwoBackEndPointX, Constants.kFourBallFarPartTwoBackEndPointY, new Rotation2d(Constants.kFourBallFarPartTwoBackEndRotation)),
// Pass config
config);
- }
+ }
+
+ /*private Trajectory generateTrajectory(TrajectoryConfig config) {
+
+ var start = new Pose2d(3.129, 2.402, Rotation2d.fromDegrees(0));
+ var end = new Pose2d(8.106, .75, Rotation2d.fromDegrees(0));
+
+ var interiorWaypoints = new ArrayList();
+ interiorWaypoints.add(new Translation2d(5.708, .75));
+
+ var trajectory = TrajectoryGenerator.generateTrajectory(start, interiorWaypoints, end, config);
+ return trajectory;
+ }*/
+
+
// private Trajectory getTrenchToLineTrajectory(TrajectoryConfig config)
// {
// return TrajectoryGenerator.generateTrajectory(new Pose2d(Constants.kTrenchPathEndX,Constants.kTrenchPathEndY, new Rotation2d(Constants.kTrenchPathEndAngle)),
diff --git a/2022 Season/src/main/java/frc/robot/Utilities/VisionTargetState.java b/2022 Season/src/main/java/frc/robot/Utilities/VisionTargetState.java
index a4aedaa..588ca53 100644
--- a/2022 Season/src/main/java/frc/robot/Utilities/VisionTargetState.java
+++ b/2022 Season/src/main/java/frc/robot/Utilities/VisionTargetState.java
@@ -10,11 +10,19 @@ public VisionTargetState(double offset, double distance)
_distance = distance;
}
+ /**
+ * The offset of the VisionTargetState
+ * @return offset
+ */
public double getOffset()
{
return _offset;
}
+ /**
+ * The distance of the VisionTargetState
+ * @return distance to target
+ */
public double getDistance()
{
return _distance;