Added Test and Additional methods to AnglerSubsystem #43
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,41 @@ | ||
| package frc.robot.commands.angler; | ||
|
|
||
| import frc.robot.subsystems.AnglerSubsystem; | ||
| import frc.robot.utils.logging.commands.LoggableCommand; | ||
|
|
||
| /** | ||
| * Runs the angler to the reverse limit switch and resets the encoder to zero. | ||
| */ | ||
| public class RunAnglerToReverseLimit extends LoggableCommand { | ||
| private final AnglerSubsystem angler; | ||
| private boolean finished = false; | ||
|
|
||
| public RunAnglerToReverseLimit(AnglerSubsystem angler) { | ||
| this.angler = angler; | ||
| addRequirements(angler); | ||
| } | ||
|
|
||
| @Override | ||
| public void initialize() { | ||
| finished = false; | ||
| } | ||
|
|
||
| @Override | ||
| public void execute() { | ||
| angler.runToReverseLimit(); | ||
| if (angler.isAtReverseLimit()) { | ||
| angler.resetEncoderToZero(); | ||
| finished = true; | ||
| } | ||
| } | ||
|
|
||
| @Override | ||
| public void end(boolean interrupted) { | ||
| angler.stopMotors(); | ||
| } | ||
|
|
||
| @Override | ||
| public boolean isFinished() { | ||
| return finished; | ||
| } | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -7,6 +7,7 @@ | |
| import com.revrobotics.spark.config.SparkBaseConfig; | ||
| import com.revrobotics.spark.config.SparkMaxConfig; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.constants.Constants; | ||
| import frc.robot.utils.logging.input.MotorLoggableInputs; | ||
|
|
@@ -41,16 +42,80 @@ public void periodic() { | |
|
|
||
| /** | ||
| * Gets the desired Encoder Position and uses a PID controller to get the motor there | ||
| */ | ||
| public void setPosition(double targetEncoderPosition) { | ||
| io.setPidPosition(targetEncoderPosition); | ||
| } | ||
|
|
||
| /** | ||
| * Gets the desired Angle Position and uses a PID controller to get the motor there | ||
| */ | ||
|
Contributor
There was a problem hiding this comment. Add comment about the units the angle is expected to be (@param...) |
||
| public void setAngle(double targetAngle) { | ||
| double targetRotations = calculateRotationsForAngle( | ||
| targetAngle, | ||
| Constants.ANGLER_ENCODER_HIGH, | ||
| Constants.ANGLER_ENCODER_LOW, | ||
| Constants.ANGLER_ANGLE_HIGH, | ||
| Constants.ANGLER_ANGLE_LOW); | ||
| setPosition(targetRotations); | ||
| } | ||
|
|
||
| //Range translate code with a clamp | ||
| public static double calculateRotationsForAngle( | ||
| double targetAngle, | ||
| double encoderHigh, | ||
| double encoderLow, | ||
| double angleHigh, | ||
| double angleLow) { | ||
|
|
||
| double multipler = (encoderHigh - encoderLow) / (angleHigh - angleLow); | ||
| double targetRotations = targetAngle * multipler - (multipler * angleHigh - encoderHigh); | ||
| return MathUtil.clamp(targetRotations, encoderLow, encoderHigh); | ||
| } | ||
|
|
||
| public void stopMotors() { | ||
| io.stopMotor(); | ||
| } | ||
|
|
||
| /** | ||
| * Drive toward the forward limit switch and stop once it is pressed. | ||
| * Call this repeatedly from a command while homing. | ||
| */ | ||
| public void runToForwardLimit() { | ||
|
Contributor
There was a problem hiding this comment. The way this method is written, it is expecting to be called in a loop until the switch is hit - if it only called once, the motor will run forever (well, it will run until the limit switch is hit and the hardware stops the motor). |
||
| if (io.isFwdSwitchPressed()) { | ||
| io.stopMotor(); | ||
| return; | ||
| } | ||
| io.set(Math.abs(Constants.ANGLER_LIMIT_SPEED)); | ||
| } | ||
|
|
||
| /** | ||
| * Drive toward the reverse limit switch and stop once it is pressed. | ||
| * Call this repeatedly from a command while homing. | ||
| */ | ||
| public void runToReverseLimit() { | ||
| if (io.isRevSwitchPressed()) { | ||
| io.stopMotor(); | ||
| return; | ||
| } | ||
| io.set(-Math.abs(Constants.ANGLER_LIMIT_SPEED)); | ||
| } | ||
|
|
||
| /** | ||
| * Reset the encoder position to zero. | ||
| */ | ||
| public void resetEncoderToZero() { | ||
| io.resetEncoderPosition(0.0); | ||
| } | ||
|
|
||
| public boolean isAtForwardLimit() { | ||
| return io.isFwdSwitchPressed(); | ||
| } | ||
|
|
||
| public boolean isAtReverseLimit() { | ||
| return io.isRevSwitchPressed(); | ||
| } | ||
|
|
||
| public static SparkMaxPidMotorIo createMockIo() { | ||
| return new MockSparkMaxPidMotorIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); | ||
| } | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,68 @@ | ||
| import static org.junit.jupiter.api.Assertions.assertEquals; | ||
|
|
||
| import org.junit.jupiter.api.Test; | ||
|
|
||
| import frc.robot.constants.Constants; | ||
| import frc.robot.subsystems.AnglerSubsystem; | ||
|
|
||
| public class AnglerCalculationsTest { | ||
|
|
||
| static final double DELTA = 0.1; | ||
|
|
||
| double[] angles = {0, 45, 20, -20, 60, 30}; | ||
| double[] expectedRotations = { | ||
| 0.0, | ||
| 100.0, | ||
| 44.46, | ||
| 0.0, | ||
| 100.0, | ||
| 66.67 | ||
| }; | ||
|
|
||
| private int index; | ||
|
|
||
| private double calculate(double angle) { | ||
| return AnglerSubsystem.calculateRotationsForAngle( | ||
| angle, | ||
| Constants.ANGLER_ENCODER_HIGH, | ||
|
Contributor
There was a problem hiding this comment. This would break when the constants change, so you should hardcode test values that will not change with the constants to separate the test code from the real ones. |
||
| Constants.ANGLER_ENCODER_LOW, | ||
| Constants.ANGLER_ANGLE_HIGH, | ||
| Constants.ANGLER_ANGLE_LOW); | ||
| } | ||
|
|
||
| @Test | ||
| void angle0Test() { | ||
| index = 0; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
|
|
||
| @Test | ||
| void angle45Test() { | ||
| index = 1; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
|
|
||
| @Test | ||
| void angle20Test() { | ||
| index = 2; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
|
|
||
| @Test | ||
| void angleNeg20Test() { | ||
| index = 3; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
|
|
||
| @Test | ||
| void angle60Test() { | ||
| index = 4; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
|
|
||
| @Test | ||
| void angle30Test() { | ||
| index = 5; | ||
| assertEquals(expectedRotations[index], calculate(angles[index]), DELTA); | ||
| } | ||
| } | ||
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We should probably have a timeout of 5 seconds(?) to prevent this from hanging forever if the switch is broken