Skip to content

Commit

Permalink
Cleaned up Arm Subsystem Interface
Browse files Browse the repository at this point in the history
  • Loading branch information
BrownGenius committed Mar 25, 2024
1 parent ca9a1d7 commit 82b4a88
Show file tree
Hide file tree
Showing 3 changed files with 17 additions and 30 deletions.
29 changes: 5 additions & 24 deletions src/main/java/frc/robot/subsystems/arm/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,40 +3,21 @@
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.config.RobotConfig.ArmConstants;

public interface Arm extends Subsystem {
// gets the angle of the arm
public double getAngle();

public double getTargetAngle();

// sets of the angle of the arm
public void setAngle(double degrees);

public default double getVelocity() {
return 0;
}

public default double getRelativeAngle() {
return getAngle();
}
public boolean isAtMaxLimit();

public default boolean isAbsoluteEncoderConnected() {
return true;
}
public boolean isAtMinLimit();

public default boolean isAtMaxLimit() {
return false;
}
public double getVelocity();

public default boolean isAtMinLimit() {
return false;
}

public default void stow() {
setAngle(ArmConstants.stowIntakeAngleInDegrees);
}
// sets of the angle of the arm
public void setAngle(double degrees);

public Command getStowCommand();

Expand Down
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/arm/ArmIOStub.java
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,10 @@ public void updateInputs(ArmIOInputs inputs) {
inputs.currentAmps = arm.getCurrentDrawAmps();
inputs.positionDegrees = Units.radiansToDegrees(arm.getAngleRads());
inputs.velocityDegrees = Units.radiansToDegrees(arm.getVelocityRadPerSec());
inputs.relativePositionDegrees =
inputs
.positionDegrees; // In sim, the relative/absolute encoder are identical, so just it to
// the same value;

if (softwarePidEnabled) {
currentVoltage =
Expand Down
14 changes: 8 additions & 6 deletions src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,10 @@ public void useState(TrapezoidProfile.State setpoint) {
// System.out.println("vel: " + setpoint.velocity);
}

public double getRelativeAngle() {
return inputs.relativePositionDegrees;
}

@Override
public TrapezoidProfile.State getMeasurement() {
return new TrapezoidProfile.State(getRelativeAngle(), getVelocity());
Expand All @@ -125,11 +129,6 @@ public double getVelocity() {
return inputs.velocityDegrees;
}

@Override
public double getRelativeAngle() {
return inputs.relativePositionDegrees;
}

@Override
public double getTargetAngle() {
return targetDegrees;
Expand Down Expand Up @@ -182,7 +181,6 @@ public void setAngle(double degrees) {
enable();
}

@Override
public boolean isAbsoluteEncoderConnected() {
return io.isAbsoluteEncoderConnected();
}
Expand Down Expand Up @@ -316,6 +314,10 @@ public boolean isAtMinLimit() {
return isLimitLow();
}

private void stow() {
setAngle(ArmConstants.stowIntakeAngleInDegrees);
}

@Override
public Command getStowCommand() {
return runOnce(() -> stow());
Expand Down

0 comments on commit 82b4a88

Please sign in to comment.