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

Remove unused files for Pedro Pathing

Browse Source
This commit is contained in:
Carlos Rivas
2024-09-13 15:17:18 -07:00
parent ac1be6387b
commit 51c3e0bd93
20 changed files with 0 additions and 1937 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/Drawing.java
View File

@ -1,22 +0,0 @@
package org.firstinspires.ftc.classic;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Vector2d;
public final class Drawing {
private Drawing() {}
public static void drawRobot(Canvas c, Pose2d t) {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.position.plus(halfv);
Vector2d p2 = p1.plus(halfv);
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/Localizer.java
View File

@ -1,8 +0,0 @@
package org.firstinspires.ftc.classic;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
public interface Localizer {
Twist2dDual<Time> update();
}

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

@ -1,494 +0,0 @@
package org.firstinspires.ftc.classic;
import static org.firstinspires.ftc.classic.MecanumDriveParameters.*;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.*;
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.Time;
import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LazyImu;
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;
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 org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.classic.messages.DriveCommandMessage;
import org.firstinspires.ftc.classic.messages.MecanumCommandMessage;
import org.firstinspires.ftc.classic.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.classic.messages.PoseMessage;
import java.lang.Math;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
@Config
public final class MecanumDrive {
public static class Params {
/*
IMU Orientation
See https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
*/
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
// drive model parameters
public double inPerTick = INCHES_PER_TICK_VALUE;
public double lateralInPerTick = LATERAL_INCHES_PER_TICK_VALUE;
public double trackWidthTicks = TRACK_WIDTH_IN_TICKS_VALUE;
// feedforward parameters (in tick units)
public double kS = KS_VALUE;
public double kV = KV_VALUE;
public double kA = KA_VALUE;
// path profile parameters (in inches)
public double maxWheelVel = 50;
public double minProfileAccel = -30;
public double maxProfileAccel = 50;
// turn profile parameters (in radians)
public double maxAngVel = Math.PI; // shared with path
public double maxAngAccel = Math.PI;
// path controller gains
public double axialGain = 0.0;
public double lateralGain = 0.0;
public double headingGain = 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(
PARAMS.inPerTick * PARAMS.trackWidthTicks,
PARAMS.inPerTick / PARAMS.lateralInPerTick);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
new AngularVelConstraint(PARAMS.maxAngVel)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final DcMotorEx leftFront, leftBack, rightBack, rightFront;
public final VoltageSensor voltageSensor;
public final LazyImu lazyImu;
public final Localizer localizer;
public Pose2d pose;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);
public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront;
public final IMU imu;
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading;
private boolean initialized;
public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack));
rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
imu = lazyImu.get();
}
@Override
public Twist2dDual<Time> update() {
PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity();
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
if (!initialized) {
initialized = true;
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos),
leftFrontPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(leftBackPosVel.position - lastLeftBackPos),
leftBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightBackPosVel.position - lastRightBackPos),
rightBackPosVel.velocity,
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[]{
(rightFrontPosVel.position - lastRightFrontPos),
rightFrontPosVel.velocity,
}).times(PARAMS.inPerTick)
));
lastLeftFrontPos = leftFrontPosVel.position;
lastLeftBackPos = leftBackPosVel.position;
lastRightBackPos = rightBackPosVel.position;
lastRightFrontPos = rightFrontPosVel.position;
lastHeading = heading;
return new Twist2dDual<>(
twist.line,
DualNum.cons(headingDelta, twist.angle.drop(1))
);
}
}
public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
/*
Configure motors definitions
*/
leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
leftBack = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
rightBack = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
/*
Configure motors direction
*/
//leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
/*
IMU Orientation Application
See https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
*/
lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new DriveLocalizer();
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
maxPowerMag = Math.max(maxPowerMag, power.value());
}
leftFront.setPower(wheelVels.leftFront.get(0) / maxPowerMag);
leftBack.setPower(wheelVels.leftBack.get(0) / maxPowerMag);
rightBack.setPower(wheelVels.rightBack.get(0) / maxPowerMag);
rightFront.setPower(wheelVels.rightFront.get(0) / maxPowerMag);
}
public final class FollowTrajectoryAction implements Action {
public final TimeTrajectory timeTrajectory;
private double beginTs = -1;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t;
List<Double> disps = com.acmerobotics.roadrunner.Math.range(
0, t.path.length(),
Math.max(2, (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.position.x;
yPoints[i] = p.position.y;
}
}
@Override
public boolean run(@NonNull TelemetryPacket p) {
double t;
if (beginTs < 0) {
beginTs = Actions.now();
t = 0;
} else {
t = Actions.now() - beginTs;
}
if (t >= timeTrajectory.duration) {
leftFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);
rightFront.setPower(0);
return false;
}
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController(
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
driveCommandWriter.write(new DriveCommandMessage(command));
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
mecanumCommandWriter.write(new MecanumCommandMessage(
voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
));
leftFront.setPower(leftFrontPower);
leftBack.setPower(leftBackPower);
rightBack.setPower(rightBackPower);
rightFront.setPower(rightFrontPower);
p.put("x", pose.position.x);
p.put("y", pose.position.y);
p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.position.x);
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
Drawing.drawRobot(c, pose);
c.setStroke("#4CAF50FF");
c.setStrokeWidth(1);
c.strokePolyline(xPoints, yPoints);
return true;
}
@Override
public void preview(Canvas c) {
c.setStroke("#4CAF507A");
c.setStrokeWidth(1);
c.strokePolyline(xPoints, yPoints);
}
}
public final class TurnAction implements Action {
private final TimeTurn turn;
private double beginTs = -1;
public TurnAction(TimeTurn turn) {
this.turn = turn;
}
@Override
public boolean run(@NonNull TelemetryPacket p) {
double t;
if (beginTs < 0) {
beginTs = Actions.now();
t = 0;
} else {
t = Actions.now() - beginTs;
}
if (t >= turn.duration) {
leftFront.setPower(0);
leftBack.setPower(0);
rightBack.setPower(0);
rightFront.setPower(0);
return false;
}
Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new HolonomicController(
PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
driveCommandWriter.write(new DriveCommandMessage(command));
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
mecanumCommandWriter.write(new MecanumCommandMessage(
voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
));
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");
Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
Drawing.drawRobot(c, pose);
c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
return true;
}
@Override
public void preview(Canvas c) {
c.setStroke("#7C4DFF7A");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
}
}
public PoseVelocity2d updatePoseEstimate() {
Twist2dDual<Time> twist = localizer.update();
pose = pose.plus(twist.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(pose));
return twist.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.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
}

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/MecanumDriveParameters.java
View File

@ -1,20 +0,0 @@
package org.firstinspires.ftc.classic;
public class MecanumDriveParameters {
// Roadrunner tuning parameters
public static final double INCHES_PER_TICK_VALUE = 125.5/42126.8333333;
public static final double LATERAL_INCHES_PER_TICK_VALUE = 0.0026542073080477006;
public static final double TRACK_WIDTH_IN_TICKS_VALUE = 4012.7667474312616;
// FeedForward parameters (in tick units)
public static final double KS_VALUE = 0.7579547290234911;
public static final double KV_VALUE = 0.0005724562271687877;
public static final double KA_VALUE = 0;
// Robot motor configurations
public static final String FRONT_LEFT_MOTOR = "Drive front lt";
public static final String BACK_LEFT_MOTOR = "Drive back lt";
public static final String FRONT_RIGHT_MOTOR = "Drive front rt";
public static final String BACK_RIGHT_MOTOR = "Drive back rt";
}

144
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/SensorIMUOrthogonal.java
View File

@ -1,144 +0,0 @@
/* Copyright (c) 2022 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.classic;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
/*
* This OpMode shows how to use the new universal IMU interface. This
* interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
* on the robot with the name "imu".
*
* The sample will display the current Yaw, Pitch and Roll of the robot.<br>
* With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
* Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) <br>
* Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) <br>
* Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) <br>
*
* The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
*
* This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
* (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
*
* Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
* the alternative SensorImuNonOrthogonal sample in this folder.
*
* This "Orthogonal" requirement means that:
*
* 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* 2) The USB ports can only be pointing in one of the same six directions:<br>
* FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
*
* So, To fully define how your Hub is mounted to the robot, you must simply specify:<br>
* logoFacingDirection<br>
* usbFacingDirection
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*
* Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
* to use those parameters.
*/
@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
@Disabled // Comment this out to add to the OpMode list
public class SensorIMUOrthogonal extends LinearOpMode
{
// The IMU sensor object
IMU imu;
//----------------------------------------------------------------------------------------------
// Main logic
//----------------------------------------------------------------------------------------------
@Override public void runOpMode() throws InterruptedException {
// Retrieve and initialize the IMU.
// This sample expects the IMU to be in a REV Hub and named "imu".
imu = hardwareMap.get(IMU.class, "imu");
/* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
*
* Two input parameters are required to fully specify the Orientation.
* The first parameter specifies the direction the printed logo on the Hub is pointing.
* The second parameter specifies the direction the USB connector on the Hub is pointing.
* All directions are relative to the robot, and left/right is as-viewed from behind the robot.
*/
/* The next two lines define Hub orientation.
* The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
*
* To Do: EDIT these two lines to match YOUR mounting configuration.
*/
RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
// Now initialize the IMU with this mounting orientation
// Note: if you choose two conflicting directions, this initialization will cause a code exception.
imu.initialize(new IMU.Parameters(orientationOnRobot));
// Loop and update the dashboard
while (!isStopRequested()) {
telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
// Check to see if heading reset is requested
if (gamepad1.y) {
telemetry.addData("Yaw", "Resetting\n");
imu.resetYaw();
} else {
telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
}
// Retrieve Rotational Angles and Velocities
YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
telemetry.update();
}
}
}

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

@ -1,497 +0,0 @@
package org.firstinspires.ftc.classic;
import androidx.annotation.NonNull;
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.Action;
import com.acmerobotics.roadrunner.Actions;
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.PoseVelocity2d;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.ProfileParams;
import com.acmerobotics.roadrunner.RamseteController;
import com.acmerobotics.roadrunner.TankKinematics;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TrajectoryBuilderParams;
import com.acmerobotics.roadrunner.TurnConstraints;
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.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LazyImu;
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;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.classic.messages.DriveCommandMessage;
import org.firstinspires.ftc.classic.messages.PoseMessage;
import org.firstinspires.ftc.classic.messages.TankCommandMessage;
import org.firstinspires.ftc.classic.messages.TankLocalizerInputsMessage;
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 {
public static class Params {
// IMU orientation
// TODO: fill in these values based on
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
// 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 (in inches)
public double maxWheelVel = 50;
public double minProfileAccel = -30;
public double maxProfileAccel = 50;
// turn profile parameters (in radians)
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(PARAMS.inPerTick * PARAMS.trackWidthTicks);
public final TurnConstraints defaultTurnConstraints = new TurnConstraints(
PARAMS.maxAngVel, -PARAMS.maxAngVel, PARAMS.maxAngAccel);
public final VelConstraint defaultVelConstraint =
new MinVelConstraint(Arrays.asList(
kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
new AngularVelConstraint(PARAMS.maxAngVel)
));
public final AccelConstraint defaultAccelConstraint =
new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel);
public final List<DcMotorEx> leftMotors, rightMotors;
public final LazyImu lazyImu;
public final VoltageSensor voltageSensor;
public final Localizer localizer;
public Pose2d pose;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000);
public class DriveLocalizer implements Localizer {
public final List<Encoder> leftEncs, rightEncs;
private double lastLeftPos, lastRightPos;
private boolean initialized;
public DriveLocalizer() {
{
List<Encoder> leftEncs = new ArrayList<>();
for (DcMotorEx m : leftMotors) {
Encoder e = new OverflowEncoder(new RawEncoder(m));
leftEncs.add(e);
}
this.leftEncs = Collections.unmodifiableList(leftEncs);
}
{
List<Encoder> rightEncs = new ArrayList<>();
for (DcMotorEx m : rightMotors) {
Encoder e = new OverflowEncoder(new RawEncoder(m));
rightEncs.add(e);
}
this.rightEncs = Collections.unmodifiableList(rightEncs);
}
// TODO: reverse encoder directions if needed
// leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
}
@Override
public Twist2dDual<Time> update() {
List<PositionVelocityPair> leftReadings = new ArrayList<>(), rightReadings = new ArrayList<>();
double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) {
PositionVelocityPair p = e.getPositionAndVelocity();
meanLeftPos += p.position;
meanLeftVel += p.velocity;
leftReadings.add(p);
}
meanLeftPos /= leftEncs.size();
meanLeftVel /= leftEncs.size();
double meanRightPos = 0.0, meanRightVel = 0.0;
for (Encoder e : rightEncs) {
PositionVelocityPair p = e.getPositionAndVelocity();
meanRightPos += p.position;
meanRightVel += p.velocity;
rightReadings.add(p);
}
meanRightPos /= rightEncs.size();
meanRightVel /= rightEncs.size();
FlightRecorder.write("TANK_LOCALIZER_INPUTS",
new TankLocalizerInputsMessage(leftReadings, rightReadings));
if (!initialized) {
initialized = true;
lastLeftPos = meanLeftPos;
lastRightPos = meanRightPos;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
new DualNum<Time>(new double[] {
meanLeftPos - lastLeftPos,
meanLeftVel
}).times(PARAMS.inPerTick),
new DualNum<Time>(new double[] {
meanRightPos - lastRightPos,
meanRightVel,
}).times(PARAMS.inPerTick)
);
lastLeftPos = meanLeftPos;
lastRightPos = meanRightPos;
return kinematics.forward(twist);
}
}
public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
this.pose = pose;
LynxFirmware.throwIfModulesAreOutdated(hardwareMap);
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: make sure your config has motors with these names (or change them)
// add additional motors on each side if you have them
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
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);
}
// TODO: reverse motor directions if needed
// leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new TankDrive.DriveLocalizer();
FlightRecorder.write("TANK_PARAMS", PARAMS);
}
public void setDrivePowers(PoseVelocity2d powers) {
TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
maxPowerMag = Math.max(maxPowerMag, power.value());
}
for (DcMotorEx m : leftMotors) {
m.setPower(wheelVels.left.get(0) / maxPowerMag);
}
for (DcMotorEx m : rightMotors) {
m.setPower(wheelVels.right.get(0) / maxPowerMag);
}
}
public final class FollowTrajectoryAction implements Action {
public final TimeTrajectory timeTrajectory;
private double beginTs = -1;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t;
List<Double> disps = com.acmerobotics.roadrunner.Math.range(
0, t.path.length(),
Math.max(2, (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.position.x;
yPoints[i] = p.position.y;
}
}
@Override
public boolean run(@NonNull TelemetryPacket p) {
double t;
if (beginTs < 0) {
beginTs = Actions.now();
t = 0;
} else {
t = Actions.now() - beginTs;
}
if (t >= timeTrajectory.duration) {
for (DcMotorEx m : leftMotors) {
m.setPower(0);
}
for (DcMotorEx m : rightMotors) {
m.setPower(0);
}
return false;
}
DualNum<Time> x = timeTrajectory.profile.get(t);
Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
updatePoseEstimate();
PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
.compute(x, txWorldTarget, pose);
driveCommandWriter.write(new DriveCommandMessage(command));
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftPower = feedforward.compute(wheelVels.left) / voltage;
double rightPower = feedforward.compute(wheelVels.right) / voltage;
tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
for (DcMotorEx m : leftMotors) {
m.setPower(leftPower);
}
for (DcMotorEx m : rightMotors) {
m.setPower(rightPower);
}
p.put("x", pose.position.x);
p.put("y", pose.position.y);
p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.position.x);
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
Drawing.drawRobot(c, pose);
c.setStroke("#4CAF50FF");
c.setStrokeWidth(1);
c.strokePolyline(xPoints, yPoints);
return true;
}
@Override
public void preview(Canvas c) {
c.setStroke("#4CAF507A");
c.setStrokeWidth(1);
c.strokePolyline(xPoints, yPoints);
}
}
public final class TurnAction implements Action {
private final TimeTurn turn;
private double beginTs = -1;
public TurnAction(TimeTurn turn) {
this.turn = turn;
}
@Override
public boolean run(@NonNull TelemetryPacket p) {
double t;
if (beginTs < 0) {
beginTs = Actions.now();
t = 0;
} else {
t = Actions.now() - beginTs;
}
if (t >= turn.duration) {
for (DcMotorEx m : leftMotors) {
m.setPower(0);
}
for (DcMotorEx m : rightMotors) {
m.setPower(0);
}
return false;
}
Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
PoseVelocity2dDual<Time> command = new PoseVelocity2dDual<>(
Vector2dDual.constant(new Vector2d(0, 0), 3),
txWorldTarget.heading.velocity().plus(
PARAMS.turnGain * pose.heading.minus(txWorldTarget.heading.value()) +
PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
)
);
driveCommandWriter.write(new DriveCommandMessage(command));
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftPower = feedforward.compute(wheelVels.left) / voltage;
double rightPower = feedforward.compute(wheelVels.right) / voltage;
tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));
for (DcMotorEx m : leftMotors) {
m.setPower(leftPower);
}
for (DcMotorEx m : rightMotors) {
m.setPower(rightPower);
}
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
c.setStroke("#4CAF50");
Drawing.drawRobot(c, txWorldTarget.value());
c.setStroke("#3F51B5");
Drawing.drawRobot(c, pose);
c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
return true;
}
@Override
public void preview(Canvas c) {
c.setStroke("#7C4DFF7A");
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
}
}
public PoseVelocity2d updatePoseEstimate() {
Twist2dDual<Time> twist = localizer.update();
pose = pose.plus(twist.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(pose));
return twist.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.position.x;
yPoints[i] = t.position.y;
i++;
}
c.setStrokeWidth(1);
c.setStroke("#3F51B5");
c.strokePolyline(xPoints, yPoints);
}
public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
new TrajectoryBuilderParams(
1e-6,
new ProfileParams(
0.25, 0.1, 1e-2
)
),
beginPose, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint
);
}
}

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

@ -1,99 +0,0 @@
package org.firstinspires.ftc.classic;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
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.classic.messages.ThreeDeadWheelInputsMessage;
@Config
public final class ThreeDeadWheelLocalizer implements Localizer {
public static class Params {
public double par0YTicks = 0.0; // y position of the first parallel encoder (in tick units)
public double par1YTicks = 1.0; // y position of the second parallel encoder (in tick units)
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
}
public static Params PARAMS = new Params();
public final Encoder par0, par1, perp;
public final double inPerTick;
private int lastPar0Pos, lastPar1Pos, lastPerpPos;
private boolean initialized;
public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
// TODO: make sure your config has **motors** with these names (or change them)
// the encoders should be plugged into the slot matching the named motor
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "encoder left")));
par1 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "encoder right")));
perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "encoder back")));
// TODO: reverse encoder directions if needed
// par0.setDirection(DcMotorSimple.Direction.REVERSE);
this.inPerTick = inPerTick;
FlightRecorder.write("THREE_DEAD_WHEEL_PARAMS", PARAMS);
}
public Twist2dDual<Time> update() {
PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
FlightRecorder.write("THREE_DEAD_WHEEL_INPUTS", new ThreeDeadWheelInputsMessage(par0PosVel, par1PosVel, perpPosVel));
if (!initialized) {
initialized = true;
lastPar0Pos = par0PosVel.position;
lastPar1Pos = par1PosVel.position;
lastPerpPos = perpPosVel.position;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
int par0PosDelta = par0PosVel.position - lastPar0Pos;
int par1PosDelta = par1PosVel.position - lastPar1Pos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
(PARAMS.par0YTicks * par1PosDelta - PARAMS.par1YTicks * par0PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
(PARAMS.par0YTicks * par1PosVel.velocity - PARAMS.par1YTicks * par0PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
}).times(inPerTick),
new DualNum<Time>(new double[] {
(PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosDelta - par0PosDelta) + perpPosDelta),
(PARAMS.perpXTicks / (PARAMS.par0YTicks - PARAMS.par1YTicks) * (par1PosVel.velocity - par0PosVel.velocity) + perpPosVel.velocity),
}).times(inPerTick)
),
new DualNum<>(new double[] {
(par0PosDelta - par1PosDelta) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
(par0PosVel.velocity - par1PosVel.velocity) / (PARAMS.par0YTicks - PARAMS.par1YTicks),
})
);
lastPar0Pos = par0PosVel.position;
lastPar1Pos = par1PosVel.position;
lastPerpPos = perpPosVel.position;
return twist;
}
}

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

@ -1,120 +0,0 @@
package org.firstinspires.ftc.classic;
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.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.OverflowEncoder;
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.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import org.firstinspires.ftc.classic.messages.TwoDeadWheelInputsMessage;
@Config
public final class TwoDeadWheelLocalizer implements Localizer {
public static class Params {
public double parYTicks = 0.0; // y position of the parallel encoder (in tick units)
public double perpXTicks = 0.0; // x position of the perpendicular encoder (in tick units)
}
public static Params PARAMS = new Params();
public final Encoder par, perp;
public final IMU imu;
private int lastParPos, lastPerpPos;
private Rotation2d lastHeading;
private final double inPerTick;
private double lastRawHeadingVel, headingVelOffset;
private boolean initialized;
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
// TODO: make sure your config has **motors** with these names (or change them)
// the encoders should be plugged into the slot matching the named motor
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
// TODO: reverse encoder directions if needed
// par.setDirection(DcMotorSimple.Direction.REVERSE);
this.imu = imu;
this.inPerTick = inPerTick;
FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
}
public Twist2dDual<Time> update() {
PositionVelocityPair parPosVel = par.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.RADIANS);
FlightRecorder.write("TWO_DEAD_WHEEL_INPUTS", new TwoDeadWheelInputsMessage(parPosVel, perpPosVel, angles, angularVelocity));
Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS));
// see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
double rawHeadingVel = angularVelocity.zRotationRate;
if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
}
lastRawHeadingVel = rawHeadingVel;
double headingVel = headingVelOffset + rawHeadingVel;
if (!initialized) {
initialized = true;
lastParPos = parPosVel.position;
lastPerpPos = perpPosVel.position;
lastHeading = heading;
return new Twist2dDual<>(
Vector2dDual.constant(new Vector2d(0.0, 0.0), 2),
DualNum.constant(0.0, 2)
);
}
int parPosDelta = parPosVel.position - lastParPos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
double headingDelta = heading.minus(lastHeading);
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
parPosDelta - PARAMS.parYTicks * headingDelta,
parPosVel.velocity - PARAMS.parYTicks * headingVel,
}).times(inPerTick),
new DualNum<Time>(new double[] {
perpPosDelta - PARAMS.perpXTicks * headingDelta,
perpPosVel.velocity - PARAMS.perpXTicks * headingVel,
}).times(inPerTick)
),
new DualNum<>(new double[] {
headingDelta,
headingVel,
})
);
lastParPos = parPosVel.position;
lastPerpPos = perpPosVel.position;
lastHeading = heading;
return twist;
}
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/DriveCommandMessage.java
View File

@ -1,24 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.Time;
public final class DriveCommandMessage {
public long timestamp;
public double forwardVelocity;
public double forwardAcceleration;
public double lateralVelocity;
public double lateralAcceleration;
public double angularVelocity;
public double angularAcceleration;
public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) {
this.timestamp = System.nanoTime();
this.forwardVelocity = poseVelocity.linearVel.x.get(0);
this.forwardAcceleration = poseVelocity.linearVel.x.get(1);
this.lateralVelocity = poseVelocity.linearVel.y.get(0);
this.lateralAcceleration = poseVelocity.linearVel.y.get(1);
this.angularVelocity = poseVelocity.angVel.get(0);
this.angularAcceleration = poseVelocity.angVel.get(1);
}
}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/MecanumCommandMessage.java
View File

@ -1,19 +0,0 @@
package org.firstinspires.ftc.classic.messages;
public final class MecanumCommandMessage {
public long timestamp;
public double voltage;
public double leftFrontPower;
public double leftBackPower;
public double rightBackPower;
public double rightFrontPower;
public MecanumCommandMessage(double voltage, double leftFrontPower, double leftBackPower, double rightBackPower, double rightFrontPower) {
this.timestamp = System.nanoTime();
this.voltage = voltage;
this.leftFrontPower = leftFrontPower;
this.leftBackPower = leftBackPower;
this.rightBackPower = rightBackPower;
this.rightFrontPower = rightFrontPower;
}
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/MecanumLocalizerInputsMessage.java
View File

@ -1,30 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
public final class MecanumLocalizerInputsMessage {
public long timestamp;
public PositionVelocityPair leftFront;
public PositionVelocityPair leftBack;
public PositionVelocityPair rightBack;
public PositionVelocityPair rightFront;
public double yaw;
public double pitch;
public double roll;
public MecanumLocalizerInputsMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront, YawPitchRollAngles angles) {
this.timestamp = System.nanoTime();
this.leftFront = leftFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.rightFront = rightFront;
{
this.yaw = angles.getYaw(AngleUnit.RADIANS);
this.pitch = angles.getPitch(AngleUnit.RADIANS);
this.roll = angles.getRoll(AngleUnit.RADIANS);
}
}
}

18
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/PoseMessage.java
View File

@ -1,18 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.Pose2d;
public final class PoseMessage {
public long timestamp;
public double x;
public double y;
public double heading;
public PoseMessage(Pose2d pose) {
this.timestamp = System.nanoTime();
this.x = pose.position.x;
this.y = pose.position.y;
this.heading = pose.heading.toDouble();
}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/TankCommandMessage.java
View File

@ -1,15 +0,0 @@
package org.firstinspires.ftc.classic.messages;
public final class TankCommandMessage {
public long timestamp;
public double voltage;
public double leftPower;
public double rightPower;
public TankCommandMessage(double voltage, double leftPower, double rightPower) {
this.timestamp = System.nanoTime();
this.voltage = voltage;
this.leftPower = leftPower;
this.rightPower = rightPower;
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/TankLocalizerInputsMessage.java
View File

@ -1,17 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import java.util.List;
public final class TankLocalizerInputsMessage {
public long timestamp;
public PositionVelocityPair[] left;
public PositionVelocityPair[] right;
public TankLocalizerInputsMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
this.timestamp = System.nanoTime();
this.left = left.toArray(new PositionVelocityPair[0]);
this.right = right.toArray(new PositionVelocityPair[0]);
}
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/ThreeDeadWheelInputsMessage.java
View File

@ -1,17 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
public final class ThreeDeadWheelInputsMessage {
public long timestamp;
public PositionVelocityPair par0;
public PositionVelocityPair par1;
public PositionVelocityPair perp;
public ThreeDeadWheelInputsMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
this.timestamp = System.nanoTime();
this.par0 = par0;
this.par1 = par1;
this.perp = perp;
}
}

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/messages/TwoDeadWheelInputsMessage.java
View File

@ -1,35 +0,0 @@
package org.firstinspires.ftc.classic.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
public final class TwoDeadWheelInputsMessage {
public long timestamp;
public PositionVelocityPair par;
public PositionVelocityPair perp;
public double yaw;
public double pitch;
public double roll;
public double xRotationRate;
public double yRotationRate;
public double zRotationRate;
public TwoDeadWheelInputsMessage(PositionVelocityPair par, PositionVelocityPair perp, YawPitchRollAngles angles, AngularVelocity angularVelocity) {
this.timestamp = System.nanoTime();
this.par = par;
this.perp = perp;
{
this.yaw = angles.getYaw(AngleUnit.RADIANS);
this.pitch = angles.getPitch(AngleUnit.RADIANS);
this.roll = angles.getRoll(AngleUnit.RADIANS);
}
{
this.xRotationRate = angularVelocity.xRotationRate;
this.yRotationRate = angularVelocity.yRotationRate;
this.zRotationRate = angularVelocity.zRotationRate;
}
}
}

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

@ -1,76 +0,0 @@
package org.firstinspires.ftc.classic.tuning;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.classic.Drawing;
import org.firstinspires.ftc.classic.MecanumDrive;
import org.firstinspires.ftc.classic.TankDrive;
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
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 (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
telemetry.update();
TelemetryPacket packet = new TelemetryPacket();
packet.fieldOverlay().setStroke("#3F51B5");
Drawing.drawRobot(packet.fieldOverlay(), drive.pose);
FtcDashboard.getInstance().sendTelemetryPacket(packet);
}
} 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 (deg)", Math.toDegrees(drive.pose.heading.toDouble()));
telemetry.update();
TelemetryPacket packet = new TelemetryPacket();
packet.fieldOverlay().setStroke("#3F51B5");
Drawing.drawRobot(packet.fieldOverlay(), drive.pose);
FtcDashboard.getInstance().sendTelemetryPacket(packet);
}
} else {
throw new RuntimeException();
}
}
}

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

@ -1,63 +0,0 @@
package org.firstinspires.ftc.classic.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.classic.MecanumDrive;
import org.firstinspires.ftc.classic.TankDrive;
import org.firstinspires.ftc.classic.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.classic.TwoDeadWheelLocalizer;
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));
if (drive.localizer instanceof TwoDeadWheelLocalizer) {
if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
}
} else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
}
}
waitForStart();
while (opModeIsActive()) {
Actions.runBlocking(
drive.actionBuilder(new Pose2d(0, 0, 0))
.lineToX(DISTANCE)
.lineToX(0)
.build());
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
if (drive.localizer instanceof TwoDeadWheelLocalizer) {
if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) {
throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
}
} else if (drive.localizer instanceof ThreeDeadWheelLocalizer) {
if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) {
throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them.");
}
}
waitForStart();
while (opModeIsActive()) {
Actions.runBlocking(
drive.actionBuilder(new Pose2d(0, 0, 0))
.lineToX(DISTANCE)
.lineToX(0)
.build());
}
} else {
throw new RuntimeException();
}
}
}

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

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

180
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/classic/tuning/TuningOpModes.java
View File

@ -1,180 +0,0 @@
package org.firstinspires.ftc.classic.tuning;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.reflection.ReflectionConfig;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ftc.AngularRampLogger;
import com.acmerobotics.roadrunner.ftc.DeadWheelDirectionDebugger;
import com.acmerobotics.roadrunner.ftc.DriveType;
import com.acmerobotics.roadrunner.ftc.DriveView;
import com.acmerobotics.roadrunner.ftc.DriveViewFactory;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.ForwardPushTest;
import com.acmerobotics.roadrunner.ftc.ForwardRampLogger;
import com.acmerobotics.roadrunner.ftc.LateralPushTest;
import com.acmerobotics.roadrunner.ftc.LateralRampLogger;
import com.acmerobotics.roadrunner.ftc.ManualFeedforwardTuner;
import com.acmerobotics.roadrunner.ftc.MecanumMotorDirectionDebugger;
import com.qualcomm.hardware.lynx.LynxModule;
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.classic.MecanumDrive;
import org.firstinspires.ftc.classic.TankDrive;
import org.firstinspires.ftc.classic.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.classic.TwoDeadWheelLocalizer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
public final class TuningOpModes {
// TODO: change this to TankDrive.class if you're using tank
public static final Class<?> DRIVE_CLASS = MecanumDrive.class;
public static final String GROUP = "quickstart";
public static final boolean DISABLED = false;
private TuningOpModes() {}
private static OpModeMeta metaForClass(Class<? extends OpMode> cls) {
return new OpModeMeta.Builder()
.setName(cls.getSimpleName())
.setGroup(GROUP)
.setFlavor(OpModeMeta.Flavor.TELEOP)
.build();
}
@OpModeRegistrar
public static void register(OpModeManager manager) {
if (DISABLED) return;
DriveViewFactory dvf;
if (DRIVE_CLASS.equals(MecanumDrive.class)) {
dvf = hardwareMap -> {
MecanumDrive md = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
if (md.localizer instanceof MecanumDrive.DriveLocalizer) {
MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) md.localizer;
leftEncs.add(dl.leftFront);
leftEncs.add(dl.leftBack);
rightEncs.add(dl.rightFront);
rightEncs.add(dl.rightBack);
} else if (md.localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) md.localizer;
parEncs.add(dl.par0);
parEncs.add(dl.par1);
perpEncs.add(dl.perp);
} else if (md.localizer instanceof TwoDeadWheelLocalizer) {
TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) md.localizer;
parEncs.add(dl.par);
perpEncs.add(dl.perp);
} else {
throw new RuntimeException("unknown localizer: " + md.localizer.getClass().getName());
}
return new DriveView(
DriveType.MECANUM,
MecanumDrive.PARAMS.inPerTick,
MecanumDrive.PARAMS.maxWheelVel,
MecanumDrive.PARAMS.minProfileAccel,
MecanumDrive.PARAMS.maxProfileAccel,
hardwareMap.getAll(LynxModule.class),
Arrays.asList(
md.leftFront,
md.leftBack
),
Arrays.asList(
md.rightFront,
md.rightBack
),
leftEncs,
rightEncs,
parEncs,
perpEncs,
md.lazyImu,
md.voltageSensor,
() -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
MecanumDrive.PARAMS.kA / MecanumDrive.PARAMS.inPerTick)
);
};
} else if (DRIVE_CLASS.equals(TankDrive.class)) {
dvf = hardwareMap -> {
TankDrive td = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
List<Encoder> leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>();
List<Encoder> parEncs = new ArrayList<>(), perpEncs = new ArrayList<>();
if (td.localizer instanceof TankDrive.DriveLocalizer) {
TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) td.localizer;
leftEncs.addAll(dl.leftEncs);
rightEncs.addAll(dl.rightEncs);
} else if (td.localizer instanceof ThreeDeadWheelLocalizer) {
ThreeDeadWheelLocalizer dl = (ThreeDeadWheelLocalizer) td.localizer;
parEncs.add(dl.par0);
parEncs.add(dl.par1);
perpEncs.add(dl.perp);
} else if (td.localizer instanceof TwoDeadWheelLocalizer) {
TwoDeadWheelLocalizer dl = (TwoDeadWheelLocalizer) td.localizer;
parEncs.add(dl.par);
perpEncs.add(dl.perp);
} else {
throw new RuntimeException("unknown localizer: " + td.localizer.getClass().getName());
}
return new DriveView(
DriveType.TANK,
TankDrive.PARAMS.inPerTick,
TankDrive.PARAMS.maxWheelVel,
TankDrive.PARAMS.minProfileAccel,
TankDrive.PARAMS.maxProfileAccel,
hardwareMap.getAll(LynxModule.class),
td.leftMotors,
td.rightMotors,
leftEncs,
rightEncs,
parEncs,
perpEncs,
td.lazyImu,
td.voltageSensor,
() -> new MotorFeedforward(TankDrive.PARAMS.kS,
TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,
TankDrive.PARAMS.kA / TankDrive.PARAMS.inPerTick)
);
};
} else {
throw new RuntimeException();
}
manager.register(metaForClass(AngularRampLogger.class), new AngularRampLogger(dvf));
manager.register(metaForClass(ForwardPushTest.class), new ForwardPushTest(dvf));
manager.register(metaForClass(ForwardRampLogger.class), new ForwardRampLogger(dvf));
manager.register(metaForClass(LateralPushTest.class), new LateralPushTest(dvf));
manager.register(metaForClass(LateralRampLogger.class), new LateralRampLogger(dvf));
manager.register(metaForClass(ManualFeedforwardTuner.class), new ManualFeedforwardTuner(dvf));
manager.register(metaForClass(MecanumMotorDirectionDebugger.class), new MecanumMotorDirectionDebugger(dvf));
manager.register(metaForClass(DeadWheelDirectionDebugger.class), new DeadWheelDirectionDebugger(dvf));
manager.register(metaForClass(ManualFeedbackTuner.class), ManualFeedbackTuner.class);
manager.register(metaForClass(SplineTest.class), SplineTest.class);
manager.register(metaForClass(LocalizationTest.class), LocalizationTest.class);
FtcDashboard.getInstance().withConfigRoot(configRoot -> {
for (Class<?> c : Arrays.asList(
AngularRampLogger.class,
ForwardRampLogger.class,
LateralRampLogger.class,
ManualFeedforwardTuner.class,
MecanumMotorDirectionDebugger.class,
ManualFeedbackTuner.class
)) {
configRoot.putVariable(c.getSimpleName(), ReflectionConfig.createVariableFromClass(c));
}
});
}
}