Skip to content

Commit c310cfb

Browse files
committed
Added Function Docs
1 parent d62bae2 commit c310cfb

File tree

7 files changed

+118
-12
lines changed

7 files changed

+118
-12
lines changed

src/main/java/frc/robot/Modules/AimFire.java

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,9 @@
66
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
77

88
public class AimFire {
9-
//Shooter, will probably be replaced with autonomous shooter stuff, need encoder
9+
/**
10+
* If the right bumper is pressed, run the shooter flywheel at full speed. Otherwise, stop it
11+
*/
1012
public static void shooter() {
1113
if(Robot.controller.getRightBumper()) {
1214
Robot.shooterFly.set(ControlMode.PercentOutput, 1);
@@ -15,9 +17,14 @@ public static void shooter() {
1517
}
1618
}
1719

20+
/**
21+
* The function that will center the robot's aim on the target.
22+
*
23+
* @param target The name of the target to aim at. ("top_hub","ball")
24+
*/
1825
public static void centerAim(String target) {
1926
//https://i.kym-cdn.com/entries/icons/original/000/039/393/cover2.jpg
20-
switch(target) {
27+
switch(target.toLowerCase()) {
2128
case "top_hub": // TODO: check PID on Triclops
2229
Limelight.setPipeline(0);
2330
Limelight.getLimelightData();
@@ -46,6 +53,10 @@ public static void centerAim(String target) {
4653
}
4754
}
4855

56+
57+
/**
58+
* One click fire function
59+
*/
4960
public static void fire() { //TODO: One click fire
5061

5162
}

src/main/java/frc/robot/Modules/BallTracking.java

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,18 @@
77

88

99
public class BallTracking {
10+
/**
11+
* This function is called to initalise Alliance color and ball tracking
12+
*/
1013
public static void maininit() {
1114
SmartDashboard.putString("Alliance Color", "Waiting for Alliance Color");
1215
}
1316

17+
/**
18+
* This function is called when auto first started.
19+
*
20+
* It sets the alliance color to the value of the alliance color selected in the SmartDashboard
21+
*/
1422
public static void autoinit() {
1523
NetworkTableInstance.getDefault().getTable("TalonPi").getEntry("Alliance Color").setString(SmartDashboard.getString("Alliance Color", "RED"));
1624
}

src/main/java/frc/robot/Modules/Climbers.java

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,10 @@
66
public class Climbers {
77

88
//Vertical Climb, no encoder
9+
/**
10+
* If the D pad is up, then the climb extension motor will be set to 1. If the D pad is down, then the
11+
* climb extension motor will be set to -1. Otherwise, the climb extension motor will be set to 0
12+
*/
913
public static void climb() {
1014
if (Robot.controller.getPOV() == 0) { //Up on D pad
1115
Robot.climbExtension.set(ControlMode.PercentOutput, 1);
@@ -17,6 +21,10 @@ public static void climb() {
1721
}
1822

1923
//Climb Rotation, no encoder
24+
/**
25+
* If the D pad is pointed right, rotate the climber clockwise. If the D pad is pointed left, rotate
26+
* the climber counterclockwise. Otherwise, stop the climber.
27+
*/
2028
public static void climbrotation() {
2129
if (Robot.controller.getPOV() == 90) { // Right on the D pad
2230
Robot.climbRotation.set(ControlMode.PercentOutput, 1);

src/main/java/frc/robot/Modules/DriveCode.java

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,12 @@ public static void moveBack() {
1515
Robot.drive.tankDrive(-0.1, -0.1);
1616
}
1717

18+
/**
19+
* Drive the robot using the left and right master motors at the given motor speeds.*
20+
*
21+
* @param motorSpeedLeft The speed that you want the left motor to go.
22+
* @param motorSpeedRight The speed that you want the right side to go.
23+
*/
1824
public static void oldDriveTrain(double motorSpeedLeft, double motorSpeedRight) {
1925
Robot.leftMaster.set(ControlMode.PercentOutput, motorSpeedLeft);
2026
Robot.rightMaster.set(ControlMode.PercentOutput, motorSpeedRight);

src/main/java/frc/robot/Modules/Intake.java

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,11 @@
55
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
66

77
public class Intake {
8-
/** The spinny part that is duct taped together by "The Contractor" */
8+
/** The spinny part that is duct taped together by "The Contractor"
9+
* If the right bumper is pressed, the intake spins from the field to the robot. If the left bumper is
10+
* pressed, the intake spins from the entrance of the robot to the flywheel. If neither bumper is
11+
* pressed, the intake is stopped
12+
*/
913
public static void rollers() { // Had absolute value of getRightY, would never be below zero!
1014
if (Robot.controller.getLeftBumper()) { //Right trigger spins intake from field to robot
1115
Robot.rollers.set(ControlMode.PercentOutput, -RobotInformation.DriveTeamInfo.rollerLBTransferPercentage);
@@ -17,20 +21,19 @@ public static void rollers() { // Had absolute value of getRightY, would never b
1721
}
1822

1923
/** The stupid thing that looks cool but doesnt really work // edit: it works apparently */
24+
25+
/**
26+
* This function is called when the left joystick is moved in the Y direction.
27+
* If the joystick is moved up, the wrist is set to the output of the joystick multiplied by the
28+
* transfer percentage.
29+
* If the joystick is moved down, the wrist is set to 0
30+
*/
2031
public static void wrist() {
2132
if(Math.abs(Robot.controller.getLeftY()) > 0.2) {
2233
Robot.wrist.set(ControlMode.PercentOutput, (Robot.controller.getLeftY() * -RobotInformation.DriveTeamInfo.wristTransferPercentage));
2334
} else {
2435
Robot.wrist.set(ControlMode.PercentOutput, 0);
2536
}
26-
27-
// if (Robot.controller.getBButton()) { //sets intake in position to feed ball into flywheel
28-
// Robot.wrist.set(ControlMode.PercentOutput, 0.2);
29-
// } else if (Robot.controller.getXButton()) { //sets intake in position to pick balls off field
30-
// Robot.wrist.set(ControlMode.PercentOutput, -0.2);
31-
// } else {
32-
// Robot.wrist.set(ControlMode.PercentOutput, 0);
33-
// }
3437
}
3538

3639
}

src/main/java/frc/robot/Modules/Limelight.java

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,12 @@
55
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
66

77
public class Limelight {
8+
/** The last angle seen by the limelight that wasn't zero */
89
public static double nonZeroLimelightHorAng; // Used to orient bot
910

11+
/**
12+
* This function is used to update the SmartDashboard with the current values of the Limelight
13+
*/
1014
public static void updateSmartDashboard() {
1115
double[][] shooterCalculations = Limelight.getLimelightData();
1216
SmartDashboard.putNumber("Distance: ",shooterCalculations[0][0]);
@@ -18,6 +22,20 @@ public static void updateSmartDashboard() {
1822
SmartDashboard.putNumber("Non Zero Angle", nonZeroLimelightHorAng);
1923
}
2024

25+
/**
26+
* This function returns the distance between the limelight and the retroreflector, the optimal angle
27+
* to shoot at, and the optimal ball velocity
28+
*
29+
* @return The horizontal angle, vertical angle, and latency of the limelight.
30+
* <ul>
31+
<li>LimelightInfo[0][0] = distance; //Horizontal distance between the hubs and the limelight
32+
<li>LimelightInfo[0][1] = angle; //Optimal Shooter Angle
33+
<li>LimelightInfo[0][2] = velocity; //Optimal Ball velocity
34+
<li>LimelightInfo[1][0] = horizontalAngle; //Horizontal angle between the limelight and the retroreflector
35+
<li>LimelightInfo[1][1] = verticalAngle; //Vertical angle between limelight and retroreflector
36+
<li>LimelightInfo[1][2] = limelightLatency; // Latency for limelight calculations
37+
* </ul>
38+
*/
2139
public static double[][] getLimelightData() {
2240
double[][] LimelightInfo = {{0,0,0},{0,0,0}};
2341
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
@@ -44,6 +62,11 @@ public static double[][] getLimelightData() {
4462
return LimelightInfo;
4563
}
4664

65+
/**
66+
* Is the top hub present in the FOV?
67+
*
68+
* @return A boolean value.
69+
*/
4770
public static boolean hubPresent() {
4871
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
4972
double targetPresence = table.getEntry("tv").getDouble(0);
@@ -54,20 +77,36 @@ public static boolean hubPresent() {
5477
}
5578
}
5679

80+
/**
81+
* Initialises the Limelight for Top Hub tracking: Sets the LEDS to on, sets the pipeline to 0, and sets the Limelight as a Vision Processor
82+
*/
5783
public static void init() {
5884
setLEDS("on");
5985
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); //Sets the pipeline to 0
6086
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); //Sets the Limelight as a Vision Proccesor
6187
}
6288

89+
/**
90+
* Sets the pipeline to the value of pipeline
91+
*
92+
* @param pipeline 0-10
93+
*/
6394
public static void setPipeline(double pipeline) {
6495
NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipeline); //Sets the pipeline to pipeline
6596
}
6697

98+
/**
99+
* Code run for limelight when robot is in a disabled state
100+
*/
67101
public static void disabled() {
68102
setLEDS("off"); // Turns off the god damm limelight cause im going to go blind and gouge my eyes out because wtf does it need to be so bright like holy hell
69103
}
70104

105+
/**
106+
* This function sets the LED mode of the limelight
107+
*
108+
* @param mode The LED mode. ("off", "blink", "on")
109+
*/
71110
public static void setLEDS(String mode) {
72111
switch (mode) {
73112
case "off":

src/main/java/frc/robot/Modules/MotorControl.java

Lines changed: 32 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,18 +16,40 @@
1616

1717

1818
public class MotorControl {
19+
/**
20+
* Given the ideal velocity and the transfer percentage, return the RPM
21+
*
22+
* @param idealVelocity the velocity you want to achieve in meters per second
23+
* @param transferPercent the percentage of the ideal velocity that is transferred to the wheel.
24+
* @return The RPM of the motor.
25+
*/
1926
public static double getRPM(double idealVelocity, double transferPercent) {
2027
return (((idealVelocity*(1/transferPercent))/(Math.PI*0.1016))*60); // rudimentary calculation that's 90% wrong
2128
}
2229

30+
/**
31+
* This function returns the current velocity of the motor. The motor must be a Falcon500
32+
*
33+
* @param motor The motor you want to get the velocity of.
34+
* @return The integrated sensor velocity of the motor.
35+
*/
2336
public static double getCurrentVelocity(WPI_TalonFX motor) { // If using a TalonFX motor only
2437
return (motor.getSensorCollection().getIntegratedSensorVelocity());
2538
}
2639

40+
/**
41+
* This function returns the current position of the motor. The motor must be a Falcon500
42+
*
43+
* @param motor The motor you want to get the current position of.
44+
* @return The current position of the motor.
45+
*/
2746
public static double getCurrentPosition(WPI_TalonFX motor) { // If using a TalonFX motor only
2847
return (motor.getSensorCollection().getIntegratedSensorPosition());
2948
}
3049

50+
/**
51+
* If the right bumper is pressed, run the flywheel at full power.
52+
*/
3153
public static void flywheel() {
3254
if(Robot.controller.getRightBumper()) {
3355
Robot.shooterFly.set(ControlMode.PercentOutput, 1);
@@ -36,6 +58,9 @@ public static void flywheel() {
3658
}
3759
}
3860

61+
/**
62+
* Initializes the motors for the robot
63+
*/
3964
public static void motor_init() {
4065
Robot.rightMaster.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 40, 40, .5));
4166
Robot.rightMaster.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 40, 40, .5));
@@ -69,7 +94,13 @@ public static void motor_init() {
6994
// Robot.shooterFly.config_kD(0, RobotInformation.PID_Values.flywheel.kD, 1); // (D) Differentiable Term
7095

7196
}
72-
/** Two parameters: motor, mode (RPM or Velocity) */
97+
98+
/**
99+
* This function sets the RPM of a motor
100+
*
101+
* @param position The motor position to set the RPM for.
102+
* @param RPM The RPM of the motor.
103+
*/
73104
public static void setRPM(MotorPositions position, int RPM) {
74105
switch(position) {
75106
case Shooter:

0 commit comments

Comments
 (0)