Skip to content

Instantly share code, notes, and snippets.

@NoahBres
Last active March 2, 2024 20:09
Show Gist options
  • Save NoahBres/9b9710eaa9f9fd23efa30a16de0f610e to your computer and use it in GitHub Desktop.
Save NoahBres/9b9710eaa9f9fd23efa30a16de0f610e to your computer and use it in GitHub Desktop.
package org.firstinspires.ftc.teamcode.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import java.util.Arrays;
import java.util.List;
/*
* Sample tracking wheel localizer implementation assuming the standard configuration:
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || |
* | |
* | |
* \--------------/
*
*/
@Config
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 0;
public static double WHEEL_RADIUS = 2; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
public static double X_MULTIPLIER = 1; // Multiplier in the X direction
public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction
private Encoder leftEncoder, rightEncoder, frontEncoder;
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
super(Arrays.asList(
new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
}
public static double encoderTicksToInches(int ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
@NonNull
@Override
public List<Double> getWheelPositions() {
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getCurrentPosition()) * Y_MULTIPLIER
);
}
@NonNull
@Override
public List<Double> getWheelVelocities() {
// TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
// competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
// compensation method
return Arrays.asList(
encoderTicksToInches(leftEncoder.getRawVelocity()) * X_MULTIPLIER,
encoderTicksToInches(rightEncoder.getRawVelocity()) * X_MULTIPLIER,
encoderTicksToInches(frontEncoder.getRawVelocity()) * Y_MULTIPLIER
);
}
}
@liangshaogithub
Copy link

TICKS_PER_REV = 0; why is it 0 instead of 8192?

@NoahBres
Copy link
Author

NoahBres commented Mar 2, 2024

You are supposed to input with whatever value corresponds with your encoder specification. The Rev Encoder is 8192. However, this has not historically been the only encoder in use.
I do not know the current state of affairs and the Rev Encoder may be the default now.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment