KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

before merge

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-28 19:42:08 -06:00
parent 661730ef18
commit 66bb5c747f
2 changed files with 142 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java
View File

@@ -97,7 +97,7 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
public static double normalIntakeTime = 3.0;
public static double shoot1Turr = 0.57;
public static double shoot0XTolerance = 1.0;
public static double turretShootPos = 0.6;
public static double turretShootPos = 0.5;
public static double shootAllTime = 1.8;
public static double shoot0Time = 1.6;
public static double intake1Time = 3.0;
@@ -609,6 +609,8 @@ public class ProtoAutoClose_V4 extends LinearOpMode {
TrajectoryActionBuilder pickup3 = null;
TrajectoryActionBuilder shoot3 = null;
robot.limelight.start();
robot.light.setPosition(1);
while (opModeInInit()) {

275
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -5,9 +5,7 @@ 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.*;
import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController;
@@ -16,20 +14,12 @@ 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.Rotation2d;
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.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
@@ -56,131 +46,13 @@ import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.libs.RR.messages.MecanumLocalizerInputsMessage;
import org.firstinspires.ftc.teamcode.libs.RR.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 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;
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 MecanumDrive(HardwareMap hardwareMap, Pose2d 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)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
rightBack = hardwareMap.get(DcMotorEx.class, "br");
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
//
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.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 LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
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 PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
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
);
}
public static class Params {
// IMU orientation
// TODO: fill in these values based on
@@ -206,8 +78,8 @@ public final class MecanumDrive {
public double maxProfileAccel = 180;
// turn profile parameters (in radians)
public double maxAngVel = 4 * Math.PI; // shared with path
public double maxAngAccel = 4 * Math.PI;
public double maxAngVel = 4* Math.PI; // shared with path
public double maxAngAccel = 4* Math.PI;
// path controller gains
public double axialGain = 4;
@@ -219,6 +91,35 @@ public final class MecanumDrive {
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;
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;
@@ -243,13 +144,13 @@ public final class MecanumDrive {
}
@Override
public Pose2d getPose() {
return pose;
public void setPose(Pose2d pose) {
this.pose = pose;
}
@Override
public void setPose(Pose2d pose) {
this.pose = pose;
public Pose2d getPose() {
return pose;
}
@Override
@@ -315,11 +216,64 @@ public final class MecanumDrive {
}
}
public MecanumDrive(HardwareMap hardwareMap, Pose2d 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)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "fl");
leftBack = hardwareMap.get(DcMotorEx.class, "bl");
rightBack = hardwareMap.get(DcMotorEx.class, "br");
rightFront = hardwareMap.get(DcMotorEx.class, "fr");
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// TODO: reverse motor directions if needed
//
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.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 LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next();
localizer = new PinpointLocalizer(hardwareMap, PARAMS.inPerTick, pose);
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 final double[] xPoints, yPoints;
private double beginTs = -1;
private final double[] xPoints, yPoints;
public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t;
@@ -496,4 +450,51 @@ public final class MecanumDrive {
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
}
}
public PoseVelocity2d updatePoseEstimate() {
PoseVelocity2d vel = localizer.update();
poseHistory.add(localizer.getPose());
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}
estimatedPoseWriter.write(new PoseMessage(localizer.getPose()));
return vel;
}
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
);
}
}