Last active
March 2, 2024 20:09
-
-
Save NoahBres/9b9710eaa9f9fd23efa30a16de0f610e to your computer and use it in GitHub Desktop.
This file contains 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
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 | |
); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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.