Skip to content

Commit

Permalink
acceleration blocks teleop dt
Browse files Browse the repository at this point in the history
  • Loading branch information
@Bi1ku
Bi1ku committed Nov 16, 2024
1 parent 52b2435 commit 0722516
Show file tree
Hide file tree
Showing 2 changed files with 149 additions and 141 deletions.
254 changes: 123 additions & 131 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;
Expand All @@ -52,6 +53,127 @@

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

public MecanumDrive(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)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");

leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

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 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 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
);
}

public static class Params {
// IMU orientation
// TODO: fill in these values based on
Expand Down Expand Up @@ -90,37 +212,6 @@ public static class Params {
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;
Expand Down Expand Up @@ -209,62 +300,10 @@ public Twist2dDual<Time> update() {
}
}

public MecanumDrive(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)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftBack = hardwareMap.get(DcMotorEx.class, "leftBack");
rightBack = hardwareMap.get(DcMotorEx.class, "rightBack");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");

leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

// TODO: reverse motor directions if needed
// leftFront.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 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;
private double beginTs = -1;

public FollowTrajectoryAction(TimeTrajectory t) {
timeTrajectory = t;
Expand Down Expand Up @@ -442,51 +481,4 @@ public void preview(Canvas c) {
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
);
}
}
36 changes: 26 additions & 10 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,36 +14,46 @@

@Config
public class Drivetrain extends SubsystemBase {
public static double MAX_ACCEL = 0.5;
public static double MAX_ANGULAR_ACCEL = 0.5;
public static double MAX_VEL = 1;
public static double MAX_ANGULAR_VEL = 1;
public static double MAX_ACCEL = 0.15;
public static double MAX_ANGULAR_ACCEL = 0.15;
public static double MAX_VEL = 0.75;
public static double MAX_ANGULAR_VEL = 0.75;

private final MecanumDrive drive;
private final MultipleTelemetry telemetry;

private double xPower = 0.0;
private double angPower = 0.0;
private double yPower = 0.0;

public Drivetrain(final HardwareMap hwMap, final MultipleTelemetry telemetry, Pose2d startPose) {
this.telemetry = telemetry;

this.drive = new MecanumDrive(hwMap, startPose);
}

private static double relativeMin(double min, double check) {
double rel = Math.min(min, Math.abs(check));

return check >= 0 ? rel : -rel;
}

public void startThread(final GamepadEx gamepad, CommandOpMode opMode) {
new Thread(() -> {
while (opMode.opModeIsActive())
try {
// TODO: Check if this works
synchronized (this.drive) {
double yPower = Math.max(-MAX_ACCEL, Math.min(MAX_ACCEL, -gamepad.getLeftY()));
double xPower = Math.max(-MAX_ACCEL, Math.min(MAX_ACCEL, gamepad.getLeftX()));
double angPower = Math.max(-MAX_ANGULAR_ACCEL, Math.min(MAX_ANGULAR_ACCEL, -gamepad.getRightX()));
this.yPower += Drivetrain.relativeMin(MAX_ACCEL, gamepad.getLeftY() - this.yPower);
this.xPower += Drivetrain.relativeMin(MAX_ACCEL, gamepad.getLeftX() - this.xPower);
this.angPower += Drivetrain.relativeMin(MAX_ANGULAR_ACCEL, gamepad.getRightX() - this.angPower);

this.drive.setDrivePowers(new PoseVelocity2d(
new Vector2d(
angPower * MAX_ANGULAR_VEL,
xPower * MAX_VEL
this.yPower * MAX_VEL,
this.xPower * MAX_VEL
),
yPower * MAX_VEL));
this.angPower * MAX_ANGULAR_VEL));
}
Thread.sleep(10);
} catch (InterruptedException e) {
Expand All @@ -52,6 +62,12 @@ public void startThread(final GamepadEx gamepad, CommandOpMode opMode) {
}).start();
}

@Override
public void periodic() {
this.telemetry.addData("xPower: ", this.xPower);
this.telemetry.addData("yPower: ", this.yPower);
}

public MecanumDrive getDrive() {
return drive;
}
Expand Down

0 comments on commit 0722516

Please sign in to comment.