Skip to content

Added java docs for the second half of SuperSystem #66

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

Open
aadijainn wants to merge 1 commit into
base: V1
Choose a base branch
from
Open

Added java docs for the second half of SuperSystem #66

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
92 changes: 77 additions & 15 deletions src/main/java/frc/robot/subsystems/SuperSystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,9 @@ public class SuperSystem {
boolean elevatorWithinRange, pivotWithinRange;

private BooleanSupplier pivotAtPosition, elevatorAtPosition, wristAtPosition, pivotAtPositionWide, elevatorAtPositionWide, wristAtPositionWide, intakeDetected;

/**
*
*/
public enum ExecutionOrder {
ALL_TOGETHER,
ELV_PVT_WRT,
Expand Down Expand Up @@ -96,13 +98,14 @@ public SuperSystem(SwerveDrivetrain swerveDrivetrain, Elevator elevator, Pivot p
}

// subsystems

public void reConfigureMotors() {
pivot.configureMotorV1();
elevator.setMotorConfigs();
wrist.configurePID(wrist.motorConfigs);
intakeRoller.configureMotor(intakeRoller.motorConfigs);
}

public void setNeutralMode(NeutralModeValue neutralMode) {
elevator.setNeutralMode(neutralMode);
pivot.setNeutralMode(neutralMode);
Expand Down Expand Up @@ -294,6 +297,12 @@ public Command moveTo(PositionEquivalents position) {
}

// movement
/**
*
* @param position
* @param previousPosition
* @return
*/
private Command goTo(Position position, Position previousPosition) {
Command gotoCommand;
if (position.intermediateWristPosition == position.finalWristPosition)
Expand Down Expand Up @@ -327,14 +336,25 @@ private Command goTo(Position position, Position previousPosition) {
// () -> (position == PositionEquivalents.GroundIntake.coralPos ||
// previousPosition == PositionEquivalents.GroundIntake.coralPos));
}

/**
* Changes lastPosition to currentPosition
* Changes all subsystem positions to the correct position to score
* @param position Get position to tell where which types of positions subsystems should be
* @return
*/
public Command moveToAuto(PositionEquivalents position) {
return Commands.sequence(
updatePositions(position),
Commands.either(goToAuto(position.coralPos), goToAuto(position.algaePos), () -> (positionMode == PositionMode.Coral))
);
}

/**
* Changes are made to for subsystems to go to scoring positions
* If current wrist is equal to final then change everything besides that
* If not change everything
* @param position
* @return
*/
public Command goToAuto(Position position) {
if (position.intermediateWristPosition == position.finalWristPosition)
return Commands.sequence(
Expand All @@ -355,28 +375,48 @@ public Command goToAuto(Position position) {
// public Command moveToCage() { //TODO
// return moveTo(NamedPositions.Cage);
// }

/**
* Supposed to be for algae
* Changes all subsystems position to score on L4
* Not implemented
* @return
*/
public Command moveToNet() { //TODO
return moveTo(PositionEquivalents.L4); //Uses net position from position equivalents
}

/**
* Changes to opposite position mode
* If Coral -> Algae, If Algae -> Coral
*/
public void togglePositionMode() {
if (positionMode == PositionMode.Coral) positionMode = PositionMode.Algae;
else positionMode = PositionMode.Coral;
}

/**
* Set the Position Mode
* @param mode Changes the different positions that the subsystems can get set to
*/
public void setPositionMode(PositionMode mode) {
positionMode = mode;
}

/**
* Actually runs the togglePosition command
* @return The command
*/
public Command togglePositionModeCommand() {
return Commands.runOnce(() -> togglePositionMode());
}

/**
* Actually runs the setPosition command
* @return The command for coral
*/
public Command setPositionModeCoral() {
return Commands.runOnce(() -> setPositionMode(PositionMode.Coral));
}

/**
* Actually runs the setPosition command
* @return The command for algae
*/
public Command setPositionModeAlgae() {
return Commands.runOnce(() -> setPositionMode(PositionMode.Algae));
}
Expand All @@ -385,7 +425,10 @@ public Command setPositionModeAlgae() {
// // note: we may not need this one, because the intake action could cover it.
// return moveTo(PositionEquivalents.Processor);
// }

/**
* Enables all subsystems
* Sets all positions to starting positions
*/
public void initialize() {
pivot.setEnabled(true);
wrist.setEnabled(true);
Expand All @@ -400,14 +443,21 @@ public void initialize() {
// climbMotor.setVoltageCommand(0.0);
isStarted = false;
}

/**
* Sets current subsystem pivot angle equal to the the current angle of the pivot
* This is used to for the gravity equation for each subsystem
*
*/
public void updateDependencies() {
double curPivotAngle = pivot.getPosition();
// pivot.setElevatorLength(elevator.getPosition()); // TODO fake
elevator.setPivotAngle(curPivotAngle);
wrist.setPivotAngle(curPivotAngle);
}

/**
* Sets target position to current position
* @return Runs the command
*/
public Command preExecute()
{
return Commands.runOnce(()-> {
Expand All @@ -416,7 +466,15 @@ public Command preExecute()
wrist.setTargetPosition(wrist.getPosition());
}, pivot, elevator, wrist);
}

/**
* Looks at the enums and tells when to run each command to prperly get to desired positions
* @param exeOrder Order of running subsystems
* @param timeout Stops if stuck in position for to long
* @param pivotAngle Angle the pivot is supposed to be at
* @param elevatorPosition Angle the elevator is supposed to be at
* @param wristAngle Angle the wrist is supposed to be at
* @return Commands to run
*/
public Command execute(ExecutionOrder exeOrder, double timeout,
double pivotAngle, double elevatorPosition, double wristAngle)
{
Expand Down Expand Up @@ -578,7 +636,11 @@ public Command execute(ExecutionOrder exeOrder, double timeout,
)
);
}

/**
* Used for logging
* Prints out Supersystem last position, Supersystem current position, Supersystem mode, and Intake Detected
* @param priority
*/
public void initShuffleboard(LOG_LEVEL priority){
if (priority == LOG_LEVEL.OFF) {
return;
Expand Down