Skip to content

Commit

Permalink
Cleaned up some variables to satisfy style checker.
Browse files Browse the repository at this point in the history
  • Loading branch information
rhsftc committed Nov 17, 2024
1 parent 4a79133 commit f3c255e
Showing 1 changed file with 9 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.util.ElapsedTime;

Expand All @@ -47,14 +46,9 @@
@TeleOp(name = "Encoder Test", group = "Test")
//@Disabled
public class EncoderTest extends OpMode {
private ElapsedTime runtime = new ElapsedTime();
private final ElapsedTime runtime = new ElapsedTime();
private GamepadEx gamePad;
private DcMotorEx motor;
private final float ENCODER_INCREMENT = 145.1f * 50f; // 10 revolutions
private final double MAX_VELOCITY = 2900;
private double RUN_VELOCITY = MAX_VELOCITY * .7f;
private PIDFCoefficients pidfVelocityCoefficients;
private PIDFCoefficients pidfPositionCoefficients;

/**
* This method will be called once, when the INIT button is pressed.
Expand All @@ -71,8 +65,8 @@ public void init() {
// Use encoders to enable manual movement of the motor to deternmine position values.
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
pidfVelocityCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER);
pidfPositionCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION);
PIDFCoefficients pidfVelocityCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER);
PIDFCoefficients pidfPositionCoefficients = motor.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION);
// These values are recommended as a starting point if you are tuning a PID.
pidfVelocityCoefficients.p = 1.063f;
pidfVelocityCoefficients.i = 1.063f;
Expand Down Expand Up @@ -113,7 +107,11 @@ public void start() {
@Override
public void loop() {
gamePad.readButtons();
telemetry.addData("Status", "Run Time: " + runtime.toString());
telemetry.addData("Status", "Run Time: " + runtime);
// The actual increment and speed will depend on the motor you are testing.
float ENCODER_INCREMENT = 5000f;
double MAX_VELOCITY = 2900;
double RUN_VELOCITY = MAX_VELOCITY * .7f;
if (gamePad.wasJustPressed(GamepadKeys.Button.DPAD_UP)) {
motor.setTargetPosition((int) (motor.getCurrentPosition() + ENCODER_INCREMENT));
motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Expand Down Expand Up @@ -156,7 +154,7 @@ public void stop() {
* This seems to be the only way to reliably stop a motor and reset the encoder.
* This wos only tested on a goBilda motor.
*
* @param motor
* @param motor Motor to stop.
*/
private void stopAndResetEncoder(DcMotorEx motor) {
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
Expand Down

0 comments on commit f3c255e

Please sign in to comment.