Skip to content

Commit

Permalink
documentation updates
Browse files Browse the repository at this point in the history
  • Loading branch information
VincentShao32 committed Jan 30, 2024
1 parent 78ccdd1 commit edcab41
Show file tree
Hide file tree
Showing 4 changed files with 25 additions and 6 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/SetPivot.java
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public void execute() {
s_Pivot.setVoltage(voltage);
}

@Override
@Override
public boolean isFinished() {
return Math.abs(s_Pivot.getSetPoint() - s_Pivot.getCANcoderPosition()) < 45;
}
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/commands/SmartIntake.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,10 @@
public class SmartIntake extends Command{

private final IntakeShooter s_IntakeShooter;
private boolean finished;

public SmartIntake(){
s_IntakeShooter = IntakeShooter.getInstance();
addRequirements(s_IntakeShooter);
finished = false;
}

@Override
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/IntakeShooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ public void configFollowerMotor(){
// mLeaderShooter.getPIDController().setI(Constants.SwerveConstants.driveKI);
// mLeaderShooter.getPIDController().setD(Constants.SwerveConstants.driveKD);
}

public boolean hasNote(){
return m_intakeSensor.getProximity() >= Constants.noteIntakeDistance;
}
Expand Down
25 changes: 23 additions & 2 deletions src/main/java/frc/robot/subsystems/Pivot.java
Original file line number Diff line number Diff line change
Expand Up @@ -43,22 +43,40 @@ public Pivot() {
configCANcoder();
}

/**
* Sets the desired state for the pivot
* @param state Desired state
*/
public void setState(PivotState state) {
curState = state;
}

/**
* Gets the current position measured by the CANcoder
* @return Current position measured by CANcoder
*/
public double getCANcoderPosition() {
return pivotCANcoder.getPosition().getValueAsDouble();
}

/**
* Gets the current set point of the pivot.
* @return Current set point in CANcoder values.
*/
public double getSetPoint() {
return curState.pos;
}

/**
* Sets the voltage of the pivot motor
* @param Desired voltage.
*/
public void setVoltage(double voltage) {
mPivotMotor.setVoltage(voltage);
}

/**
* Configures the pivot motor with current limit, idle mode, and PID values
*/
private void configMotor() {
mPivotMotor.setSmartCurrentLimit(Constants.pivotPeakCurrentLimit);
mPivotMotor.setIdleMode(IdleMode.kBrake);
Expand All @@ -67,7 +85,10 @@ private void configMotor() {
mPivotMotor.getPIDController().setI(Constants.hardwarePIDs.pivotkI);
mPivotMotor.getPIDController().setD(Constants.hardwarePIDs.pivotkD);
}


/**
* Configures the pivot CANcoder with sensor direction and absolute range.
*/
private void configCANcoder(){
/* Swerve CANCoder Configuration */
// CANcoder is always initialized to absolute position on boot in Phoenix 6 - https://www.chiefdelphi.com/t/what-kind-of-encoders-are-built-into-the-kraken-motors/447253/7
Expand Down

0 comments on commit edcab41

Please sign in to comment.