Skip to content

Commit

Permalink
Adress some comments
Browse files Browse the repository at this point in the history
  • Loading branch information
AnkitKumar5250 committed Jan 21, 2025
1 parent 86590d7 commit 0f7cc73
Show file tree
Hide file tree
Showing 2 changed files with 12 additions and 6 deletions.
3 changes: 0 additions & 3 deletions src/main/java/org/sciborgs1155/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,6 @@ private void configureBindings() {
drive.setDefaultCommand(drive.drive(x, y, omega));
led.setDefaultCommand(led.scrolling());

arm.setDefaultCommand(arm.goTo(ArmConstants.DEFAULT_ANGLE));

coroller.setDefaultCommand(coroller.stop());

Expand All @@ -164,8 +163,6 @@ private void configureBindings() {
.onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
.onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));

// TODO: Add any additional bindings.

}

/**
Expand Down
15 changes: 12 additions & 3 deletions src/main/java/org/sciborgs1155/robot/arm/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,8 @@ private Arm(ArmIO hardware) {
fb.setTolerance(POSITION_TOLERANCE.in(Radians));
fb.reset(STARTING_ANGLE.in(Radians));
fb.setGoal(STARTING_ANGLE.in(Radians));

setDefaultCommand(goTo(ArmConstants.DEFAULT_ANGLE));

this.sysIdRoutine =
new SysIdRoutine(
Expand Down Expand Up @@ -137,7 +139,7 @@ public Command goTo(Angle goal) {
/**
* Changes the current limit of the arm motor.
*
* @param limit The limit, in amps.
* @param limit : The limit, in amps.
*/
public void currentLimit(double limit) {
hardware.currentLimit(limit);
Expand Down Expand Up @@ -172,28 +174,35 @@ public Command climbSetup() {
* @return A command to climb.
*/
public Command climbExecute() {
return run(() -> currentLimit(CLIMB_LIMIT.in(Amps))).andThen(goTo(CLIMB_FINAL_ANGLE));
return runOnce(() -> currentLimit(CLIMB_LIMIT.in(Amps))).andThen(goTo(CLIMB_INTAKE_ANGLE));
}

// funny sysid stuff
/** @return A command that runs a quasistatic {@link SysIdRoutine}*/
@Log.NT
public Command quasistaticForward() {
return sysIdRoutine
.quasistatic(Direction.kForward)
.until(() -> position() > MAX_ANGLE.in(Radians) - 0.2);
}

/** @return A command that runs a quasistatic {@link SysIdRoutine}*/
@Log.NT
public Command quasistaticBack() {
return sysIdRoutine
.quasistatic(Direction.kReverse)
.until(() -> position() < MIN_ANGLE.in(Radians) + 0.2);
}

/** @return A command that runs a dynamic {@link SysIdRoutine}*/
@Log.NT
public Command dynamicForward() {
return sysIdRoutine
.dynamic(Direction.kForward)
.until(() -> position() > MAX_ANGLE.in(Radians) - 0.2);
}

/** @return A command that runs a dynamic {@link SysIdRoutine}*/
@Log.NT
public Command dynamicBack() {
return sysIdRoutine
.dynamic(Direction.kReverse)
Expand Down

0 comments on commit 0f7cc73

Please sign in to comment.