SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Add initial quickstart files

Browse Source
This commit is contained in:
Ryan Brott
2022-09-22 08:11:23 -07:00
parent 2390680ce6
commit 67b0165c32
37 changed files with 4139 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

261
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Action.java Normal file
View File

@ -0,0 +1,261 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
public interface Action {
void init();
default boolean loop(TelemetryPacket p) {
return false;
}
default void draw(Canvas c) {
}
default void runBlocking() {
if (Thread.currentThread().isInterrupted()) return;
init();
boolean b = true;
while (!Thread.currentThread().isInterrupted() && b) {
TelemetryPacket p = new TelemetryPacket();
draw(p.fieldOverlay());
b = loop(p);
FtcDashboard.getInstance().sendTelemetryPacket(p);
}
}
default double clock() {
return 1e-9 * System.nanoTime();
}
// not persistent!
interface IBuilder<T extends IBuilder<T>> {
T add(Action a);
}
// parent-less, generic-less SeqBuilder
class Builder implements IBuilder<Builder> {
private final List<Action> as = new ArrayList<>();
@Override
public Builder add(Action a) {
as.add(a);
return this;
}
public SeqBuilder<Builder> seq() {
return new SeqBuilder<>(this);
}
public ParBuilder<Builder> par() {
return new ParBuilder<>(this);
}
public Builder sleep(double duration) {
return add(new SleepAction(duration));
}
public Builder after(double duration, Action a) {
return seq().sleep(duration).add(a).end();
}
public Action build() {
return new SequentialAction(as);
}
}
class SeqBuilder<T extends IBuilder<T>> implements IBuilder<SeqBuilder<T>> {
private final T parent;
private final List<Action> as = new ArrayList<>();
private SeqBuilder(T parent) {
this.parent = parent;
}
@Override
public SeqBuilder<T> add(Action a) {
as.add(a);
return this;
}
public T end() {
parent.add(new SequentialAction(as));
return parent;
}
public SeqBuilder<SeqBuilder<T>> seq() {
return new SeqBuilder<>(this);
}
public ParBuilder<SeqBuilder<T>> par() {
return new ParBuilder<>(this);
}
public SeqBuilder<T> sleep(double duration) {
return add(new SleepAction(duration));
}
public SeqBuilder<T> after(double duration, Action a) {
return seq().sleep(duration).add(a).end();
}
}
final class ParBuilder<T extends IBuilder<T>> implements IBuilder<ParBuilder<T>> {
private final T parent;
private final List<Action> as = new ArrayList<>();
private ParBuilder(T parent) {
this.parent = parent;
}
@Override
public ParBuilder<T> add(Action a) {
as.add(a);
return this;
}
public T end() {
parent.add(new ParallelAction(as));
return parent;
}
public SeqBuilder<ParBuilder<T>> seq() {
return new SeqBuilder<>(this);
}
public ParBuilder<ParBuilder<T>> par() {
return new ParBuilder<>(this);
}
public ParBuilder<T> sleep(double duration) {
return add(new SleepAction(duration));
}
public ParBuilder<T> after(double duration, Action a) {
return seq().sleep(duration).add(a).end();
}
}
}
final class SleepAction implements Action {
public final double duration;
private double endTs;
public SleepAction(double duration) {
this.duration = duration;
}
@Override
public void init() {
endTs = clock() + duration;
}
@Override
public boolean loop(TelemetryPacket p) {
return clock() <= endTs;
}
}
final class ParallelAction implements Action {
public final List<Action> actions;
private List<Action> remaining;
public ParallelAction(List<Action> actions) {
this.actions = Collections.unmodifiableList(actions);
}
public ParallelAction(Action... actions) {
this(Arrays.asList(actions));
}
@Override
public void init() {
remaining = new ArrayList<>(actions);
for (Action a : remaining) {
a.init();
}
}
@Override
public boolean loop(TelemetryPacket p) {
List<Action> newRem = new ArrayList<>();
for (Action a : remaining) {
if (a.loop(p)) {
newRem.add(a);
}
}
remaining = newRem;
return remaining.size() > 0;
}
@Override
public void draw(Canvas c) {
for (Action a : actions) {
draw(c);
}
}
}
final class SequentialAction implements Action {
public final List<Action> actions;
private int index;
private boolean needsInit;
public SequentialAction(List<Action> actions) {
this.actions = Collections.unmodifiableList(actions);
}
public SequentialAction(Action... actions) {
this(Arrays.asList(actions));
}
@Override
public void init() {
if (actions.isEmpty()) {
return;
}
actions.get(0).init();
}
@Override
public boolean loop(TelemetryPacket p) {
if (index >= actions.size()) {
return false;
}
Action a = actions.get(index);
if (needsInit) {
a.init();
needsInit = false;
}
if (!a.loop(p)) {
index++;
needsInit = true;
}
return index < actions.size();
}
@Override
public void draw(Canvas c) {
for (Action a : actions) {
draw(c);
}
}
}

476
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java Normal file
View File

@ -0,0 +1,476 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.AccelConstraint;
import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController;
import com.acmerobotics.roadrunner.MecanumKinematics;
import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.Profiles;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Rotation2dDual;
import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeProfile;
import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.Trajectory;
import com.acmerobotics.roadrunner.TrajectoryBuilder;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
@Config
public final class MecanumDrive {
// drive model parameters
public static double IN_PER_TICK = 0;
public static double LATERAL_IN_PER_TICK = 1;
public static double TRACK_WIDTH_TICKS = 0;
// feedforward parameters
public static double kS = 0;
public static double kV = 0;
public static double kA = 0;
// path profile parameters
public static double MAX_WHEEL_VEL = 50;
public static double MIN_PROFILE_ACCEL = -30;
public static double MAX_PROFILE_ACCEL = 50;
// turn profile parameters
public static double MAX_ANG_VEL = Math.PI; // shared with path
public static double MAX_ANG_ACCEL = Math.PI;
// path controller gains
public static double AXIAL_GAIN = 0.0;
public static double LATERAL_GAIN = 0.0;
public static double HEADING_GAIN = 0.0; // shared with turn
public static double AXIAL_VEL_GAIN = 0.0;
public static double LATERAL_VEL_GAIN = 0.0;
public static double HEADING_VEL_GAIN = 0.0; // shared with turn
public final MecanumKinematics kinematics = new MecanumKinematics(
IN_PER_TICK * TRACK_WIDTH_TICKS,
IN_PER_TICK / LATERAL_IN_PER_TICK);
public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
new AngularVelConstraint(MAX_ANG_VEL)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
public final VoltageSensor voltageSensor;
public final BNO055Wrapper imu;
public final Localizer localizer;
public Pose2d pose;
public final double inPerTick = IN_PER_TICK;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftRear, rightRear, rightFront;
private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos;
public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
lastLeftFrontPos = leftFront.getPositionAndVelocity().position;
lastLeftRearPos = leftRear.getPositionAndVelocity().position;
lastRightRearPos = rightRear.getPositionAndVelocity().position;
lastRightFrontPos = rightFront.getPositionAndVelocity().position;
}
@Override
public Twist2dIncrDual<Time> updateAndGetIncr() {
Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
MecanumKinematics.WheelIncrements<Time> incrs = new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
leftFrontPosVel.position - lastLeftFrontPos,
leftFrontPosVel.velocity,
}).times(inPerTick),
new DualNum<Time>(new double[]{
leftRearPosVel.position - lastLeftRearPos,
leftRearPosVel.velocity,
}).times(inPerTick),
new DualNum<Time>(new double[]{
rightRearPosVel.position - lastRightRearPos,
rightRearPosVel.velocity,
}).times(inPerTick),
new DualNum<Time>(new double[]{
rightFrontPosVel.position - lastRightFrontPos,
rightFrontPosVel.velocity,
}).times(inPerTick)
);
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftRearPos = leftRearPosVel.position;
lastRightRearPos = rightRearPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
return kinematics.forward(incrs);
}
}
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
new LynxFirmwareVersion(1, 8, 2));
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
baseImu.initialize(parameters);
imu = new BNO055Wrapper(baseImu);
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new DriveLocalizer();
}
public void setDrivePowers(Twist2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
leftFront.setPower(wheelVels.leftFront.get(0));
leftBack.setPower(wheelVels.leftBack.get(0));
rightBack.setPower(wheelVels.rightBack.get(0));
rightFront.setPower(wheelVels.rightFront.get(0));
}
public final class FollowTrajectoryAction implements Action {
public final Trajectory trajectory;
private TimeTrajectory timeTrajectory;
private double beginTs;
private boolean active;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(Trajectory t) {
this.trajectory = t;
timeTrajectory = new TimeTrajectory(t);
List<Double> disps = com.acmerobotics.roadrunner.Math.range(
0, t.path.length(),
(int) Math.ceil(t.path.length() / 2));
xPoints = new double[disps.size()];
yPoints = new double[disps.size()];
for (int i = 0; i < disps.size(); i++) {
Pose2d p = t.path.get(disps.get(i), 1).value();
xPoints[i] = p.trans.x;
yPoints[i] = p.trans.y;
}
}
@Override
public void init() {
beginTs = clock();
active = true;
}
@Override
public boolean loop(TelemetryPacket p) {
double t = clock() - beginTs;
if (t >= timeTrajectory.duration) {
leftFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);
rightFront.setPower(0);
active = false;
return false;
}
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
Twist2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
)
.compute(txWorldTarget, pose, robotVelRobot);
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
p.put("x", pose.trans.x);
p.put("y", pose.trans.y);
p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.trans.x);
p.put("yError", error.trans.y);
p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
drawRobot(c, pose);
return true;
}
public void cancel() {
double t = clock() - beginTs;
double s = timeTrajectory.profile.get(t).value();
beginTs += t;
timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
}
@Override
public void draw(Canvas c) {
c.setStrokeWidth(1);
c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
c.strokePolyline(xPoints, yPoints);
}
}
public final class TurnAction implements Action {
private final Pose2d beginPose;
private final TimeProfile profile;
private double beginTs;
private boolean active;
public TurnAction(Pose2d beginPose, double angle) {
this.beginPose = beginPose;
profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
}
@Override
public void init() {
beginTs = clock();
active = true;
}
@Override
public boolean loop(TelemetryPacket p) {
double t = clock() - beginTs;
if (t >= profile.duration) {
leftFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);
rightFront.setPower(0);
active = false;
return false;
}
DualNum<Time> x = profile.get(t);
Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
Twist2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
)
.compute(txWorldTarget, pose, robotVelRobot);
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
drawRobot(c, pose);
return true;
}
@Override
public void draw(Canvas c) {
c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
}
}
private Twist2d updatePoseEstimateAndGetActualVel() {
Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
pose = pose.plus(incr.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
return incr.velocity().value();
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.trans.x;
yPoints[i] = t.trans.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
private static void drawRobot(Canvas c, Pose2d t) {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.trans.plus(halfv);
Vector2d p2 = p1.plus(halfv);
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
}
public TurnAction turn(Pose2d beginPose, double angle) {
return new TurnAction(beginPose, angle);
}
public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
return new TurnAction(beginPose, rot.minus(beginPose.rot));
}
public TurnAction turnTo(Pose2d beginPose, double rot) {
return turnTo(beginPose, Rotation2d.exp(rot));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return new SafeTrajectoryBuilder(
beginPose, beginTangent, 1e-6,
0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, false, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
return new SafeTrajectoryBuilder(
beginPose, beginTangent, 1e-6,
0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
return trajectoryBuilder(beginPose, false);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
return trajectoryBuilder(traj.path.end(1).value(), reversed);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
return trajectoryBuilder(traj.path.end(1).value());
}
public FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
return new FollowTrajectoryAction(trajectory);
}
}

502
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java Normal file
View File

@ -0,0 +1,502 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.AccelConstraint;
import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.Arclength;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.Profiles;
import com.acmerobotics.roadrunner.RamseteController;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Rotation2dDual;
import com.acmerobotics.roadrunner.SafeTrajectoryBuilder;
import com.acmerobotics.roadrunner.TankKinematics;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeProfile;
import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.Trajectory;
import com.acmerobotics.roadrunner.TrajectoryBuilder;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
@Config
public final class TankDrive {
// drive model parameters
public static double IN_PER_TICK = 0;
public static double TRACK_WIDTH_TICKS = 0;
// feedforward parameters
public static double kS = 0;
public static double kV = 0;
public static double kA = 0;
// path profile parameters
public static double MAX_WHEEL_VEL = 50;
public static double MIN_PROFILE_ACCEL = -30;
public static double MAX_PROFILE_ACCEL = 50;
// turn profile parameters
public static double MAX_ANG_VEL = Math.PI; // shared with path
public static double MAX_ANG_ACCEL = Math.PI;
// path controller gains
public static double RAMSETE_ZETA = 0.7; // in the range (0, 1)
public static double RAMSETE_BBAR = 2.0; // positive
// turn controller gains
public static double TURN_GAIN = 0.0;
public static double TURN_VEL_GAIN = 0.0;
public final TankKinematics kinematics = new TankKinematics(IN_PER_TICK * TRACK_WIDTH_TICKS);
public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV, kA);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
new AngularVelConstraint(MAX_ANG_VEL)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL);
public final List<DcMotorEx> leftMotors, rightMotors;
public final BNO055Wrapper imu;
public final VoltageSensor voltageSensor;
public final Localizer localizer;
public Pose2d pose;
public final double inPerTick = IN_PER_TICK;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
public class DriveLocalizer implements Localizer {
public final List<Encoder> leftEncs, rightEncs;
private double lastLeftPos, lastRightPos;
public DriveLocalizer() {
{
List<Encoder> leftEncs = new ArrayList<>();
for (DcMotorEx m : leftMotors) {
Encoder e = new OverflowEncoder(new RawEncoder(m));
leftEncs.add(e);
lastLeftPos += e.getPositionAndVelocity().position;
}
lastLeftPos /= leftEncs.size();
this.leftEncs = Collections.unmodifiableList(leftEncs);
}
{
List<Encoder> rightEncs = new ArrayList<>();
for (DcMotorEx m : rightMotors) {
Encoder e = new OverflowEncoder(new RawEncoder(m));
rightEncs.add(e);
lastRightPos += e.getPositionAndVelocity().position;
}
lastRightPos /= rightEncs.size();
this.rightEncs = Collections.unmodifiableList(rightEncs);
}
}
@Override
public Twist2dIncrDual<Time> updateAndGetIncr() {
double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
meanLeftPos += p.position;
meanLeftVel += p.velocity;
}
meanLeftPos /= leftEncs.size();
meanLeftVel /= leftEncs.size();
double meanRightPos = 0.0, meanRightVel = 0.0;
for (Encoder e : rightEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
meanRightPos += p.position;
meanRightVel += p.velocity;
}
meanRightPos /= rightEncs.size();
meanRightVel /= rightEncs.size();
TankKinematics.WheelIncrements<Time> incrs = new TankKinematics.WheelIncrements<>(
new DualNum<Time>(new double[] {
meanLeftPos - lastLeftPos,
meanLeftVel
}).times(inPerTick),
new DualNum<Time>(new double[] {
meanRightPos - lastRightPos,
meanRightVel,
}).times(inPerTick)
);
lastLeftPos = meanLeftPos;
lastRightPos = meanRightPos;
return kinematics.forward(incrs);
}
}
public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
new LynxFirmwareVersion(1, 8, 2));
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left"));
rightMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "right"));
for (DcMotorEx m : leftMotors) {
m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
for (DcMotorEx m : rightMotors) {
m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
baseImu.initialize(parameters);
imu = new BNO055Wrapper(baseImu);
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new TankDrive.DriveLocalizer();
}
public void setDrivePowers(Twist2d powers) {
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(Twist2dDual.constant(powers, 1));
for (DcMotorEx m : leftMotors) {
m.setPower(wheelVels.left.get(0));
}
for (DcMotorEx m : rightMotors) {
m.setPower(wheelVels.right.get(0));
}
}
public final class FollowTrajectoryAction implements Action {
public final Trajectory trajectory;
private TimeTrajectory timeTrajectory;
private double beginTs;
private boolean active;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(Trajectory t) {
this.trajectory = t;
timeTrajectory = new TimeTrajectory(t);
List<Double> disps = com.acmerobotics.roadrunner.Math.range(
0, t.path.length(),
(int) Math.ceil(t.path.length() / 2));
xPoints = new double[disps.size()];
yPoints = new double[disps.size()];
for (int i = 0; i < disps.size(); i++) {
Pose2d p = t.path.get(disps.get(i), 1).value();
xPoints[i] = p.trans.x;
yPoints[i] = p.trans.y;
}
}
@Override
public void init() {
beginTs = clock();
active = true;
}
@Override
public boolean loop(TelemetryPacket p) {
double t = clock() - beginTs;
if (t >= timeTrajectory.duration) {
for (DcMotorEx m : leftMotors) {
m.setPower(0);
}
for (DcMotorEx m : rightMotors) {
m.setPower(0);
}
active = false;
return false;
}
DualNum<Time> x = timeTrajectory.profile.get(t);
Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
updatePoseEstimateAndGetActualVel();
Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
.compute(x, txWorldTarget, pose);
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage);
}
for (DcMotorEx m : rightMotors) {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
}
p.put("x", pose.trans.x);
p.put("y", pose.trans.y);
p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.trans.x);
p.put("yError", error.trans.y);
p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
drawRobot(c, pose);
return true;
}
public void cancel() {
double t = clock() - beginTs;
double s = timeTrajectory.profile.get(t).value();
beginTs += t;
timeTrajectory = new TimeTrajectory(trajectory.cancel(s));
}
@Override
public void draw(Canvas c) {
c.setStrokeWidth(1);
c.setStroke(active ? "#4CAF50FF" : "#4CAF507A");
c.strokePolyline(xPoints, yPoints);
}
}
public final class TurnAction implements Action {
private final Pose2d beginPose;
private final TimeProfile profile;
private double beginTs;
private boolean active;
public TurnAction(Pose2d beginPose, double angle) {
this.beginPose = beginPose;
profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
}
@Override
public void init() {
beginTs = clock();
active = true;
}
@Override
public boolean loop(TelemetryPacket p) {
double t = clock() - beginTs;
if (t >= profile.duration) {
for (DcMotorEx m : leftMotors) {
m.setPower(0);
}
for (DcMotorEx m : rightMotors) {
m.setPower(0);
}
active = false;
return false;
}
DualNum<Time> x = profile.get(t);
Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
Rotation2dDual<Time> target = Rotation2dDual.<Time>constant(beginPose.rot, 3).plus(x);
Twist2dDual<Time> command = new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0, 0), 3),
target.velocity().plus(
TURN_GAIN * pose.rot.minus(target.value()) +
TURN_VEL_GAIN * (robotVelRobot.rotVel - target.velocity().value())
)
);
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage);
}
for (DcMotorEx m : rightMotors) {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
}
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
drawRobot(c, new Pose2d(beginPose.trans, beginPose.rot.plus(x.value())));
c.setStroke("#3F51B5");
drawRobot(c, pose);
return true;
}
@Override
public void draw(Canvas c) {
c.setFill(active ? "#7C4DFFFF" : "#7C4DFF7A");
c.fillCircle(beginPose.trans.x, beginPose.trans.y, 2);
}
}
public Twist2d updatePoseEstimateAndGetActualVel() {
Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
pose = pose.plus(incr.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
return incr.velocity().value();
}
private void drawPoseHistory(Canvas c) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.trans.x;
yPoints[i] = t.trans.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
private static void drawRobot(Canvas c, Pose2d t) {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.trans.plus(halfv);
Vector2d p2 = p1.plus(halfv);
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
}
public TurnAction turn(Pose2d beginPose, double angle) {
return new TurnAction(beginPose, angle);
}
public TurnAction turnTo(Pose2d beginPose, Rotation2d rot) {
return new TurnAction(beginPose, rot.minus(beginPose.rot));
}
public TurnAction turnTo(Pose2d beginPose, double rot) {
return turnTo(beginPose, Rotation2d.exp(rot));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return new SafeTrajectoryBuilder(
beginPose, beginTangent, 1e-6,
0.0, defaultVelConstraint, defaultAccelConstraint, 0.25, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(beginPose, false, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, Rotation2d beginTangent) {
return new SafeTrajectoryBuilder(
beginPose, beginTangent, 1e-6,
0.0, defaultVelConstraint, defaultAccelConstraint, 0.25);
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, double beginTangent) {
return trajectoryBuilder(beginPose, Rotation2d.exp(beginTangent));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose, boolean reversed) {
return trajectoryBuilder(beginPose, beginPose.rot.plus(reversed ? Math.PI : 0));
}
public SafeTrajectoryBuilder trajectoryBuilder(Pose2d beginPose) {
return trajectoryBuilder(beginPose, false);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), reversed, poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, TrajectoryBuilder.PoseMap poseMap) {
return trajectoryBuilder(traj.path.end(1).value(), poseMap);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, Rotation2d beginTangent) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, double beginTangent) {
return trajectoryBuilder(traj.path.end(1).value(), beginTangent);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj, boolean reversed) {
return trajectoryBuilder(traj.path.end(1).value(), reversed);
}
public SafeTrajectoryBuilder trajectoryBuilder(Trajectory traj) {
return trajectoryBuilder(traj.path.end(1).value());
}
public TankDrive.FollowTrajectoryAction followTrajectory(Trajectory trajectory) {
return new TankDrive.FollowTrajectoryAction(trajectory);
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java Normal file
View File

@ -0,0 +1,71 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
@Config
public final class ThreeDeadWheelLocalizer implements Localizer {
public static double PAR0_Y_TICKS = 0.0;
public static double PAR1_Y_TICKS = 0.0;
public static double PERP_X_TICKS = 0.0;
public final Encoder par0, par1, perp;
public final double inPerTick;
private int lastPar0Pos, lastPar1Pos, lastPerpPos;
public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
par0 = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"));
par1 = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par1"));
perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
lastPar0Pos = par0.getPositionAndVelocity().position;
lastPar1Pos = par1.getPositionAndVelocity().position;
lastPerpPos = perp.getPositionAndVelocity().position;
this.inPerTick = inPerTick;
}
public Twist2dIncrDual<Time> updateAndGetIncr() {
Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
int par0PosDelta = par0PosVel.position - lastPar0Pos;
int par1PosDelta = par1PosVel.position - lastPar1Pos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
(PAR0_Y_TICKS * par1PosDelta - PAR1_Y_TICKS * par0PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
(PAR0_Y_TICKS * par1PosVel.velocity - PAR1_Y_TICKS * par0PosVel.velocity) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
}).times(inPerTick),
new DualNum<Time>(new double[] {
(PERP_X_TICKS / (PAR0_Y_TICKS - PAR1_Y_TICKS) * (par1PosDelta - par0PosDelta) + perpPosDelta),
(PERP_X_TICKS / (PAR0_Y_TICKS - PAR1_Y_TICKS) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
}).times(inPerTick)
),
new DualNum<>(new double[] {
(par0PosDelta - par1PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
(par0PosVel.velocity - par1PosVel.velocity) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
})
);
lastPar0Pos = par0PosVel.position;
lastPar1Pos = par1PosVel.position;
lastPerpPos = perpPosVel.position;
return twistIncr;
}
}

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java Normal file
View File

@ -0,0 +1,76 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
@Config
public final class TwoDeadWheelLocalizer implements Localizer {
public static double PAR_Y_TICKS = 0.0;
public static double PERP_X_TICKS = 0.0;
public final Encoder par, perp;
public final BNO055Wrapper imu;
private int lastParPos, lastPerpPos;
private Rotation2d lastHeading;
private final double inPerTick;
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, BNO055Wrapper imu, double inPerTick) {
par = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"));
perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
this.imu = imu;
lastParPos = par.getPositionAndVelocity().position;
lastPerpPos = perp.getPositionAndVelocity().position;
lastHeading = imu.getHeading();
this.inPerTick = inPerTick;
}
public Twist2dIncrDual<Time> updateAndGetIncr() {
Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = imu.getHeading();
int parPosDelta = parPosVel.position - lastParPos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
double headingDelta = heading.minus(lastHeading);
double headingVel = imu.getHeadingVelocity();
Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
parPosDelta - PAR_Y_TICKS * headingDelta,
parPosVel.velocity - PAR_Y_TICKS * headingVel,
}).times(inPerTick),
new DualNum<Time>(new double[] {
perpPosDelta - PERP_X_TICKS * headingDelta,
perpPosVel.velocity - PERP_X_TICKS * headingVel,
}).times(inPerTick)
),
new DualNum<>(new double[] {
headingDelta,
headingVel,
})
);
lastParPos = parPosVel.position;
lastPerpPos = perpPosVel.position;
lastHeading = heading;
return twistIncr;
}
}

135
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java Normal file
View File

@ -0,0 +1,135 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import java.util.ArrayList;
import java.util.List;
public final class AngularRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
class Data {
final String type = view.type;
final List<List<Double>> leftPowerTimes = new ArrayList<>();
final List<List<Double>> leftPowers = new ArrayList<>();
final List<List<Double>> rightPowerTimes = new ArrayList<>();
final List<List<Double>> rightPowers = new ArrayList<>();
final List<Double> voltageTimes = new ArrayList<>();
final List<Double> voltages = new ArrayList<>();
final List<Double> encTimes = new ArrayList<>();
final List<List<Integer>> leftEncPositions = new ArrayList<>();
final List<List<Integer>> leftEncVels = new ArrayList<>();
final List<List<Integer>> rightEncPositions = new ArrayList<>();
final List<List<Integer>> rightEncVels = new ArrayList<>();
final List<List<Integer>> parEncPositions = new ArrayList<>();
final List<List<Integer>> parEncVels = new ArrayList<>();
final List<List<Integer>> perpEncPositions = new ArrayList<>();
final List<List<Integer>> perpEncVels = new ArrayList<>();
final List<Double> angVelTimes = new ArrayList<>();
final List<List<Double>> angVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.leftMotors) {
data.leftPowerTimes.add(new ArrayList<>());
data.leftPowers.add(new ArrayList<>());
}
for (DcMotorEx m : view.rightMotors) {
data.rightPowerTimes.add(new ArrayList<>());
data.rightPowers.add(new ArrayList<>());
}
for (Encoder e : view.leftEncs) {
data.leftEncPositions.add(new ArrayList<>());
data.leftEncVels.add(new ArrayList<>());
}
for (Encoder e : view.rightEncs) {
data.rightEncPositions.add(new ArrayList<>());
data.rightEncVels.add(new ArrayList<>());
}
for (Encoder e : view.parEncs) {
data.parEncPositions.add(new ArrayList<>());
data.parEncVels.add(new ArrayList<>());
}
for (Encoder e : view.perpEncs) {
data.perpEncPositions.add(new ArrayList<>());
data.perpEncVels.add(new ArrayList<>());
}
for (int i = 0; i < 3; i++) {
data.angVels.add(new ArrayList<>());
}
waitForStart();
MidpointTimer t = new MidpointTimer();
while (opModeIsActive()) {
for (int i = 0; i < view.leftMotors.size(); i++) {
double power = -power(t.seconds());
view.leftMotors.get(i).setPower(power);
data.leftPowers.get(i).add(power);
data.leftPowerTimes.get(i).add(t.addSplit());
}
for (int i = 0; i < view.rightMotors.size(); i++) {
double power = power(t.seconds());
view.rightMotors.get(i).setPower(power);
data.rightPowers.get(i).add(power);
data.rightPowerTimes.get(i).add(t.addSplit());
}
data.voltages.add(view.voltageSensor.getVoltage());
data.voltageTimes.add(t.addSplit());
for (int i = 0; i < view.leftEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.leftEncs.get(i).getPositionAndVelocity();
data.leftEncPositions.get(i).add(p.position);
data.leftEncVels.get(i).add(p.velocity);
}
for (int i = 0; i < view.rightEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.rightEncs.get(i).getPositionAndVelocity();
data.rightEncPositions.get(i).add(p.position);
data.rightEncVels.get(i).add(p.velocity);
}
for (int i = 0; i < view.parEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.parEncs.get(i).getPositionAndVelocity();
data.parEncPositions.get(i).add(p.position);
data.parEncVels.get(i).add(p.velocity);
}
for (int i = 0; i < view.perpEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.perpEncs.get(i).getPositionAndVelocity();
data.perpEncPositions.get(i).add(p.position);
data.perpEncVels.get(i).add(p.velocity);
}
data.encTimes.add(t.addSplit());
AngularVelocity av = view.imu.getAngularVelocity();
data.angVels.get(0).add((double) av.xRotationRate);
data.angVels.get(1).add((double) av.yRotationRate);
data.angVels.get(2).add((double) av.zRotationRate);
data.angVelTimes.add(t.addSplit());
}
for (DcMotorEx m : view.motors) {
m.setPower(0);
}
TuningFiles.save(TuningFiles.FileType.ANGULAR_RAMP, data);
}
}

170
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java Normal file
View File

@ -0,0 +1,170 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
final class DriveView {
public final String type;
public final double inPerTick;
public final double maxVel, minAccel, maxAccel;
public final List<DcMotorEx> leftMotors, rightMotors;
public final List<DcMotorEx> motors;
// invariant: (leftEncs.isEmpty() && rightEncs.isEmpty()) ||
// (parEncs.isEmpty() && perpEncs.isEmpty())
public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
public final List<RawEncoder> forwardEncs;
public final BNO055Wrapper imu;
public final VoltageSensor voltageSensor;
private final MecanumDrive md;
private final TankDrive td;
private static RawEncoder unwrap(Encoder e) {
if (e instanceof OverflowEncoder) {
return ((OverflowEncoder) e).encoder;
} else {
return (RawEncoder) e;
}
}
public DriveView(HardwareMap hardwareMap) {
final Localizer localizer;
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
type = "mecanum";
inPerTick = MecanumDrive.IN_PER_TICK;
maxVel = MecanumDrive.MAX_WHEEL_VEL;
minAccel = MecanumDrive.MIN_PROFILE_ACCEL;
maxAccel = MecanumDrive.MAX_PROFILE_ACCEL;
md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); td = null;
leftMotors = Arrays.asList(md.leftFront, md.leftBack);
rightMotors = Arrays.asList(md.rightFront, md.rightBack);
imu = md.imu;
voltageSensor = md.voltageSensor;
localizer = md.localizer;
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
type = "tank";
inPerTick = TankDrive.IN_PER_TICK;
maxVel = TankDrive.MAX_WHEEL_VEL;
minAccel = TankDrive.MIN_PROFILE_ACCEL;
maxAccel = TankDrive.MAX_PROFILE_ACCEL;
td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); md = null;
leftMotors = td.leftMotors;
rightMotors = td.rightMotors;
imu = td.imu;
voltageSensor = td.voltageSensor;
localizer = td.localizer;
} else {
throw new AssertionError();
}
if (localizer instanceof TwoDeadWheelLocalizer) {
TwoDeadWheelLocalizer l2 = (TwoDeadWheelLocalizer) localizer;
parEncs = Collections.singletonList(unwrap(l2.par));
perpEncs = Collections.singletonList(unwrap(l2.perp));
leftEncs = Collections.emptyList();
rightEncs = Collections.emptyList();
} else if (localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer l3 = (ThreeDeadWheelLocalizer) localizer;
parEncs = Arrays.asList(unwrap(l3.par0), unwrap(l3.par1));
perpEncs = Collections.singletonList(unwrap(l3.perp));
leftEncs = Collections.emptyList();
rightEncs = Collections.emptyList();
} else if (localizer instanceof MecanumDrive.DriveLocalizer) {
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) localizer;
parEncs = Collections.emptyList();
perpEncs = Collections.emptyList();
leftEncs = Arrays.asList(unwrap(dl.leftFront), unwrap(dl.leftRear));
rightEncs = Arrays.asList(unwrap(dl.rightFront), unwrap(dl.rightRear));
} else if (localizer instanceof TankDrive.DriveLocalizer) {
TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) localizer;
parEncs = Collections.emptyList();
perpEncs = Collections.emptyList();
leftEncs = new ArrayList<>();
for (Encoder e : dl.leftEncs) {
leftEncs.add(unwrap(e));
}
rightEncs = new ArrayList<>();
for (Encoder e : dl.rightEncs) {
rightEncs.add(unwrap(e));
}
} else {
throw new AssertionError();
}
motors = new ArrayList<>();
motors.addAll(leftMotors);
motors.addAll(rightMotors);
forwardEncs = new ArrayList<>();
forwardEncs.addAll(leftEncs);
forwardEncs.addAll(rightEncs);
forwardEncs.addAll(parEncs);
List<RawEncoder> allEncs = new ArrayList<>();
allEncs.addAll(forwardEncs);
allEncs.addAll(perpEncs);
DcMotorController c1 = allEncs.get(0).getController();
for (Encoder e : allEncs) {
DcMotorController c2 = e.getController();
if (c1 != c2) {
throw new IllegalArgumentException("all encoders must be attached to the same hub");
}
}
}
public MotorFeedforward feedforward() {
if (md != null) {
return new MotorFeedforward(MecanumDrive.kS, MecanumDrive.kV, MecanumDrive.kA);
}
if (td != null) {
return new MotorFeedforward(TankDrive.kS, TankDrive.kV, TankDrive.kA);
}
throw new AssertionError();
}
public void setDrivePowers(Twist2d powers) {
if (md != null) {
md.setDrivePowers(powers);
}
if (td != null) {
td.setDrivePowers(powers);
}
throw new AssertionError();
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardPushTest.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.util.Encoder;
import java.util.List;
public final class ForwardPushTest extends LinearOpMode {
private static double avgPos(List<? extends Encoder> es) {
double avgPos = 0;
for (Encoder e : es) {
avgPos += e.getPositionAndVelocity().position;
}
return avgPos / es.size();
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
for (DcMotorEx m : view.motors) {
m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
}
waitForStart();
double initAvgPos = avgPos(view.forwardEncs);
while (opModeIsActive()) {
telemetry.addData("ticks traveled", avgPos(view.forwardEncs) - initAvgPos);
telemetry.update();
}
}
}

74
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java Normal file
View File

@ -0,0 +1,74 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import java.util.ArrayList;
import java.util.List;
public final class ForwardRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
class Data {
final String type = view.type;
final List<List<Double>> powerTimes = new ArrayList<>();
final List<List<Double>> powers = new ArrayList<>();
final List<Double> voltageTimes = new ArrayList<>();
final List<Double> voltages = new ArrayList<>();
final List<Double> encTimes = new ArrayList<>();
final List<List<Integer>> forwardEncPositions = new ArrayList<>();
final List<List<Integer>> forwardEncVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.motors) {
data.powerTimes.add(new ArrayList<>());
data.powers.add(new ArrayList<>());
}
for (Encoder e : view.forwardEncs) {
data.forwardEncPositions.add(new ArrayList<>());
data.forwardEncVels.add(new ArrayList<>());
}
waitForStart();
MidpointTimer t = new MidpointTimer();
while (opModeIsActive()) {
for (int i = 0; i < view.motors.size(); i++) {
double power = power(t.seconds());
view.motors.get(i).setPower(power);
data.powers.get(i).add(power);
data.powerTimes.get(i).add(t.addSplit());
}
data.voltages.add(view.voltageSensor.getVoltage());
data.voltageTimes.add(t.addSplit());
for (int i = 0; i < view.forwardEncs.size(); i++) {
Encoder.PositionVelocityPair p = view.forwardEncs.get(i).getPositionAndVelocity();
data.forwardEncPositions.get(i).add(p.position);
data.forwardEncVels.get(i).add(p.velocity);
}
data.encTimes.add(t.addSplit());
}
for (DcMotorEx m : view.motors) {
m.setPower(0);
}
TuningFiles.save(TuningFiles.FileType.FORWARD_RAMP, data);
}
}

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LateralPushTest.java Normal file
View File

@ -0,0 +1,41 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.MecanumDrive;
public final class LateralPushTest extends LinearOpMode {
private static double lateralSum(MecanumDrive.DriveLocalizer dl) {
return 0.25 * (
-dl.leftFront.getPositionAndVelocity().position
+dl.leftRear.getPositionAndVelocity().position
-dl.rightRear.getPositionAndVelocity().position
+dl.rightFront.getPositionAndVelocity().position);
}
@Override
public void runOpMode() throws InterruptedException {
if (!TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
throw new RuntimeException(getClass().getSimpleName() + " is for mecanum drives only.");
}
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) drive.localizer;
drive.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
drive.leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
drive.rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
drive.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
waitForStart();
double initLateralSum = lateralSum(dl);
while (opModeIsActive()) {
telemetry.addData("ticks traveled", lateralSum(dl) - initLateralSum);
telemetry.update();
}
}
}

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java Normal file
View File

@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Trajectory;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
public final class ManualFeedbackTuner extends LinearOpMode {
public static double DISTANCE = 64;
@Override
public void runOpMode() throws InterruptedException {
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
Trajectory forward = drive.trajectoryBuilder(drive.pose)
.forward(DISTANCE)
.build();
Trajectory backward = drive.trajectoryBuilder(forward, true)
.forward(-DISTANCE)
.build();
waitForStart();
while (opModeIsActive()) {
drive.followTrajectory(forward).runBlocking();
drive.followTrajectory(backward).runBlocking();
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
Trajectory forward = drive.trajectoryBuilder(drive.pose)
.forward(DISTANCE)
.build();
Trajectory backward = drive.trajectoryBuilder(forward, true)
.forward(-DISTANCE)
.build();
waitForStart();
while (opModeIsActive()) {
drive.followTrajectory(forward).runBlocking();
drive.followTrajectory(backward).runBlocking();
}
} else {
throw new AssertionError();
}
}
}

101
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java Normal file
View File

@ -0,0 +1,101 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Profiles;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeProfile;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@Config
public class ManualFeedforwardTuner extends LinearOpMode {
public static double DISTANCE = 64;
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
@Override
public void runOpMode() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
final DriveView view = new DriveView(hardwareMap);
final TimeProfile profile = new TimeProfile(Profiles.constantProfile(
DISTANCE, 0, view.maxVel, view.minAccel, view.maxAccel).baseProfile);
Mode mode = Mode.TUNING_MODE;
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
double startTs = System.nanoTime() / 1e9;
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE: {
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
}
for (int i = 0; i < view.forwardEncs.size(); i++) {
double v = view.forwardEncs.get(i).getPositionAndVelocity().velocity;
telemetry.addData("v" + i, view.inPerTick * v);
}
double ts = System.nanoTime() / 1e9;
double t = ts - startTs;
if (t > profile.duration) {
movingForwards = !movingForwards;
startTs = ts;
}
DualNum<Time> v = profile.get(t).drop(1);
if (!movingForwards) {
v = v.unaryMinus();
}
telemetry.addData("vref", v.get(0));
double power = view.feedforward().compute(v) / view.voltageSensor.getVoltage();
view.setDrivePowers(new Twist2d(new Vector2d(power, 0), 0));
break;
}
case DRIVER_MODE: {
if (gamepad1.b) {
mode = Mode.TUNING_MODE;
movingForwards = true;
startTs = System.nanoTime() / 1e9;
}
view.setDrivePowers(
new Twist2d(
new Vector2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x
),
-gamepad1.right_stick_x
)
);
break;
}
}
telemetry.update();
}
}
}

93
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/MecanumMotorDirectionDebugger.java Normal file
View File

@ -0,0 +1,93 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.MecanumDrive;
/**
* This is a simple teleop routine for debugging your motor configuration.
* Pressing each of the buttons will power its respective motor.
*
* Button Mappings:
*
* Xbox/PS4 Button - Motor
* X / ▢ - Front Left
* Y / Δ - Front Right
* B / O - Rear Right
* A / X - Rear Left
* The buttons are mapped to match the wheels spatially if you
* were to rotate the gamepad 45deg°. x/square is the front left
* ________ and each button corresponds to the wheel as you go clockwise
* / ______ \
* ------------.-' _ '-..+ Front of Bot
* / _ ( Y ) _ \ ^
* | ( X ) _ ( B ) | Front Left \ Front Right
* ___ '. ( A ) /| Wheel \ Wheel
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
* | | | \
* '.___.' '. | Rear Left \ Rear Right
* '. / Wheel \ Wheel
* \. .' (A/X) \ (B/O)
* \________/
*/
@Config
public class MecanumMotorDirectionDebugger extends LinearOpMode {
public static double MOTOR_POWER = 0.7;
@Override
public void runOpMode() throws InterruptedException {
if (!TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
throw new RuntimeException(getClass().getSimpleName() + " is for mecanum drives only.");
}
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Press play to begin the debugging op mode");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
while (opModeIsActive()) {
telemetry.addLine("Press each button to turn on its respective motor");
telemetry.addLine();
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
telemetry.addLine();
if (gamepad1.x) {
drive.leftFront.setPower(MOTOR_POWER);
telemetry.addLine("Running Motor: Front Left");
} else if (gamepad1.y) {
drive.rightFront.setPower(MOTOR_POWER);
telemetry.addLine("Running Motor: Front Right");
} else if (gamepad1.b) {
drive.rightBack.setPower(MOTOR_POWER);
telemetry.addLine("Running Motor: Rear Right");
} else if (gamepad1.a) {
drive.leftBack.setPower(MOTOR_POWER);
telemetry.addLine("Running Motor: Rear Left");
} else {
drive.leftFront.setPower(0);
drive.rightFront.setPower(0);
drive.rightBack.setPower(0);
drive.leftBack.setPower(0);
telemetry.addLine("Running Motor: None");
}
telemetry.update();
}
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java Normal file
View File

@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
public final class SplineTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
drive.followTrajectory(
drive.trajectoryBuilder(drive.pose)
.splineTo(new Vector2d(30, 30), Math.PI / 2)
.splineTo(new Vector2d(60, 0), Math.PI)
.build()
).runBlocking();
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
drive.followTrajectory(
drive.trajectoryBuilder(drive.pose)
.splineTo(new Vector2d(30, 30), Math.PI / 2)
.splineTo(new Vector2d(60, 0), Math.PI)
.build()
).runBlocking();
} else {
throw new AssertionError();
}
}
}

174
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningFiles.java Normal file
View File

@ -0,0 +1,174 @@
package org.firstinspires.ftc.teamcode.tuning;
import android.content.Context;
import android.content.res.AssetManager;
import com.fasterxml.jackson.core.JsonFactory;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.qualcomm.robotcore.util.RobotLog;
import com.qualcomm.robotcore.util.WebHandlerManager;
import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import org.firstinspires.ftc.robotcore.internal.webserver.WebHandler;
import org.firstinspires.ftc.robotserver.internal.webserver.MimeTypesUtil;
import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.util.Objects;
import fi.iki.elonen.NanoHTTPD;
public final class TuningFiles {
private static final File ROOT =
new File(AppUtil.ROOT_FOLDER + "/RoadRunner/tuning/");
private static final Object IO_LOCK = new Object();
private static WebHandlerManager whm; // guarded by ioLock
public enum FileType {
FORWARD_RAMP("forward-ramp"),
ANGULAR_RAMP("angular-ramp"),
ACCEL("accel");
public final String name;
FileType(String s) {
name = s;
}
}
private TuningFiles() {
}
private static File getFileTypeDir(FileType ty) {
return new File(ROOT, ty.name);
}
public static void save(FileType ty, Object data) {
synchronized (IO_LOCK) {
File f = new File(getFileTypeDir(ty), System.currentTimeMillis() + ".json");
try {
new ObjectMapper(new JsonFactory())
.writerWithDefaultPrettyPrinter()
.writeValue(f, data);
if (whm != null) {
whm.register("/tuning/" + ty.name + "/" + f.getName(),
newStaticFileHandler(f));
}
} catch (IOException e) {
RobotLog.setGlobalErrorMsg(new RuntimeException(e),
"Unable to write data to " + f.getAbsolutePath());
}
}
}
@WebHandlerRegistrar
public static void registerRoutes(Context context, WebHandlerManager manager) {
synchronized (IO_LOCK) {
AssetManager assetManager = context.getAssets();
registerAssetsUnderPath(manager, assetManager, "tuning");
for (FileType ty : FileType.values()) {
String base = "/tuning/" + ty.name;
WebHandler wh = newStaticAssetHandler(assetManager, ty.name + "/index.html");
manager.register(base, wh);
manager.register(base + "/", wh);
File dir = getFileTypeDir(ty);
//noinspection ResultOfMethodCallIgnored
dir.mkdirs();
manager.register(base + "/latest.json",
newLatestFileHandler(dir));
for (File f : Objects.requireNonNull(dir.listFiles())) {
manager.register(base + "/" + f.getName(),
newStaticFileHandler(f));
}
}
whm = manager;
}
}
private static WebHandler newStaticAssetHandler(AssetManager assetManager, String file) {
return session -> {
if (session.getMethod() == NanoHTTPD.Method.GET) {
String mimeType = MimeTypesUtil.determineMimeType(file);
return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
mimeType, assetManager.open(file));
} else {
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
NanoHTTPD.MIME_PLAINTEXT, "");
}
};
}
private static void registerAssetsUnderPath(WebHandlerManager webHandlerManager,
AssetManager assetManager, String path) {
try {
String[] list = assetManager.list(path);
if (list == null) return;
if (list.length > 0) {
for (String file : list) {
registerAssetsUnderPath(webHandlerManager, assetManager, path + "/" + file);
}
} else {
webHandlerManager.register(path, newStaticAssetHandler(assetManager, path));
}
} catch (IOException e) {
RobotLog.setGlobalErrorMsg(new RuntimeException(e),
"unable to register tuning web routes");
}
}
private static WebHandler newLatestFileHandler(File dir) {
return session -> {
File[] files = dir.listFiles();
if (files != null) {
long mostRecentLastModified = 0;
File mostRecentFile = null;
for (File f : files) {
long lastModified = f.lastModified();
if (lastModified > mostRecentLastModified) {
mostRecentLastModified = lastModified;
mostRecentFile = f;
}
}
if (mostRecentFile != null) {
String mimeType = MimeTypesUtil.determineMimeType(mostRecentFile.getName());
final NanoHTTPD.Response res = NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
mimeType,
new FileInputStream(mostRecentFile));
res.addHeader("X-Filename", mostRecentFile.getName());
return res;
}
}
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
NanoHTTPD.MIME_PLAINTEXT, "");
};
}
private static WebHandler newStaticFileHandler(File f) {
return session -> {
if (session.getMethod() == NanoHTTPD.Method.GET) {
String mimeType = MimeTypesUtil.determineMimeType(f.getName());
return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
mimeType, new FileInputStream(f));
} else {
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
NanoHTTPD.MIME_PLAINTEXT, "");
}
};
}
}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java Normal file
View File

@ -0,0 +1,47 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar;
import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import java.util.Arrays;
import java.util.List;
public final class TuningOpModes {
public static final Class<?> DRIVE_CLASS = MecanumDrive.class;
public static final String GROUP = "quickstart";
public static final boolean DISABLED = false;
private TuningOpModes() {
}
@OpModeRegistrar
public static void register(OpModeManager manager) {
if (DISABLED) return;
List<Class<? extends OpMode>> opModes = Arrays.asList(
AccelLogger.class,
AngularRampLogger.class,
ForwardPushTest.class,
ForwardRampLogger.class,
LateralPushTest.class,
ManualFeedbackTuner.class,
ManualFeedforwardTuner.class,
SplineTest.class,
MecanumMotorDirectionDebugger.class
);
for (Class<? extends OpMode> o : opModes) {
manager.register(new OpModeMeta.Builder()
.setName(o.getSimpleName())
.setGroup(GROUP)
.setFlavor(OpModeMeta.Flavor.TELEOP)
.build(), o);
}
}
}

148
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055Wrapper.java Normal file
View File

@ -0,0 +1,148 @@
package org.firstinspires.ftc.teamcode.util;
import com.acmerobotics.roadrunner.Rotation2d;
import com.qualcomm.hardware.bosch.BNO055IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
public final class BNO055Wrapper {
private final BNO055IMU bno;
public BNO055Wrapper(BNO055IMU bno) {
this.bno = bno;
}
public Rotation2d getHeading() {
return Rotation2d.exp(bno.getAngularOrientation()
.toAngleUnit(AngleUnit.RADIANS)
.toAxesOrder(AxesOrder.ZYX)
.firstAngle);
}
public double getHeadingVelocity() {
return getAngularVelocity().zRotationRate;
}
public AngularVelocity getAngularVelocity() {
return bno.getAngularVelocity().toAngleUnit(AngleUnit.RADIANS);
}
/**
* A direction for an axis to be remapped to
*/
public enum AxisDirection {
POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
}
/**
* IMU axes signs in the order XYZ (after remapping).
*/
public enum AxesSigns {
PPP(0b000),
PPN(0b001),
PNP(0b010),
PNN(0b011),
NPP(0b100),
NPN(0b101),
NNP(0b110),
NNN(0b111);
public final int bVal;
AxesSigns(int bVal) {
this.bVal = bVal;
}
}
/**
* Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
* Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
* mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
*
* Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
* Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
*
* NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if
* appropriate.
*
* @param order axes order
* @param signs axes signs
*/
public void swapThenFlipAxes(AxesOrder order, AxesSigns signs) {
try {
// the indices correspond with the 2-bit axis encodings specified in the datasheet
int[] indices = order.indices();
// AxesSign's values align with the datasheet
int axisMapSigns = signs.bVal;
if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
throw new RuntimeException("Same axis cannot be included in AxesOrder twice");
}
// ensure right-handed coordinate system
boolean isXSwapped = indices[0] != 0;
boolean isYSwapped = indices[1] != 1;
boolean isZSwapped = indices[2] != 2;
boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
&& (!isXSwapped || !isYSwapped || !isZSwapped);
boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
// != functions as xor
if (areTwoAxesSwapped != oddNumOfFlips) {
throw new RuntimeException("Coordinate system is left-handed");
}
// Bit: 7 6 | 5 4 | 3 2 | 1 0 |
// reserved | z axis | y axis | x axis |
int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
// Enter CONFIG mode
bno.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
Thread.sleep(100);
// Write the AXIS_MAP_CONFIG register
bno.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
// Write the AXIS_MAP_SIGN register
bno.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
// Switch back to the previous mode
bno.write8(BNO055IMU.Register.OPR_MODE, bno.getParameters().mode.bVal & 0x0F);
Thread.sleep(100);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
/**
* Remaps the IMU coordinate system so that the remapped +Z faces the provided
* {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
*
* @param direction axis direction
*/
public void remapZAxis(AxisDirection direction) {
switch (direction) {
case POS_X:
swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.NPP);
break;
case NEG_X:
swapThenFlipAxes(AxesOrder.ZYX, AxesSigns.PPN);
break;
case POS_Y:
swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PNP);
break;
case NEG_Y:
swapThenFlipAxes(AxesOrder.XZY, AxesSigns.PPN);
break;
case POS_Z:
swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PPP);
break;
case NEG_Z:
swapThenFlipAxes(AxesOrder.XYZ, AxesSigns.PNN);
break;
}
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java Normal file
View File

@ -0,0 +1,18 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.DcMotorController;
public interface Encoder {
class PositionVelocityPair {
public final int position, velocity;
public PositionVelocityPair(int position, int velocity) {
this.position = position;
this.velocity = velocity;
}
}
PositionVelocityPair getPositionAndVelocity();
DcMotorController getController();
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.util;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
public interface Localizer {
Twist2dIncrDual<Time> updateAndGetIncr();
}

108
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxFirmwareVersion.java Normal file
View File

@ -0,0 +1,108 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import java.util.HashMap;
import java.util.Map;
/**
* Parsed representation of a Lynx module firmware version.
*/
public final class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
/**
* Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
* @param hardwareMap hardware map containing Lynx modules
*/
public static void throwIfAnyModulesBelowVersion(HardwareMap hardwareMap, LynxFirmwareVersion minVersion) {
Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
LynxFirmwareVersion version = LynxFirmwareVersion.parse(module.getNullableFirmwareVersionString());
if (version == null || version.compareTo(minVersion) < 0) {
for (String name : hardwareMap.getNamesOf(module)) {
outdatedModules.put(name, version);
}
}
}
if (outdatedModules.size() > 0) {
StringBuilder msgBuilder = new StringBuilder();
msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
minVersion.toString()));
for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
msgBuilder.append(Misc.formatInvariant(
"\t%s: %s\n", entry.getKey(),
entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
}
throw new RuntimeException(msgBuilder.toString());
}
}
/**
* Parses Lynx module firmware version string into structured version object. Returns null for
* null argument or upon error.
*/
public static LynxFirmwareVersion parse(String s) {
if (s == null) {
return null;
}
String[] parts = s.split("[ :,]+");
try {
// note: for now, we ignore the hardware entry
return new LynxFirmwareVersion(
Integer.parseInt(parts[3]),
Integer.parseInt(parts[5]),
Integer.parseInt(parts[7])
);
} catch (NumberFormatException e) {
return null;
}
}
public final int major, minor, eng;
public LynxFirmwareVersion(int major, int minor, int eng) {
this.major = major;
this.minor = minor;
this.eng = eng;
}
@Override
public boolean equals(Object other) {
if (other instanceof LynxFirmwareVersion) {
LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
return major == otherVersion.major && minor == otherVersion.minor &&
eng == otherVersion.eng;
} else {
return false;
}
}
@Override
public int compareTo(LynxFirmwareVersion other) {
int majorComp = Integer.compare(major, other.major);
if (majorComp == 0) {
int minorComp = Integer.compare(minor, other.minor);
if (minorComp == 0) {
return Integer.compare(eng, other.eng);
} else {
return minorComp;
}
} else {
return majorComp;
}
}
@Override
public String toString() {
return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/MidpointTimer.java Normal file
View File

@ -0,0 +1,17 @@
package org.firstinspires.ftc.teamcode.util;
public final class MidpointTimer {
private final long beginTs = System.nanoTime();
private long lastTime;
public double seconds() {
return 1e-9 * (System.nanoTime() - beginTs);
}
public double addSplit() {
long time = System.nanoTime() - beginTs;
double midTimeSecs = 0.5e-9 * (lastTime + time);
lastTime = time;
return midTimeSecs;
}
}

57
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OverflowEncoder.java Normal file
View File

@ -0,0 +1,57 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.util.ElapsedTime;
public final class OverflowEncoder implements Encoder {
// encoder velocities are sent as 16-bit ints
// by the time they reach here, they are widened into an int and possibly negated
private static final int CPS_STEP = 0x10000;
private static int inverseOverflow(int input, double estimate) {
// convert to uint16
int real = input & 0xFFFF;
// initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
// because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
real += ((real % 20) / 4) * CPS_STEP;
// estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
return real;
}
public final RawEncoder encoder;
private int lastPosition;
private final ElapsedTime lastUpdate;
private final RollingThreeMedian velEstimate;
public OverflowEncoder(RawEncoder e) {
encoder = e;
lastPosition = e.getPositionAndVelocity().position;
lastUpdate = new ElapsedTime();
velEstimate = new RollingThreeMedian();
}
@Override
public PositionVelocityPair getPositionAndVelocity() {
PositionVelocityPair p = encoder.getPositionAndVelocity();
double dt = lastUpdate.seconds();
double v = velEstimate.update((p.position - lastPosition) / dt);
lastPosition = p.position;
lastUpdate.reset();
return new PositionVelocityPair(
p.position,
inverseOverflow(p.velocity, v)
);
}
@Override
public DcMotorController getController() {
return encoder.getController();
}
}

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java Normal file
View File

@ -0,0 +1,34 @@
package org.firstinspires.ftc.teamcode.util;
import com.acmerobotics.roadrunner.Curves;
import com.acmerobotics.roadrunner.PosePath;
import com.acmerobotics.roadrunner.Vector2d;
import java.util.List;
public final class Paths {
private Paths() {
}
// at least 6 inch resolution
public static double project(PosePath path, Vector2d query) {
List<Double> xs = com.acmerobotics.roadrunner.Math.range(
0.0, path.length(), (int) Math.ceil(path.length() / 6.0));
double champDisp = Curves.project(path, query, xs.get(0));
final Vector2d initVec = path.get(champDisp, 1).trans.value();
double champSqrNorm = initVec.minus(query).sqrNorm();
for (int i = 1; i < xs.size(); i++) {
double disp = Curves.project(path, query, xs.get(i));
Vector2d vec = path.get(disp, 1).trans.value();
double sqrNorm = vec.minus(query).sqrNorm();
if (sqrNorm < champSqrNorm) {
champDisp = disp;
champSqrNorm = sqrNorm;
}
}
return champDisp;
}
}

39
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RawEncoder.java Normal file
View File

@ -0,0 +1,39 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
public final class RawEncoder implements Encoder {
private final DcMotorEx m;
public DcMotorSimple.Direction direction = DcMotorSimple.Direction.FORWARD;
public RawEncoder(DcMotorEx m) {
this.m = m;
}
private int applyDirection(int x) {
if (m.getDirection() == DcMotorSimple.Direction.REVERSE) {
x = -x;
}
if (direction == DcMotorSimple.Direction.REVERSE) {
x = -x;
}
return x;
}
@Override
public PositionVelocityPair getPositionAndVelocity() {
return new PositionVelocityPair(
applyDirection(m.getCurrentPosition()),
applyDirection((int) m.getVelocity())
);
}
@Override
public DcMotorController getController() {
return m.getController();
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RollingThreeMedian.java Normal file
View File

@ -0,0 +1,16 @@
package org.firstinspires.ftc.teamcode.util;
public final class RollingThreeMedian {
private final double[] history = new double[3];
private int i;
public double update(double x) {
history[i] = x;
i = (i + 1) % 3;
return history[0] + history[1] + history[2]
- Math.min(history[0], Math.min(history[1], history[2]))
- Math.max(history[0], Math.max(history[1], history[2]));
}
}