Skip to content

Commit

Permalink
Move quickstart files to separate module
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbrott
rbrott committed Aug 20, 2023
1 parent 0c14630 commit 9db94c0
Show file tree
Hide file tree
Showing 28 changed files with 149 additions and 1,897 deletions.
3 changes: 3 additions & 0 deletions TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ repositories {
maven {
url = 'https://maven.brott.dev/'
}
mavenLocal()
}

dependencies {
Expand All @@ -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 changes: 0 additions & 26 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ActionOpMode.java
View file

This file was deleted.

2 changes: 1 addition & 1 deletion ...inspires/ftc/teamcode/util/Localizer.java → ...firstinspires/ftc/teamcode/Localizer.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand Down
120 changes: 61 additions & 59 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,12 @@
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;
Expand All @@ -32,12 +38,6 @@
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;
Expand All @@ -46,49 +46,52 @@

@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;

// feedforward parameters in tick units
public static double kS = 0;
public static double kV = 0;
public static double kA = 0;

// path profile parameters
public static double MAX_WHEEL_VEL = 50;
public static double MIN_PROFILE_ACCEL = -30;
public static double MAX_PROFILE_ACCEL = 50;

// turn profile parameters
public static double MAX_ANG_VEL = Math.PI; // shared with path
public static double MAX_ANG_ACCEL = Math.PI;

// path controller gains
public static double AXIAL_GAIN = 0.0;
public static double LATERAL_GAIN = 0.0;
public static double HEADING_GAIN = 0.0; // shared with turn

public static double AXIAL_VEL_GAIN = 0.0;
public static double LATERAL_VEL_GAIN = 0.0;
public static double HEADING_VEL_GAIN = 0.0; // shared with turn
public static class Params {
// drive model parameters
public double inPerTick = 0;
public double lateralInPerTick = 1;
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 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(
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;

Expand All @@ -99,8 +102,6 @@ kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),
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 {
Expand All @@ -125,10 +126,10 @@ public DriveLocalizer() {

@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);
Expand All @@ -137,19 +138,19 @@ public Twist2dDual<Time> update() {
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;
Expand All @@ -169,8 +170,7 @@ public Twist2dDual<Time> update() {
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);
Expand All @@ -195,6 +195,8 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
voltageSensor = hardwareMap.voltageSensor.iterator().next();

localizer = new DriveLocalizer();

FlightRecorder.write("MECANUM_PARAMS", PARAMS);
}

public void setDrivePowers(PoseVelocity2d powers) {
Expand Down Expand Up @@ -257,8 +259,8 @@ public boolean run(@NonNull TelemetryPacket p) {
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);

Expand All @@ -269,7 +271,7 @@ public boolean run(@NonNull TelemetryPacket p) {
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);
Expand Down Expand Up @@ -338,8 +340,8 @@ public boolean run(@NonNull TelemetryPacket p) {
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);

Expand All @@ -350,7 +352,7 @@ public boolean run(@NonNull TelemetryPacket p) {
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);
Expand Down Expand Up @@ -383,7 +385,7 @@ public PoseVelocity2d updatePoseEstimate() {
poseHistory.removeFirst();
}

LogFiles.recordPose(pose);
FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));

return twist.velocity().value();
}
Expand Down
Loading

0 comments on commit 9db94c0

Please sign in to comment.