Add Roadrunner
This commit is contained in:
Nick Haemel
311
TeamCode/drive/SampleMecanumDrive.java
Normal file
311
TeamCode/drive/SampleMecanumDrive.java
Normal file
@ -0,0 +1,311 @@
|
||||
package org.firstinspires.ftc.teamcode.drive;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.control.PIDCoefficients;
|
||||
import com.acmerobotics.roadrunner.drive.DriveSignal;
|
||||
import com.acmerobotics.roadrunner.drive.MecanumDrive;
|
||||
import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
|
||||
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
|
||||
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.PIDFCoefficients;
|
||||
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
|
||||
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
|
||||
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
|
||||
import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
|
||||
|
||||
/*
|
||||
* Simple mecanum drive hardware implementation for REV hardware.
|
||||
*/
|
||||
@Config
|
||||
public class SampleMecanumDrive extends MecanumDrive {
|
||||
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
|
||||
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
|
||||
|
||||
public static double LATERAL_MULTIPLIER = 1;
|
||||
|
||||
public static double VX_WEIGHT = 1;
|
||||
public static double VY_WEIGHT = 1;
|
||||
public static double OMEGA_WEIGHT = 1;
|
||||
|
||||
private TrajectorySequenceRunner trajectorySequenceRunner;
|
||||
|
||||
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
|
||||
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
|
||||
|
||||
private TrajectoryFollower follower;
|
||||
|
||||
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
|
||||
private List<DcMotorEx> motors;
|
||||
|
||||
private IMU imu;
|
||||
private VoltageSensor batteryVoltageSensor;
|
||||
|
||||
private List<Integer> lastEncPositions = new ArrayList<>();
|
||||
private List<Integer> lastEncVels = new ArrayList<>();
|
||||
|
||||
public SampleMecanumDrive(HardwareMap hardwareMap) {
|
||||
super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
|
||||
|
||||
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
|
||||
new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
|
||||
|
||||
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
|
||||
|
||||
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
|
||||
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
|
||||
}
|
||||
|
||||
// TODO: adjust the names of the following hardware devices to match your configuration
|
||||
imu = hardwareMap.get(IMU.class, "imu");
|
||||
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
|
||||
DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
|
||||
imu.initialize(parameters);
|
||||
|
||||
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
|
||||
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
|
||||
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
|
||||
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
|
||||
|
||||
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
|
||||
|
||||
for (DcMotorEx motor : motors) {
|
||||
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
|
||||
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
|
||||
motor.setMotorType(motorConfigurationType);
|
||||
}
|
||||
|
||||
if (RUN_USING_ENCODER) {
|
||||
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
}
|
||||
|
||||
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
|
||||
if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
|
||||
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||
}
|
||||
|
||||
// TODO: reverse any motors using DcMotor.setDirection()
|
||||
|
||||
List<Integer> lastTrackingEncPositions = new ArrayList<>();
|
||||
List<Integer> lastTrackingEncVels = new ArrayList<>();
|
||||
|
||||
// TODO: if desired, use setLocalizer() to change the localization method
|
||||
// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));
|
||||
|
||||
trajectorySequenceRunner = new TrajectorySequenceRunner(
|
||||
follower, HEADING_PID, batteryVoltageSensor,
|
||||
lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels
|
||||
);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
|
||||
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
|
||||
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
|
||||
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
|
||||
}
|
||||
|
||||
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
|
||||
return new TrajectorySequenceBuilder(
|
||||
startPose,
|
||||
VEL_CONSTRAINT, ACCEL_CONSTRAINT,
|
||||
MAX_ANG_VEL, MAX_ANG_ACCEL
|
||||
);
|
||||
}
|
||||
|
||||
public void turnAsync(double angle) {
|
||||
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||
trajectorySequenceBuilder(getPoseEstimate())
|
||||
.turn(angle)
|
||||
.build()
|
||||
);
|
||||
}
|
||||
|
||||
public void turn(double angle) {
|
||||
turnAsync(angle);
|
||||
waitForIdle();
|
||||
}
|
||||
|
||||
public void followTrajectoryAsync(Trajectory trajectory) {
|
||||
trajectorySequenceRunner.followTrajectorySequenceAsync(
|
||||
trajectorySequenceBuilder(trajectory.start())
|
||||
.addTrajectory(trajectory)
|
||||
.build()
|
||||
);
|
||||
}
|
||||
|
||||
public void followTrajectory(Trajectory trajectory) {
|
||||
followTrajectoryAsync(trajectory);
|
||||
waitForIdle();
|
||||
}
|
||||
|
||||
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
|
||||
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
|
||||
}
|
||||
|
||||
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
|
||||
followTrajectorySequenceAsync(trajectorySequence);
|
||||
waitForIdle();
|
||||
}
|
||||
|
||||
public Pose2d getLastError() {
|
||||
return trajectorySequenceRunner.getLastPoseError();
|
||||
}
|
||||
|
||||
public void update() {
|
||||
updatePoseEstimate();
|
||||
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
|
||||
if (signal != null) setDriveSignal(signal);
|
||||
}
|
||||
|
||||
public void waitForIdle() {
|
||||
while (!Thread.currentThread().isInterrupted() && isBusy())
|
||||
update();
|
||||
}
|
||||
|
||||
public boolean isBusy() {
|
||||
return trajectorySequenceRunner.isBusy();
|
||||
}
|
||||
|
||||
public void setMode(DcMotor.RunMode runMode) {
|
||||
for (DcMotorEx motor : motors) {
|
||||
motor.setMode(runMode);
|
||||
}
|
||||
}
|
||||
|
||||
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
|
||||
for (DcMotorEx motor : motors) {
|
||||
motor.setZeroPowerBehavior(zeroPowerBehavior);
|
||||
}
|
||||
}
|
||||
|
||||
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
|
||||
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
|
||||
coefficients.p, coefficients.i, coefficients.d,
|
||||
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
|
||||
);
|
||||
|
||||
for (DcMotorEx motor : motors) {
|
||||
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
|
||||
}
|
||||
}
|
||||
|
||||
public void setWeightedDrivePower(Pose2d drivePower) {
|
||||
Pose2d vel = drivePower;
|
||||
|
||||
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
|
||||
+ Math.abs(drivePower.getHeading()) > 1) {
|
||||
// re-normalize the powers according to the weights
|
||||
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
|
||||
+ VY_WEIGHT * Math.abs(drivePower.getY())
|
||||
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
|
||||
|
||||
vel = new Pose2d(
|
||||
VX_WEIGHT * drivePower.getX(),
|
||||
VY_WEIGHT * drivePower.getY(),
|
||||
OMEGA_WEIGHT * drivePower.getHeading()
|
||||
).div(denom);
|
||||
}
|
||||
|
||||
setDrivePower(vel);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelPositions() {
|
||||
lastEncPositions.clear();
|
||||
|
||||
List<Double> wheelPositions = new ArrayList<>();
|
||||
for (DcMotorEx motor : motors) {
|
||||
int position = motor.getCurrentPosition();
|
||||
lastEncPositions.add(position);
|
||||
wheelPositions.add(encoderTicksToInches(position));
|
||||
}
|
||||
return wheelPositions;
|
||||
}
|
||||
|
||||
@Override
|
||||
public List<Double> getWheelVelocities() {
|
||||
lastEncVels.clear();
|
||||
|
||||
List<Double> wheelVelocities = new ArrayList<>();
|
||||
for (DcMotorEx motor : motors) {
|
||||
int vel = (int) motor.getVelocity();
|
||||
lastEncVels.add(vel);
|
||||
wheelVelocities.add(encoderTicksToInches(vel));
|
||||
}
|
||||
return wheelVelocities;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setMotorPowers(double v, double v1, double v2, double v3) {
|
||||
leftFront.setPower(v);
|
||||
leftRear.setPower(v1);
|
||||
rightRear.setPower(v2);
|
||||
rightFront.setPower(v3);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double getRawExternalHeading() {
|
||||
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Double getExternalHeadingVelocity() {
|
||||
return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
|
||||
}
|
||||
|
||||
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
|
||||
return new MinVelocityConstraint(Arrays.asList(
|
||||
new AngularVelocityConstraint(maxAngularVel),
|
||||
new MecanumVelocityConstraint(maxVel, trackWidth)
|
||||
));
|
||||
}
|
||||
|
||||
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
|
||||
return new ProfileAccelerationConstraint(maxAccel);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user