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

Move quickstart files to separate module

Browse Source
This commit is contained in:
Ryan Brott
2023-08-19 19:29:18 -07:00
parent 0c14630d40
commit 9db94c089d
28 changed files with 144 additions and 1892 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/build.gradle
View File

@ -27,6 +27,7 @@ repositories {
maven {
url = 'https://maven.brott.dev/'
}
mavenLocal()
}
dependencies {
@ -38,4 +39,6 @@ dependencies {
implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta2'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation "com.acmerobotics.roadrunner:ftc:0.1.0-SNAPSHOT"
}

26
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java
View File

@ -1,26 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Action;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
public abstract class ActionOpMode extends LinearOpMode {
private final FtcDashboard dash = FtcDashboard.getInstance();
protected void runBlocking(Action a) {
Canvas c = new Canvas();
a.preview(c);
boolean b = true;
while (b && !isStopRequested()) {
TelemetryPacket p = new TelemetryPacket();
p.fieldOverlay().getOperations().addAll(c.getOperations());
b = a.run(p);
dash.sendTelemetryPacket(p);
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Localizer.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.util;
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;

110
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -23,6 +23,12 @@ import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LynxFirmware;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
@ -32,12 +38,6 @@ import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.util.LogFiles;
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.lang.Math;
import java.util.Arrays;
@ -46,49 +46,52 @@ 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 LATERAL_MULTIPLIER = IN_PER_TICK / LATERAL_IN_PER_TICK;
public static double TRACK_WIDTH_TICKS = 0;
public static class Params {
// drive model parameters
public double inPerTick = 0;
public double lateralInPerTick = 1;
public double trackWidthTicks = 0;
// feedforward parameters in tick units
public static double kS = 0;
public static double kV = 0;
public static double kA = 0;
// feedforward parameters in tick units
public double kS = 0;
public double kV = 0;
public 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;
// path profile parameters
public double maxWheelVel = 50;
public double minProfileAccel = -30;
public double maxProfileAccel = 50;
// turn profile parameters
public static double MAX_ANG_VEL = Math.PI; // shared with path
public static double MAX_ANG_ACCEL = Math.PI;
// turn profile parameters
public double maxAngVel = Math.PI; // shared with path
public double maxAngAccel = 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
// path controller gains
public double axialGain = 0.0;
public double lateralGain = 0.0;
public double headingGain = 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 double axialVelGain = 0.0;
public double lateralVelGain = 0.0;
public double headingVelGain = 0.0; // shared with turn
}
public static Params PARAMS = new Params();
public final MecanumKinematics kinematics = new MecanumKinematics(
IN_PER_TICK * TRACK_WIDTH_TICKS, LATERAL_MULTIPLIER);
PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK);
public final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL);
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
new AngularVelConstraint(MAX_ANG_VEL)
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
new AngularVelConstraint(PARAMS.maxAngVel)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(MIN_PROFILE_ACCEL, MAX_PROFILE_ACCEL);
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
@ -99,8 +102,6 @@ public final class MecanumDrive {
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 {
@ -125,10 +126,10 @@ public final class MecanumDrive {
@Override
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
@ -137,19 +138,19 @@ public final class MecanumDrive {
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
leftFrontPosVel.velocity,
}).times(inPerTick),
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftRearPosVel.position - lastLeftRearPos) + kinematics.trackWidth * headingDelta,
leftRearPosVel.velocity,
}).times(inPerTick),
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightRearPosVel.position - lastRightRearPos) - kinematics.trackWidth * headingDelta,
rightRearPosVel.velocity,
}).times(inPerTick),
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos) - kinematics.trackWidth * headingDelta,
rightFrontPosVel.velocity,
}).times(inPerTick)
}).times(PARAMS.inPerTick)
));
lastLeftFrontPos = leftFrontPosVel.position;
@ -169,8 +170,7 @@ public final class MecanumDrive {
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
new LynxFirmwareVersion(1, 8, 2));
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
@ -195,6 +195,8 @@ public final class MecanumDrive {
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new DriveLocalizer();
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
@ -257,8 +259,8 @@ public final class MecanumDrive {
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
@ -269,7 +271,7 @@ public final class MecanumDrive {
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
LogFiles.recordTargetPose(txWorldTarget.value());
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
p.put("x", pose.position.x);
p.put("y", pose.position.y);
@ -338,8 +340,8 @@ public final class MecanumDrive {
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
@ -350,7 +352,7 @@ public final class MecanumDrive {
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
LogFiles.recordTargetPose(txWorldTarget.value());
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
@ -383,7 +385,7 @@ public final class MecanumDrive {
poseHistory.removeFirst();
}
LogFiles.recordPose(pose);
FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
return twist.velocity().value();
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -25,11 +25,16 @@ import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LynxFirmware;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
@ -38,13 +43,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.teamcode.util.LogFiles;
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;
@ -79,6 +77,36 @@ public final class TankDrive {
public static double TURN_GAIN = 0.0;
public static double TURN_VEL_GAIN = 0.0;
public static class Params {
// drive model parameters
public double inPerTick = 0;
public double trackWidthTicks = 0;
// feedforward parameters in tick units
public double kS = 0;
public double kV = 0;
public double kA = 0;
// path profile parameters
public double maxWheelVel = 50;
public double minProfileAccel = -30;
public double maxProfileAccel = 50;
// turn profile parameters
public double maxAngVel = Math.PI; // shared with path
public double maxAngAccel = Math.PI;
// path controller gains
public double ramseteZeta = 0.7; // in the range (0, 1)
public double ramseteBBar = 2.0; // positive
// turn controller gains
public double turnGain = 0.0;
public double turnVelGain = 0.0;
}
public static Params PARAMS = new Params();
public final TankKinematics kinematics = new TankKinematics(IN_PER_TICK * TRACK_WIDTH_TICKS);
public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK);
@ -139,7 +167,7 @@ public final class TankDrive {
public Twist2dDual<Time> update() {
double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
PositionVelocityPair p = e.getPositionAndVelocity();
meanLeftPos += p.position;
meanLeftVel += p.velocity;
}
@ -148,7 +176,7 @@ public final class TankDrive {
double meanRightPos = 0.0, meanRightVel = 0.0;
for (Encoder e : rightEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
PositionVelocityPair p = e.getPositionAndVelocity();
meanRightPos += p.position;
meanRightVel += p.velocity;
}
@ -176,8 +204,7 @@ public final class TankDrive {
public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmwareVersion.throwIfAnyModulesBelowVersion(hardwareMap,
new LynxFirmwareVersion(1, 8, 2));
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
@ -202,6 +229,8 @@ public final class TankDrive {
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new TankDrive.DriveLocalizer();
FlightRecorder.write("TANK_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
@ -290,7 +319,7 @@ public final class TankDrive {
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
LogFiles.recordTargetPose(txWorldTarget.value());
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
@ -368,7 +397,7 @@ public final class TankDrive {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
}
LogFiles.recordTargetPose(txWorldTarget.value());
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
@ -401,7 +430,7 @@ public final class TankDrive {
poseHistory.removeFirst();
}
LogFiles.recordPose(pose);
FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
return twist.velocity().value();
}

38
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
View File

@ -5,18 +5,22 @@ import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
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 static class Params {
public double PAR0_Y_TICKS = 0.0;
public double PAR1_Y_TICKS = 0.0;
public double PERP_X_TICKS = 0.0;
}
public static Params PARAMS = new Params();
public final Encoder par0, par1, perp;
@ -34,12 +38,14 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
lastPerpPos = perp.getPositionAndVelocity().position;
this.inPerTick = inPerTick;
FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
}
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
int par0PosDelta = par0PosVel.position - lastPar0Pos;
int par1PosDelta = par1PosVel.position - lastPar1Pos;
@ -48,17 +54,17 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
Twist2dDual<Time> twist = new Twist2dDual<>(
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),
(PARAMS.PAR0_Y_TICKS * par1PosDelta - PARAMS.PAR1_Y_TICKS * par0PosDelta) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(PARAMS.PAR0_Y_TICKS * par1PosVel.velocity - PARAMS.PAR1_Y_TICKS * par0PosVel.velocity) / (PARAMS.PAR0_Y_TICKS - PARAMS.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),
(PARAMS.PERP_X_TICKS / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS) * (par1PosDelta - par0PosDelta) + perpPosDelta),
(PARAMS.PERP_X_TICKS / (PARAMS.PAR0_Y_TICKS - PARAMS.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),
(par0PosDelta - par1PosDelta) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
(par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.PAR0_Y_TICKS - PARAMS.PAR1_Y_TICKS),
})
);

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View File

@ -6,19 +6,24 @@ import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import com.acmerobotics.roadrunner.ftc.RawEncoder;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
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 static class Params {
public double PAR_Y_TICKS = 0.0;
public double PERP_X_TICKS = 0.0;
}
public static Params PARAMS = new Params();
public final Encoder par, perp;
public final IMU imu;
@ -38,11 +43,13 @@ public final class TwoDeadWheelLocalizer implements Localizer {
lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
this.inPerTick = inPerTick;
FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
}
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
PositionVelocityPair parPosVel = par.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
int parPosDelta = parPosVel.position - lastParPos;
@ -54,12 +61,12 @@ public final class TwoDeadWheelLocalizer implements Localizer {
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
parPosDelta - PAR_Y_TICKS * headingDelta,
parPosVel.velocity - PAR_Y_TICKS * headingVel,
parPosDelta - PARAMS.PAR_Y_TICKS * headingDelta,
parPosVel.velocity - PARAMS.PAR_Y_TICKS * headingVel,
}).times(inPerTick),
new DualNum<Time>(new double[] {
perpPosDelta - PERP_X_TICKS * headingDelta,
perpPosVel.velocity - PERP_X_TICKS * headingVel,
perpPosDelta - PARAMS.PERP_X_TICKS * headingDelta,
perpPosVel.velocity - PARAMS.PERP_X_TICKS * headingVel,
}).times(inPerTick)
),
new DualNum<>(new double[] {

131
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
View File

@ -1,131 +0,0 @@
## TeamCode Module
Welcome!
This module, TeamCode, is the place where you will write/paste the code for your team's
robot controller App. This module is currently empty (a clean slate) but the
process for adding OpModes is straightforward.
## Creating your own OpModes
The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
Sample opmodes exist in the FtcRobotController module.
To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
Expand the following tree elements:
FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
### Naming of Samples
To gain a better understanding of how the samples are organized, and how to interpret the
naming system, it will help to understand the conventions that were used during their creation.
These conventions are described (in detail) in the sample_conventions.md file in this folder.
To summarize: A range of different samples classes will reside in the java/external/samples.
The class names will follow a naming convention which indicates the purpose of each class.
The prefix of the name will be one of the following:
Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
of a particular style of OpMode. These are bare bones examples.
Sensor: This is a Sample OpMode that shows how to use a specific sensor.
It is not intended to drive a functioning robot, it is simply showing the minimal code
required to read and display the sensor values.
Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
It may be used to provide a common baseline driving OpMode, or
to demonstrate how a particular sensor or concept can be used to navigate.
Concept: This is a sample OpMode that illustrates performing a specific function or concept.
These may be complex, but their operation should be explained clearly in the comments,
or the comments should reference an external doc, guide or tutorial.
Each OpMode should try to only demonstrate a single concept so they are easy to
locate based on their name. These OpModes may not produce a drivable robot.
After the prefix, other conventions will apply:
* Sensor class names are constructed as: Sensor - Company - Type
* Robot class names are constructed as: Robot - Mode - Action - OpModetype
* Concept class names are constructed as: Concept - Topic - OpModetype
Once you are familiar with the range of samples available, you can choose one to be the
basis for your own robot. In all cases, the desired sample(s) needs to be copied into
your TeamCode module to be used.
This is done inside Android Studio directly, using the following steps:
1) Locate the desired sample class in the Project/Android tree.
2) Right click on the sample class and select "Copy"
3) Expand the TeamCode/java folder
4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
5) You will be prompted for a class name for the copy.
Choose something meaningful based on the purpose of this class.
Start with a capital letter, and remember that there may be more similar classes later.
Once your copy has been created, you should prepare it for use on your robot.
This is done by adjusting the OpMode's name, and enabling it to be displayed on the
Driver Station's OpMode list.
Each OpMode sample class begins with several lines of code like the ones shown below:
```
@TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
@Disabled
```
The name that will appear on the driver station's "opmode list" is defined by the code:
``name="Template: Linear OpMode"``
You can change what appears between the quotes to better describe your opmode.
The "group=" portion of the code can be used to help organize your list of OpModes.
As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
``@Disabled`` annotation which has been included.
This line can simply be deleted , or commented out, to make the OpMode visible.
## ADVANCED Multi-Team App management: Cloning the TeamCode Module
In some situations, you have multiple teams in your club and you want them to all share
a common code organization, with each being able to *see* the others code but each having
their own team module with their own code that they maintain themselves.
In this situation, you might wish to clone the TeamCode module, once for each of these teams.
Each of the clones would then appear along side each other in the Android Studio module list,
together with the FtcRobotController module (and the original TeamCode module).
Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
prior to clicking to the green Run arrow.
Warning: This is not for the inexperienced Software developer.
You will need to be comfortable with File manipulations and managing Android Studio Modules.
These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
Also.. Make a full project backup before you start this :)
To clone TeamCode, do the following:
Note: Some names start with "Team" and others start with "team". This is intentional.
1) Using your operating system file management tools, copy the whole "TeamCode"
folder to a sibling folder with a corresponding new name, eg: "Team0417".
2) In the new Team0417 folder, delete the TeamCode.iml file.
3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
to a matching name with a lowercase 'team' eg: "team0417".
4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
package="org.firstinspires.ftc.teamcode"
to be
package="org.firstinspires.ftc.team0417"
5) Add: include ':Team0417' to the "/settings.gradle" file.
6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""

167
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java
View File

@ -1,167 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.hardware.lynx.LynxDcMotorController;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.SerialNumber;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
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;
import java.util.Map;
public final class AngularRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
private static class Signal {
public final List<Double> times = new ArrayList<>();
public final List<Double> values = new ArrayList<>();
}
private static void recordEncoderData(Encoder e, Map<SerialNumber, Double> ts, Signal ps, Signal vs) {
SerialNumber sn = ((LynxDcMotorController) e.getController()).getSerialNumber();
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
ps.times.add(ts.get(sn));
ps.values.add((double) p.position);
vs.times.add(ts.get(sn));
vs.values.add((double) p.velocity);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
view.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
class Data {
public final String type = view.type;
public final List<Signal> leftPowers = new ArrayList<>();
public final List<Signal> rightPowers = new ArrayList<>();
public final Signal voltages = new Signal();
public final List<Signal> leftEncPositions = new ArrayList<>();
public final List<Signal> rightEncPositions = new ArrayList<>();
public final List<Signal> parEncPositions = new ArrayList<>();
public final List<Signal> perpEncPositions = new ArrayList<>();
public final List<Signal> leftEncVels = new ArrayList<>();
public final List<Signal> rightEncVels = new ArrayList<>();
public final List<Signal> parEncVels = new ArrayList<>();
public final List<Signal> perpEncVels = new ArrayList<>();
public final List<Signal> angVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.leftMotors) {
data.leftPowers.add(new Signal());
}
for (DcMotorEx m : view.rightMotors) {
data.rightPowers.add(new Signal());
}
for (Encoder e : view.leftEncs) {
data.leftEncPositions.add(new Signal());
data.leftEncVels.add(new Signal());
}
for (Encoder e : view.rightEncs) {
data.rightEncPositions.add(new Signal());
data.rightEncVels.add(new Signal());
}
for (Encoder e : view.parEncs) {
data.parEncPositions.add(new Signal());
data.parEncVels.add(new Signal());
}
for (Encoder e : view.perpEncs) {
data.perpEncPositions.add(new Signal());
data.perpEncVels.add(new Signal());
}
for (int i = 0; i < 3; i++) {
data.angVels.add(new Signal());
}
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);
Signal s = data.leftPowers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
for (int i = 0; i < view.rightMotors.size(); i++) {
double power = power(t.seconds());
view.rightMotors.get(i).setPower(power);
Signal s = data.rightPowers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
data.voltages.values.add(view.voltageSensor.getVoltage());
data.voltages.times.add(t.addSplit());
Map<SerialNumber, Double> encTimes = view.resetAndBulkRead(t);
for (int i = 0; i < view.leftEncs.size(); i++) {
recordEncoderData(
view.leftEncs.get(i),
encTimes,
data.leftEncPositions.get(i),
data.leftEncVels.get(i)
);
}
for (int i = 0; i < view.rightEncs.size(); i++) {
recordEncoderData(
view.rightEncs.get(i),
encTimes,
data.rightEncPositions.get(i),
data.rightEncVels.get(i)
);
}
for (int i = 0; i < view.parEncs.size(); i++) {
recordEncoderData(
view.parEncs.get(i),
encTimes,
data.parEncPositions.get(i),
data.parEncVels.get(i)
);
}
for (int i = 0; i < view.perpEncs.size(); i++) {
recordEncoderData(
view.perpEncs.get(i),
encTimes,
data.perpEncPositions.get(i),
data.perpEncVels.get(i)
);
}
AngularVelocity av = view.imu.getRobotAngularVelocity(AngleUnit.RADIANS);
for (int i = 0; i < 3; i ++) {
data.angVels.get(i).times.add(t.addSplit());
}
data.angVels.get(0).values.add((double) av.xRotationRate);
data.angVels.get(1).values.add((double) av.yRotationRate);
data.angVels.get(2).values.add((double) av.zRotationRate);
}
for (DcMotorEx m : view.motors) {
m.setPower(0);
}
TuningFiles.save(TuningFiles.FileType.ANGULAR_RAMP, data);
}
}

210
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View File

@ -1,210 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.SerialNumber;
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.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
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.HashMap;
import java.util.List;
import java.util.Map;
final class DriveView {
public final String type;
public final double inPerTick;
public final double maxVel, minAccel, maxAccel;
public final List<LynxModule> lynxModules;
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 List<Encoder> forwardEncsWrapped;
public final IMU 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) {
lynxModules = hardwareMap.getAll(LynxModule.class);
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();
}
forwardEncsWrapped = new ArrayList<>();
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();
forwardEncsWrapped.add(l2.par);
forwardEncsWrapped.add(l2.perp);
} 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();
forwardEncsWrapped.add(l3.par0);
forwardEncsWrapped.add(l3.par1);
forwardEncsWrapped.add(l3.perp);
} 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));
forwardEncsWrapped.add(dl.leftFront);
forwardEncsWrapped.add(dl.leftRear);
forwardEncsWrapped.add(dl.rightFront);
forwardEncsWrapped.add(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));
forwardEncsWrapped.add(e);
}
rightEncs = new ArrayList<>();
for (Encoder e : dl.rightEncs) {
rightEncs.add(unwrap(e));
forwardEncsWrapped.add(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);
}
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(PoseVelocity2d powers) {
if (md != null) {
md.setDrivePowers(powers);
return;
}
if (td != null) {
td.setDrivePowers(powers);
return;
}
throw new AssertionError();
}
public void setBulkCachingMode(LynxModule.BulkCachingMode mode) {
for (LynxModule m : lynxModules) {
m.setBulkCachingMode(mode);
}
}
public Map<SerialNumber, Double> resetAndBulkRead(MidpointTimer t) {
final Map<SerialNumber, Double> times = new HashMap<>();
for (LynxModule m : lynxModules) {
m.clearBulkCache();
t.addSplit();
m.getBulkData();
times.put(m.getSerialNumber(), t.addSplit());
}
return times;
}
}

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

@ -1,36 +0,0 @@
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();
}
}
}

96
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
View File

@ -1,96 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.qualcomm.hardware.lynx.LynxDcMotorController;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.SerialNumber;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
import java.util.ArrayList;
import java.util.List;
import java.util.Map;
public final class ForwardRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}
private static class Signal {
public final List<Double> times = new ArrayList<>();
public final List<Double> values = new ArrayList<>();
}
private static void recordEncoderData(Encoder e, Map<SerialNumber, Double> ts, Signal ps, Signal vs) {
SerialNumber sn = ((LynxDcMotorController) e.getController()).getSerialNumber();
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
ps.times.add(ts.get(sn));
ps.values.add((double) p.position);
vs.times.add(ts.get(sn));
vs.values.add((double) p.velocity);
}
@Override
public void runOpMode() throws InterruptedException {
DriveView view = new DriveView(hardwareMap);
view.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
class Data {
public final String type = view.type;
public final List<Signal> powers = new ArrayList<>();
public final Signal voltages = new Signal();
public final List<Signal> forwardEncPositions = new ArrayList<>();
public final List<Signal> forwardEncVels = new ArrayList<>();
}
Data data = new Data();
for (DcMotorEx m : view.motors) {
data.powers.add(new Signal());
}
for (Encoder e : view.forwardEncs) {
data.forwardEncPositions.add(new Signal());
data.forwardEncVels.add(new Signal());
}
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);
Signal s = data.powers.get(i);
s.times.add(t.addSplit());
s.values.add(power);
}
data.voltages.values.add(view.voltageSensor.getVoltage());
data.voltages.times.add(t.addSplit());
Map<SerialNumber, Double> encTimes = view.resetAndBulkRead(t);
for (int i = 0; i < view.forwardEncs.size(); i++) {
recordEncoderData(
view.forwardEncs.get(i),
encTimes,
data.forwardEncPositions.get(i),
data.forwardEncVels.get(i)
);
}
}
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
View File

@ -1,41 +0,0 @@
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();
}
}
}

61
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
View File

@ -1,61 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
public class LocalizationTest 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();
while (opModeIsActive()) {
drive.setDrivePowers(new PoseVelocity2d(
new Vector2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x
),
-gamepad1.right_stick_x
));
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.position.x);
telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading", drive.pose.heading);
telemetry.update();
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
while (opModeIsActive()) {
drive.setDrivePowers(new PoseVelocity2d(
new Vector2d(
-gamepad1.left_stick_y,
0.0
),
-gamepad1.right_stick_x
));
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.position.x);
telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading", drive.pose.heading);
telemetry.update();
}
} else {
throw new AssertionError();
}
}
}

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java
View File

@ -1,42 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import org.firstinspires.ftc.teamcode.ActionOpMode;
import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
public final class ManualFeedbackTuner extends ActionOpMode {
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));
waitForStart();
while (opModeIsActive()) {
runBlocking(
drive.actionBuilder(drive.pose)
.lineToX(DISTANCE)
.lineToX(0)
.build());
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
waitForStart();
while (opModeIsActive()) {
runBlocking(
drive.actionBuilder(drive.pose)
.lineToX(DISTANCE)
.lineToX(0)
.build());
}
} else {
throw new AssertionError();
}
}
}

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
View File

@ -1,104 +0,0 @@
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.PoseVelocity2d;
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;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@Config
public class ManualFeedforwardTuner extends LinearOpMode {
public static double DISTANCE = 64;
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
@Override
public void runOpMode() {
Telemetry telemetry = new MultipleTelemetry(this.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.forwardEncsWrapped.size(); i++) {
double v = view.forwardEncsWrapped.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 PoseVelocity2d(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 PoseVelocity2d(
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
View File

@ -1,93 +0,0 @@
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 telemetry = new MultipleTelemetry(this.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();
}
}
}

37
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/SplineTest.java
View File

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

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

@ -1,174 +0,0 @@
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
View File

@ -1,47 +0,0 @@
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(
AngularRampLogger.class,
ForwardPushTest.class,
ForwardRampLogger.class,
LateralPushTest.class,
ManualFeedbackTuner.class,
ManualFeedforwardTuner.class,
SplineTest.class,
MecanumMotorDirectionDebugger.class,
LocalizationTest.class
);
for (Class<? extends OpMode> o : opModes) {
manager.register(new OpModeMeta.Builder()
.setName(o.getSimpleName())
.setGroup(GROUP)
.setFlavor(OpModeMeta.Flavor.TELEOP)
.build(), o);
}
}
}

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
View File

@ -1,21 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
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();
void setDirection(DcMotorSimple.Direction direction);
}

227
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
View File

@ -1,227 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import android.annotation.SuppressLint;
import android.content.Context;
import com.acmerobotics.roadrunner.Pose2d;
import com.fasterxml.jackson.core.JsonFactory;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.ObjectWriter;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
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.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.tuning.TuningOpModes;
import java.io.File;
import java.io.FileInputStream;
import java.io.IOException;
import java.text.DateFormat;
import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Date;
import java.util.List;
import java.util.Objects;
import fi.iki.elonen.NanoHTTPD;
public final class LogFiles {
private static final File ROOT =
new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/");
public static LogFile log = new LogFile("uninitialized");
public static class LogFile {
public String version = "quickstart2 v0";
public String opModeName;
public long msInit = System.currentTimeMillis();
public long nsInit = System.nanoTime();
public long nsStart, nsStop;
public String driveClassName = TuningOpModes.DRIVE_CLASS.getCanonicalName();
public double mecInPerTick = MecanumDrive.IN_PER_TICK;
public double mecLateralInPerTick = MecanumDrive.LATERAL_IN_PER_TICK;
public double mecTrackWidthTicks = MecanumDrive.TRACK_WIDTH_TICKS;
public double mecKS = MecanumDrive.kS;
public double mecKV = MecanumDrive.kV;
public double mecKA = MecanumDrive.kA;
public double mecMaxWheelVel = MecanumDrive.MAX_WHEEL_VEL;
public double mecMinProfileAccel = MecanumDrive.MIN_PROFILE_ACCEL;
public double mecMaxProfileAccel = MecanumDrive.MAX_PROFILE_ACCEL;
public double mecMaxAngVel = MecanumDrive.MAX_ANG_VEL;
public double mecMaxAngAccel = MecanumDrive.MAX_ANG_ACCEL;
public double mecAxialGain = MecanumDrive.AXIAL_GAIN;
public double mecLateralGain = MecanumDrive.LATERAL_GAIN;
public double mecHeadingGain = MecanumDrive.HEADING_GAIN;
public double mecAxialVelGain = MecanumDrive.AXIAL_VEL_GAIN;
public double mecLateralVelGain = MecanumDrive.LATERAL_VEL_GAIN;
public double mecHeadingVelGain = MecanumDrive.HEADING_VEL_GAIN;
public double tankInPerTick = TankDrive.IN_PER_TICK;
public double tankTrackWidthTicks = TankDrive.TRACK_WIDTH_TICKS;
public double tankKS = TankDrive.kS;
public double tankKV = TankDrive.kV;
public double tankKA = TankDrive.kA;
public double tankMaxWheelVel = TankDrive.MAX_WHEEL_VEL;
public double tankMinProfileAccel = TankDrive.MIN_PROFILE_ACCEL;
public double tankMaxProfileAccel = TankDrive.MAX_PROFILE_ACCEL;
public double tankMaxAngVel = TankDrive.MAX_ANG_VEL;
public double tankMaxAngAccel = TankDrive.MAX_ANG_ACCEL;
public double tankRamseteZeta = TankDrive.RAMSETE_ZETA;
public double tankRamseteBbar = TankDrive.RAMSETE_BBAR;
public double tankTurnGain = TankDrive.TURN_GAIN;
public double tankTurnVelGain = TankDrive.TURN_VEL_GAIN;
public double threePar0YTicks = ThreeDeadWheelLocalizer.PAR0_Y_TICKS;
public double threePar1YTicks = ThreeDeadWheelLocalizer.PAR1_Y_TICKS;
public double threePerpXTicks = ThreeDeadWheelLocalizer.PERP_X_TICKS;
public double twoParYTicks = TwoDeadWheelLocalizer.PAR_Y_TICKS;
public double twoPerpXTicks = TwoDeadWheelLocalizer.PERP_X_TICKS;
public List<Long> nsTimes = new ArrayList<>();
public List<Double> targetXs = new ArrayList<>();
public List<Double> targetYs = new ArrayList<>();
public List<Double> targetHeadings = new ArrayList<>();
public List<Double> xs = new ArrayList<>();
public List<Double> ys = new ArrayList<>();
public List<Double> headings = new ArrayList<>();
public LogFile(String opModeName) {
this.opModeName = opModeName;
}
}
public static void recordTargetPose(Pose2d targetPose) {
log.targetXs.add(targetPose.position.x);
log.targetYs.add(targetPose.position.y);
log.targetHeadings.add(targetPose.heading.log());
}
public static void recordPose(Pose2d pose) {
// arbitrarily add time here
log.nsTimes.add(System.nanoTime());
log.xs.add(pose.position.x);
log.ys.add(pose.position.y);
log.headings.add(pose.heading.log());
}
private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
@SuppressLint("SimpleDateFormat")
final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS");
final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory())
.writerWithDefaultPrettyPrinter();
@Override
public void onOpModePreInit(OpMode opMode) {
log = new LogFile(opMode.getClass().getCanonicalName());
// clean up old files
File[] fs = Objects.requireNonNull(ROOT.listFiles());
Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified()));
long totalSizeBytes = 0;
for (File f : fs) {
totalSizeBytes += f.length();
}
int i = 0;
while (i < fs.length && totalSizeBytes >= 8 * 1000 * 1000) {
totalSizeBytes -= fs[i].length();
if (!fs[i].delete()) {
RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath());
}
++i;
}
}
@Override
public void onOpModePreStart(OpMode opMode) {
log.nsStart = System.nanoTime();
}
@Override
public void onOpModePostStop(OpMode opMode) {
log.nsStop = System.nanoTime();
if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) {
String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json";
File file = new File(ROOT, filename);
try {
jsonWriter.writeValue(file, log);
} catch (IOException e) {
RobotLog.setGlobalErrorMsg(new RuntimeException(e),
"Unable to write data to " + file.getAbsolutePath());
}
}
}
};
@WebHandlerRegistrar
public static void registerRoutes(Context context, WebHandlerManager manager) {
//noinspection ResultOfMethodCallIgnored
ROOT.mkdirs();
// op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves
// don't use @OnCreateEventLoop because it's unreliable
OpModeManagerImpl.getOpModeManagerOfActivity(
AppUtil.getInstance().getActivity()
).registerListener(notifHandler);
manager.register("/logs", session -> {
final StringBuilder sb = new StringBuilder();
sb.append("<!doctype html><html><head><title>Logs</title></head><body><ul>");
File[] fs = Objects.requireNonNull(ROOT.listFiles());
Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
for (File f : fs) {
sb.append("<li><a href=\"/logs/download?file=");
sb.append(f.getName());
sb.append("\" download=\"");
sb.append(f.getName());
sb.append("\">");
sb.append(f.getName());
sb.append("</a></li>");
}
sb.append("</ul></body></html>");
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
NanoHTTPD.MIME_HTML, sb.toString());
});
manager.register("/logs/download", session -> {
final String[] pairs = session.getQueryParameterString().split("&");
if (pairs.length != 1) {
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
}
final String[] parts = pairs[0].split("=");
if (!parts[0].equals("file")) {
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
}
File f = new File(ROOT, parts[1]);
if (!f.exists()) {
return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
}
return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
"application/json", new FileInputStream(f));
});
}
}

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

@ -1,108 +0,0 @@
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
View File

@ -1,17 +0,0 @@
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;
}
}

63
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OverflowEncoder.java
View File

@ -1,63 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
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();
}
@Override
public void setDirection(DcMotorSimple.Direction direction) {
encoder.setDirection(direction);
}
}

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

@ -1,34 +0,0 @@
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).position.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).position.value();
double sqrNorm = vec.minus(query).sqrNorm();
if (sqrNorm < champSqrNorm) {
champDisp = disp;
champSqrNorm = sqrNorm;
}
}
return champDisp;
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RawEncoder.java
View File

@ -1,44 +0,0 @@
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();
}
@Override
public void setDirection(DcMotorSimple.Direction direction) {
this.direction = direction;
}
}

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

@ -1,16 +0,0 @@
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]));
}
}