Add Roadrunner
This commit is contained in:
Nick Haemel
94
TeamCode/drive/DriveConstants.java
Normal file
94
TeamCode/drive/DriveConstants.java
Normal file
@ -0,0 +1,94 @@
|
||||
package org.firstinspires.ftc.teamcode.drive;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
|
||||
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
|
||||
|
||||
/*
|
||||
* Constants shared between multiple drive types.
|
||||
*
|
||||
* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
|
||||
* fields may also be edited through the dashboard (connect to the robot's WiFi network and
|
||||
* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
|
||||
* adjust them in the dashboard; **config variable changes don't persist between app restarts**.
|
||||
*
|
||||
* These are not the only parameters; some are located in the localizer classes, drive base classes,
|
||||
* and op modes themselves.
|
||||
*/
|
||||
@Config
|
||||
public class DriveConstants {
|
||||
|
||||
/*
|
||||
* These are motor constants that should be listed online for your motors.
|
||||
*/
|
||||
public static final double TICKS_PER_REV = 1;
|
||||
public static final double MAX_RPM = 1;
|
||||
|
||||
/*
|
||||
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
|
||||
* Set this flag to false if drive encoders are not present and an alternative localization
|
||||
* method is in use (e.g., tracking wheels).
|
||||
*
|
||||
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
|
||||
* from DriveVelocityPIDTuner.
|
||||
*/
|
||||
public static final boolean RUN_USING_ENCODER = false;
|
||||
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
|
||||
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
|
||||
|
||||
/*
|
||||
* These are physical constants that can be determined from your robot (including the track
|
||||
* width; it will be tune empirically later although a rough estimate is important). Users are
|
||||
* free to chose whichever linear distance unit they would like so long as it is consistently
|
||||
* used. The default values were selected with inches in mind. Road runner uses radians for
|
||||
* angular distances although most angular parameters are wrapped in Math.toRadians() for
|
||||
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
|
||||
*/
|
||||
public static double WHEEL_RADIUS = 2; // in
|
||||
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
|
||||
public static double TRACK_WIDTH = 1; // in
|
||||
|
||||
/*
|
||||
* These are the feedforward parameters used to model the drive motor behavior. If you are using
|
||||
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
|
||||
* motor encoders or have elected not to use them for velocity control, these values should be
|
||||
* empirically tuned.
|
||||
*/
|
||||
public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
|
||||
public static double kA = 0;
|
||||
public static double kStatic = 0;
|
||||
|
||||
/*
|
||||
* These values are used to generate the trajectories for you robot. To ensure proper operation,
|
||||
* the constraints should never exceed ~80% of the robot's actual capabilities. While Road
|
||||
* Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
|
||||
* small and gradually increase them later after everything is working. All distance units are
|
||||
* inches.
|
||||
*/
|
||||
public static double MAX_VEL = 30;
|
||||
public static double MAX_ACCEL = 30;
|
||||
public static double MAX_ANG_VEL = Math.toRadians(60);
|
||||
public static double MAX_ANG_ACCEL = Math.toRadians(60);
|
||||
|
||||
/*
|
||||
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
|
||||
*/
|
||||
public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
|
||||
RevHubOrientationOnRobot.LogoFacingDirection.UP;
|
||||
public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
|
||||
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
|
||||
|
||||
|
||||
public static double encoderTicksToInches(double ticks) {
|
||||
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
||||
}
|
||||
|
||||
public static double rpmToVelocity(double rpm) {
|
||||
return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
|
||||
}
|
||||
|
||||
public static double getMotorVelocityF(double ticksPerSecond) {
|
||||
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
|
||||
return 32767 / ticksPerSecond;
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
305
TeamCode/drive/SampleTankDrive.java
Normal file
305
TeamCode/drive/SampleTankDrive.java
Normal file
@ -0,0 +1,305 @@
|
||||
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.TankDrive;
|
||||
import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
|
||||
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.MinVelocityConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
|
||||
import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
|
||||
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 tank drive hardware implementation for REV hardware.
|
||||
*/
|
||||
@Config
|
||||
public class SampleTankDrive extends TankDrive {
|
||||
public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
|
||||
public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
|
||||
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
|
||||
|
||||
public static double VX_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 accelConstraint = getAccelerationConstraint(MAX_ACCEL);
|
||||
|
||||
private TrajectoryFollower follower;
|
||||
|
||||
private List<DcMotorEx> motors, leftMotors, rightMotors;
|
||||
private IMU imu;
|
||||
|
||||
private VoltageSensor batteryVoltageSensor;
|
||||
|
||||
public SampleTankDrive(HardwareMap hardwareMap) {
|
||||
super(kV, kA, kStatic, TRACK_WIDTH);
|
||||
|
||||
follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_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);
|
||||
|
||||
// add/remove motors depending on your robot (e.g., 6WD)
|
||||
DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
|
||||
DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
|
||||
DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
|
||||
DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
|
||||
|
||||
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
|
||||
leftMotors = Arrays.asList(leftFront, leftRear);
|
||||
rightMotors = Arrays.asList(rightFront, rightRear);
|
||||
|
||||
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()
|
||||
|
||||
// TODO: if desired, use setLocalizer() to change the localization method
|
||||
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
|
||||
|
||||
trajectorySequenceRunner = new TrajectorySequenceRunner(
|
||||
follower, HEADING_PID, batteryVoltageSensor,
|
||||
new ArrayList<>(), new ArrayList<>(), new ArrayList<>(), new ArrayList<>()
|
||||
);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
|
||||
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
|
||||
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
|
||||
}
|
||||
|
||||
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
|
||||
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
|
||||
}
|
||||
|
||||
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
|
||||
return new TrajectorySequenceBuilder(
|
||||
startPose,
|
||||
VEL_CONSTRAINT, accelConstraint,
|
||||
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.getHeading()) > 1) {
|
||||
// re-normalize the powers according to the weights
|
||||
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
|
||||
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
|
||||
|
||||
vel = new Pose2d(
|
||||
VX_WEIGHT * drivePower.getX(),
|
||||
0,
|
||||
OMEGA_WEIGHT * drivePower.getHeading()
|
||||
).div(denom);
|
||||
} else {
|
||||
// Ensure the y axis is zeroed out.
|
||||
vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
|
||||
}
|
||||
|
||||
setDrivePower(vel);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelPositions() {
|
||||
double leftSum = 0, rightSum = 0;
|
||||
for (DcMotorEx leftMotor : leftMotors) {
|
||||
leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
|
||||
}
|
||||
for (DcMotorEx rightMotor : rightMotors) {
|
||||
rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
|
||||
}
|
||||
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
|
||||
}
|
||||
|
||||
public List<Double> getWheelVelocities() {
|
||||
double leftSum = 0, rightSum = 0;
|
||||
for (DcMotorEx leftMotor : leftMotors) {
|
||||
leftSum += encoderTicksToInches(leftMotor.getVelocity());
|
||||
}
|
||||
for (DcMotorEx rightMotor : rightMotors) {
|
||||
rightSum += encoderTicksToInches(rightMotor.getVelocity());
|
||||
}
|
||||
return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setMotorPowers(double v, double v1) {
|
||||
for (DcMotorEx leftMotor : leftMotors) {
|
||||
leftMotor.setPower(v);
|
||||
}
|
||||
for (DcMotorEx rightMotor : rightMotors) {
|
||||
rightMotor.setPower(v1);
|
||||
}
|
||||
}
|
||||
|
||||
@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 TankVelocityConstraint(maxVel, trackWidth)
|
||||
));
|
||||
}
|
||||
|
||||
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
|
||||
return new ProfileAccelerationConstraint(maxAccel);
|
||||
}
|
||||
}
|
||||
99
TeamCode/drive/StandardTrackingWheelLocalizer.java
Normal file
99
TeamCode/drive/StandardTrackingWheelLocalizer.java
Normal file
@ -0,0 +1,99 @@
|
||||
package org.firstinspires.ftc.teamcode.drive;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorEx;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
import org.firstinspires.ftc.teamcode.util.Encoder;
|
||||
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* Sample tracking wheel localizer implementation assuming the standard configuration:
|
||||
*
|
||||
* /--------------\
|
||||
* | ____ |
|
||||
* | ---- |
|
||||
* | || || |
|
||||
* | || || |
|
||||
* | |
|
||||
* | |
|
||||
* \--------------/
|
||||
*
|
||||
*/
|
||||
@Config
|
||||
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
|
||||
public static double TICKS_PER_REV = 0;
|
||||
public static double WHEEL_RADIUS = 2; // in
|
||||
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
|
||||
|
||||
public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels
|
||||
public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel
|
||||
|
||||
private Encoder leftEncoder, rightEncoder, frontEncoder;
|
||||
|
||||
private List<Integer> lastEncPositions, lastEncVels;
|
||||
|
||||
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List<Integer> lastTrackingEncPositions, List<Integer> lastTrackingEncVels) {
|
||||
super(Arrays.asList(
|
||||
new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
|
||||
new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
|
||||
new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
|
||||
));
|
||||
|
||||
lastEncPositions = lastTrackingEncPositions;
|
||||
lastEncVels = lastTrackingEncVels;
|
||||
|
||||
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder"));
|
||||
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder"));
|
||||
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder"));
|
||||
|
||||
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
|
||||
}
|
||||
|
||||
public static double encoderTicksToInches(double ticks) {
|
||||
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelPositions() {
|
||||
int leftPos = leftEncoder.getCurrentPosition();
|
||||
int rightPos = rightEncoder.getCurrentPosition();
|
||||
int frontPos = frontEncoder.getCurrentPosition();
|
||||
|
||||
lastEncPositions.clear();
|
||||
lastEncPositions.add(leftPos);
|
||||
lastEncPositions.add(rightPos);
|
||||
lastEncPositions.add(frontPos);
|
||||
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(leftPos),
|
||||
encoderTicksToInches(rightPos),
|
||||
encoderTicksToInches(frontPos)
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> getWheelVelocities() {
|
||||
int leftVel = (int) leftEncoder.getCorrectedVelocity();
|
||||
int rightVel = (int) rightEncoder.getCorrectedVelocity();
|
||||
int frontVel = (int) frontEncoder.getCorrectedVelocity();
|
||||
|
||||
lastEncVels.clear();
|
||||
lastEncVels.add(leftVel);
|
||||
lastEncVels.add(rightVel);
|
||||
lastEncVels.add(frontVel);
|
||||
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(leftVel),
|
||||
encoderTicksToInches(rightVel),
|
||||
encoderTicksToInches(frontVel)
|
||||
);
|
||||
}
|
||||
}
|
||||
221
TeamCode/drive/opmode/AutomaticFeedforwardTuner.java
Normal file
221
TeamCode/drive/opmode/AutomaticFeedforwardTuner.java
Normal file
@ -0,0 +1,221 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.util.RobotLog;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.util.LoggingUtil;
|
||||
import org.firstinspires.ftc.teamcode.util.RegressionUtil;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
|
||||
* outline of the procedure:
|
||||
* 1. Slowly ramp the motor power and record encoder values along the way.
|
||||
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
|
||||
* and an optional intercept (kStatic).
|
||||
* 3. Accelerate the robot (apply constant power) and record the encoder counts.
|
||||
* 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
|
||||
* regression.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class AutomaticFeedforwardTuner extends LinearOpMode {
|
||||
public static double MAX_POWER = 0.7;
|
||||
public static double DISTANCE = 100; // in
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
if (RUN_USING_ENCODER) {
|
||||
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
|
||||
"when using the built-in drive motor velocity PID.");
|
||||
}
|
||||
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
NanoClock clock = NanoClock.system();
|
||||
|
||||
telemetry.addLine("Press play to begin the feedforward tuning routine");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Would you like to fit kStatic?");
|
||||
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
|
||||
telemetry.update();
|
||||
|
||||
boolean fitIntercept = false;
|
||||
while (!isStopRequested()) {
|
||||
if (gamepad1.y) {
|
||||
fitIntercept = true;
|
||||
while (!isStopRequested() && gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
break;
|
||||
} else if (gamepad1.b) {
|
||||
while (!isStopRequested() && gamepad1.b) {
|
||||
idle();
|
||||
}
|
||||
break;
|
||||
}
|
||||
idle();
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine(Misc.formatInvariant(
|
||||
"Place your robot on the field with at least %.2f in of room in front", DISTANCE));
|
||||
telemetry.addLine("Press (Y/Δ) to begin");
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested() && !gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
while (!isStopRequested() && gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Running...");
|
||||
telemetry.update();
|
||||
|
||||
double maxVel = rpmToVelocity(MAX_RPM);
|
||||
double finalVel = MAX_POWER * maxVel;
|
||||
double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
|
||||
double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
|
||||
|
||||
List<Double> timeSamples = new ArrayList<>();
|
||||
List<Double> positionSamples = new ArrayList<>();
|
||||
List<Double> powerSamples = new ArrayList<>();
|
||||
|
||||
drive.setPoseEstimate(new Pose2d());
|
||||
|
||||
double startTime = clock.seconds();
|
||||
while (!isStopRequested()) {
|
||||
double elapsedTime = clock.seconds() - startTime;
|
||||
if (elapsedTime > rampTime) {
|
||||
break;
|
||||
}
|
||||
double vel = accel * elapsedTime;
|
||||
double power = vel / maxVel;
|
||||
|
||||
timeSamples.add(elapsedTime);
|
||||
positionSamples.add(drive.getPoseEstimate().getX());
|
||||
powerSamples.add(power);
|
||||
|
||||
drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
|
||||
drive.updatePoseEstimate();
|
||||
}
|
||||
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
|
||||
|
||||
RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
|
||||
timeSamples, positionSamples, powerSamples, fitIntercept,
|
||||
LoggingUtil.getLogFile(Misc.formatInvariant(
|
||||
"DriveRampRegression-%d.csv", System.currentTimeMillis())));
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Quasi-static ramp up test complete");
|
||||
if (fitIntercept) {
|
||||
telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
|
||||
rampResult.kV, rampResult.kStatic, rampResult.rSquare));
|
||||
} else {
|
||||
telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
|
||||
rampResult.kStatic, rampResult.rSquare));
|
||||
}
|
||||
telemetry.addLine("Would you like to fit kA?");
|
||||
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
|
||||
telemetry.update();
|
||||
|
||||
boolean fitAccelFF = false;
|
||||
while (!isStopRequested()) {
|
||||
if (gamepad1.y) {
|
||||
fitAccelFF = true;
|
||||
while (!isStopRequested() && gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
break;
|
||||
} else if (gamepad1.b) {
|
||||
while (!isStopRequested() && gamepad1.b) {
|
||||
idle();
|
||||
}
|
||||
break;
|
||||
}
|
||||
idle();
|
||||
}
|
||||
|
||||
if (fitAccelFF) {
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Place the robot back in its starting position");
|
||||
telemetry.addLine("Press (Y/Δ) to continue");
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested() && !gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
while (!isStopRequested() && gamepad1.y) {
|
||||
idle();
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Running...");
|
||||
telemetry.update();
|
||||
|
||||
double maxPowerTime = DISTANCE / maxVel;
|
||||
|
||||
timeSamples.clear();
|
||||
positionSamples.clear();
|
||||
powerSamples.clear();
|
||||
|
||||
drive.setPoseEstimate(new Pose2d());
|
||||
drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
|
||||
|
||||
startTime = clock.seconds();
|
||||
while (!isStopRequested()) {
|
||||
double elapsedTime = clock.seconds() - startTime;
|
||||
if (elapsedTime > maxPowerTime) {
|
||||
break;
|
||||
}
|
||||
|
||||
timeSamples.add(elapsedTime);
|
||||
positionSamples.add(drive.getPoseEstimate().getX());
|
||||
powerSamples.add(MAX_POWER);
|
||||
|
||||
drive.updatePoseEstimate();
|
||||
}
|
||||
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
|
||||
|
||||
RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
|
||||
timeSamples, positionSamples, powerSamples, rampResult,
|
||||
LoggingUtil.getLogFile(Misc.formatInvariant(
|
||||
"DriveAccelRegression-%d.csv", System.currentTimeMillis())));
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Constant power test complete");
|
||||
telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
|
||||
accelResult.kA, accelResult.rSquare));
|
||||
telemetry.update();
|
||||
}
|
||||
|
||||
while (!isStopRequested()) {
|
||||
idle();
|
||||
}
|
||||
}
|
||||
}
|
||||
52
TeamCode/drive/opmode/BackAndForth.java
Normal file
52
TeamCode/drive/opmode/BackAndForth.java
Normal file
@ -0,0 +1,52 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
|
||||
* classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
|
||||
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
|
||||
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
|
||||
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
|
||||
* you've successfully connected, start the program, and your robot will begin moving forward and
|
||||
* backward. You should observe the target position (green) and your pose estimate (blue) and adjust
|
||||
* your follower PID coefficients such that you follow the target position as accurately as possible.
|
||||
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
|
||||
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
|
||||
* These coefficients can be tuned live in dashboard.
|
||||
*
|
||||
* This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
|
||||
* is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class BackAndForth extends LinearOpMode {
|
||||
|
||||
public static double DISTANCE = 50;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
|
||||
.forward(DISTANCE)
|
||||
.build();
|
||||
|
||||
Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
|
||||
.back(DISTANCE)
|
||||
.build();
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (opModeIsActive() && !isStopRequested()) {
|
||||
drive.followTrajectory(trajectoryForward);
|
||||
drive.followTrajectory(trajectoryBackward);
|
||||
}
|
||||
}
|
||||
}
|
||||
171
TeamCode/drive/opmode/DriveVelocityPIDTuner.java
Normal file
171
TeamCode/drive/opmode/DriveVelocityPIDTuner.java
Normal file
@ -0,0 +1,171 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||
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.kV;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
|
||||
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.util.RobotLog;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
/*
|
||||
* This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
|
||||
* loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
|
||||
* important as the positional parameters. Like the other manual tuning routines, this op mode
|
||||
* relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
|
||||
* WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
|
||||
* phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
|
||||
* connected, start the program, and your robot will begin moving forward and backward according to
|
||||
* a motion profile. Your job is to graph the velocity errors over time and adjust the PID
|
||||
* coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
|
||||
* Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
|
||||
* MOTOR_VELO_PID field.
|
||||
*
|
||||
* Recommended tuning process:
|
||||
*
|
||||
* 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
|
||||
* mitigate oscillations.
|
||||
* 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
|
||||
* 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
|
||||
*
|
||||
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
|
||||
* user to reset the position of the bot in the event that it drifts off the path.
|
||||
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class DriveVelocityPIDTuner extends LinearOpMode {
|
||||
public static double DISTANCE = 72; // in
|
||||
|
||||
enum Mode {
|
||||
DRIVER_MODE,
|
||||
TUNING_MODE
|
||||
}
|
||||
|
||||
private static MotionProfile generateProfile(boolean movingForward) {
|
||||
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
|
||||
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
|
||||
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
if (!RUN_USING_ENCODER) {
|
||||
RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
|
||||
"PID is not in use", getClass().getSimpleName());
|
||||
}
|
||||
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
Mode mode = Mode.TUNING_MODE;
|
||||
|
||||
double lastKp = MOTOR_VELO_PID.p;
|
||||
double lastKi = MOTOR_VELO_PID.i;
|
||||
double lastKd = MOTOR_VELO_PID.d;
|
||||
double lastKf = MOTOR_VELO_PID.f;
|
||||
|
||||
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||
|
||||
NanoClock clock = NanoClock.system();
|
||||
|
||||
telemetry.addLine("Ready!");
|
||||
telemetry.update();
|
||||
telemetry.clearAll();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
boolean movingForwards = true;
|
||||
MotionProfile activeProfile = generateProfile(true);
|
||||
double profileStart = clock.seconds();
|
||||
|
||||
|
||||
while (!isStopRequested()) {
|
||||
telemetry.addData("mode", mode);
|
||||
|
||||
switch (mode) {
|
||||
case TUNING_MODE:
|
||||
if (gamepad1.y) {
|
||||
mode = Mode.DRIVER_MODE;
|
||||
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
}
|
||||
|
||||
// calculate and set the motor power
|
||||
double profileTime = clock.seconds() - profileStart;
|
||||
|
||||
if (profileTime > activeProfile.duration()) {
|
||||
// generate a new profile
|
||||
movingForwards = !movingForwards;
|
||||
activeProfile = generateProfile(movingForwards);
|
||||
profileStart = clock.seconds();
|
||||
}
|
||||
|
||||
MotionState motionState = activeProfile.get(profileTime);
|
||||
double targetPower = kV * motionState.getV();
|
||||
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
|
||||
|
||||
List<Double> velocities = drive.getWheelVelocities();
|
||||
|
||||
// update telemetry
|
||||
telemetry.addData("targetVelocity", motionState.getV());
|
||||
for (int i = 0; i < velocities.size(); i++) {
|
||||
telemetry.addData("measuredVelocity" + i, velocities.get(i));
|
||||
telemetry.addData(
|
||||
"error" + i,
|
||||
motionState.getV() - velocities.get(i)
|
||||
);
|
||||
}
|
||||
break;
|
||||
case DRIVER_MODE:
|
||||
if (gamepad1.b) {
|
||||
drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
mode = Mode.TUNING_MODE;
|
||||
movingForwards = true;
|
||||
activeProfile = generateProfile(movingForwards);
|
||||
profileStart = clock.seconds();
|
||||
}
|
||||
|
||||
drive.setWeightedDrivePower(
|
||||
new Pose2d(
|
||||
-gamepad1.left_stick_y,
|
||||
-gamepad1.left_stick_x,
|
||||
-gamepad1.right_stick_x
|
||||
)
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
|
||||
|| lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
|
||||
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
|
||||
|
||||
lastKp = MOTOR_VELO_PID.p;
|
||||
lastKi = MOTOR_VELO_PID.i;
|
||||
lastKd = MOTOR_VELO_PID.d;
|
||||
lastKf = MOTOR_VELO_PID.f;
|
||||
}
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
55
TeamCode/drive/opmode/FollowerPIDTuner.java
Normal file
55
TeamCode/drive/opmode/FollowerPIDTuner.java
Normal file
@ -0,0 +1,55 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
|
||||
|
||||
/*
|
||||
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
|
||||
* classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
|
||||
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
|
||||
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
|
||||
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
|
||||
* you've successfully connected, start the program, and your robot will begin driving in a square.
|
||||
* You should observe the target position (green) and your pose estimate (blue) and adjust your
|
||||
* follower PID coefficients such that you follow the target position as accurately as possible.
|
||||
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
|
||||
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
|
||||
* These coefficients can be tuned live in dashboard.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class FollowerPIDTuner extends LinearOpMode {
|
||||
public static double DISTANCE = 48; // in
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
|
||||
|
||||
drive.setPoseEstimate(startPose);
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
while (!isStopRequested()) {
|
||||
TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
|
||||
.forward(DISTANCE)
|
||||
.turn(Math.toRadians(90))
|
||||
.forward(DISTANCE)
|
||||
.turn(Math.toRadians(90))
|
||||
.forward(DISTANCE)
|
||||
.turn(Math.toRadians(90))
|
||||
.forward(DISTANCE)
|
||||
.turn(Math.toRadians(90))
|
||||
.build();
|
||||
drive.followTrajectorySequence(trajSeq);
|
||||
}
|
||||
}
|
||||
}
|
||||
45
TeamCode/drive/opmode/LocalizationTest.java
Normal file
45
TeamCode/drive/opmode/LocalizationTest.java
Normal file
@ -0,0 +1,45 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/**
|
||||
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
|
||||
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
|
||||
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
|
||||
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
|
||||
* encoder localizer heading may be significantly off if the track width has not been tuned).
|
||||
*/
|
||||
@TeleOp(group = "drive")
|
||||
public class LocalizationTest extends LinearOpMode {
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
waitForStart();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
drive.setWeightedDrivePower(
|
||||
new Pose2d(
|
||||
-gamepad1.left_stick_y,
|
||||
-gamepad1.left_stick_x,
|
||||
-gamepad1.right_stick_x
|
||||
)
|
||||
);
|
||||
|
||||
drive.update();
|
||||
|
||||
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||
telemetry.addData("x", poseEstimate.getX());
|
||||
telemetry.addData("y", poseEstimate.getY());
|
||||
telemetry.addData("heading", poseEstimate.getHeading());
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
154
TeamCode/drive/opmode/ManualFeedforwardTuner.java
Normal file
154
TeamCode/drive/opmode/ManualFeedforwardTuner.java
Normal file
@ -0,0 +1,154 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
|
||||
import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
|
||||
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;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.drive.DriveSignal;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.kinematics.Kinematics;
|
||||
import com.acmerobotics.roadrunner.profile.MotionProfile;
|
||||
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
|
||||
import com.acmerobotics.roadrunner.profile.MotionState;
|
||||
import com.acmerobotics.roadrunner.util.NanoClock;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||
import com.qualcomm.robotcore.util.RobotLog;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/*
|
||||
* This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
|
||||
* tuning these coefficients is just as important as the positional parameters. Like the other
|
||||
* manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
|
||||
* connect your computer to the RC's WiFi network. In your browser, navigate to
|
||||
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
|
||||
* you are using the Control Hub. Once you've successfully connected, start the program, and your
|
||||
* robot will begin moving forward and backward according to a motion profile. Your job is to graph
|
||||
* the velocity errors over time and adjust the feedforward coefficients. Once you've found a
|
||||
* satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
|
||||
*
|
||||
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
|
||||
* user to reset the position of the bot in the event that it drifts off the path.
|
||||
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class ManualFeedforwardTuner extends LinearOpMode {
|
||||
public static double DISTANCE = 72; // in
|
||||
|
||||
private FtcDashboard dashboard = FtcDashboard.getInstance();
|
||||
|
||||
private SampleMecanumDrive drive;
|
||||
|
||||
enum Mode {
|
||||
DRIVER_MODE,
|
||||
TUNING_MODE
|
||||
}
|
||||
|
||||
private Mode mode;
|
||||
|
||||
private static MotionProfile generateProfile(boolean movingForward) {
|
||||
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
|
||||
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
|
||||
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void runOpMode() {
|
||||
if (RUN_USING_ENCODER) {
|
||||
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
|
||||
"when using the built-in drive motor velocity PID.");
|
||||
}
|
||||
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
|
||||
|
||||
drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
mode = Mode.TUNING_MODE;
|
||||
|
||||
NanoClock clock = NanoClock.system();
|
||||
|
||||
telemetry.addLine("Ready!");
|
||||
telemetry.update();
|
||||
telemetry.clearAll();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
boolean movingForwards = true;
|
||||
MotionProfile activeProfile = generateProfile(true);
|
||||
double profileStart = clock.seconds();
|
||||
|
||||
|
||||
while (!isStopRequested()) {
|
||||
telemetry.addData("mode", mode);
|
||||
|
||||
switch (mode) {
|
||||
case TUNING_MODE:
|
||||
if (gamepad1.y) {
|
||||
mode = Mode.DRIVER_MODE;
|
||||
}
|
||||
|
||||
// calculate and set the motor power
|
||||
double profileTime = clock.seconds() - profileStart;
|
||||
|
||||
if (profileTime > activeProfile.duration()) {
|
||||
// generate a new profile
|
||||
movingForwards = !movingForwards;
|
||||
activeProfile = generateProfile(movingForwards);
|
||||
profileStart = clock.seconds();
|
||||
}
|
||||
|
||||
MotionState motionState = activeProfile.get(profileTime);
|
||||
double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
|
||||
|
||||
final double NOMINAL_VOLTAGE = 12.0;
|
||||
final double voltage = voltageSensor.getVoltage();
|
||||
drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0));
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||
double currentVelo = poseVelo.getX();
|
||||
|
||||
// update telemetry
|
||||
telemetry.addData("targetVelocity", motionState.getV());
|
||||
telemetry.addData("measuredVelocity", currentVelo);
|
||||
telemetry.addData("error", motionState.getV() - currentVelo);
|
||||
break;
|
||||
case DRIVER_MODE:
|
||||
if (gamepad1.b) {
|
||||
mode = Mode.TUNING_MODE;
|
||||
movingForwards = true;
|
||||
activeProfile = generateProfile(movingForwards);
|
||||
profileStart = clock.seconds();
|
||||
}
|
||||
|
||||
drive.setWeightedDrivePower(
|
||||
new Pose2d(
|
||||
-gamepad1.left_stick_y,
|
||||
-gamepad1.left_stick_x,
|
||||
-gamepad1.right_stick_x
|
||||
)
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
73
TeamCode/drive/opmode/MaxAngularVeloTuner.java
Normal file
73
TeamCode/drive/opmode/MaxAngularVeloTuner.java
Normal file
@ -0,0 +1,73 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
|
||||
* <p>
|
||||
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
|
||||
* <p>
|
||||
* Further fine tuning of MAX_ANG_VEL may be desired.
|
||||
*/
|
||||
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class MaxAngularVeloTuner extends LinearOpMode {
|
||||
public static double RUNTIME = 4.0;
|
||||
|
||||
private ElapsedTime timer;
|
||||
private double maxAngVelocity = 0.0;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
|
||||
telemetry.addLine("Please ensure you have enough space cleared.");
|
||||
telemetry.addLine("");
|
||||
telemetry.addLine("Press start when ready.");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.update();
|
||||
|
||||
drive.setDrivePower(new Pose2d(0, 0, 1));
|
||||
timer = new ElapsedTime();
|
||||
|
||||
while (!isStopRequested() && timer.seconds() < RUNTIME) {
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||
|
||||
maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
|
||||
}
|
||||
|
||||
drive.setDrivePower(new Pose2d());
|
||||
|
||||
telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
|
||||
telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
|
||||
telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
|
||||
telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested()) idle();
|
||||
}
|
||||
}
|
||||
84
TeamCode/drive/opmode/MaxVelocityTuner.java
Normal file
84
TeamCode/drive/opmode/MaxVelocityTuner.java
Normal file
@ -0,0 +1,84 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.VoltageSensor;
|
||||
import com.qualcomm.robotcore.util.ElapsedTime;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* This routine is designed to calculate the maximum velocity your bot can achieve under load. It
|
||||
* will also calculate the effective kF value for your velocity PID.
|
||||
* <p>
|
||||
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
|
||||
* <p>
|
||||
* Further fine tuning of kF may be desired.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class MaxVelocityTuner extends LinearOpMode {
|
||||
public static double RUNTIME = 2.0;
|
||||
|
||||
private ElapsedTime timer;
|
||||
private double maxVelocity = 0.0;
|
||||
|
||||
private VoltageSensor batteryVoltageSensor;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
|
||||
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
|
||||
telemetry.addLine("Please ensure you have enough space cleared.");
|
||||
telemetry.addLine("");
|
||||
telemetry.addLine("Press start when ready.");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.update();
|
||||
|
||||
drive.setDrivePower(new Pose2d(1, 0, 0));
|
||||
timer = new ElapsedTime();
|
||||
|
||||
while (!isStopRequested() && timer.seconds() < RUNTIME) {
|
||||
drive.updatePoseEstimate();
|
||||
|
||||
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
|
||||
|
||||
maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
|
||||
}
|
||||
|
||||
drive.setDrivePower(new Pose2d());
|
||||
|
||||
double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
|
||||
|
||||
telemetry.addData("Max Velocity", maxVelocity);
|
||||
telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
|
||||
telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested() && opModeIsActive()) idle();
|
||||
}
|
||||
|
||||
private double veloInchesToTicks(double inchesPerSec) {
|
||||
return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
|
||||
}
|
||||
}
|
||||
93
TeamCode/drive/opmode/MotorDirectionDebugger.java
Normal file
93
TeamCode/drive/opmode/MotorDirectionDebugger.java
Normal file
@ -0,0 +1,93 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/**
|
||||
* This is a simple teleop routine for debugging your motor configuration.
|
||||
* Pressing each of the buttons will power its respective motor.
|
||||
*
|
||||
* Button Mappings:
|
||||
*
|
||||
* Xbox/PS4 Button - Motor
|
||||
* X / ▢ - Front Left
|
||||
* Y / Δ - Front Right
|
||||
* B / O - Rear Right
|
||||
* A / X - Rear Left
|
||||
* The buttons are mapped to match the wheels spatially if you
|
||||
* were to rotate the gamepad 45deg°. x/square is the front left
|
||||
* ________ and each button corresponds to the wheel as you go clockwise
|
||||
* / ______ \
|
||||
* ------------.-' _ '-..+ Front of Bot
|
||||
* / _ ( Y ) _ \ ^
|
||||
* | ( X ) _ ( B ) | Front Left \ Front Right
|
||||
* ___ '. ( A ) /| Wheel \ Wheel
|
||||
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
|
||||
* | | | \
|
||||
* '.___.' '. | Rear Left \ Rear Right
|
||||
* '. / Wheel \ Wheel
|
||||
* \. .' (A/X) \ (B/O)
|
||||
* \________/
|
||||
*
|
||||
* Uncomment the @Disabled tag below to use this opmode.
|
||||
*/
|
||||
@Disabled
|
||||
@Config
|
||||
@TeleOp(group = "drive")
|
||||
public class MotorDirectionDebugger extends LinearOpMode {
|
||||
public static double MOTOR_POWER = 0.7;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
telemetry.addLine("Press play to begin the debugging opmode");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
|
||||
|
||||
while (!isStopRequested()) {
|
||||
telemetry.addLine("Press each button to turn on its respective motor");
|
||||
telemetry.addLine();
|
||||
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> X / ▢ - Front Left</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> Y / Δ - Front Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> B / O - Rear Right</font>");
|
||||
telemetry.addLine("<font face=\"monospace\"> A / X - Rear Left</font>");
|
||||
telemetry.addLine();
|
||||
|
||||
if(gamepad1.x) {
|
||||
drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
|
||||
telemetry.addLine("Running Motor: Front Left");
|
||||
} else if(gamepad1.y) {
|
||||
drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
|
||||
telemetry.addLine("Running Motor: Front Right");
|
||||
} else if(gamepad1.b) {
|
||||
drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
|
||||
telemetry.addLine("Running Motor: Rear Right");
|
||||
} else if(gamepad1.a) {
|
||||
drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
|
||||
telemetry.addLine("Running Motor: Rear Left");
|
||||
} else {
|
||||
drive.setMotorPowers(0, 0, 0, 0);
|
||||
telemetry.addLine("Running Motor: None");
|
||||
}
|
||||
|
||||
telemetry.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
38
TeamCode/drive/opmode/SplineTest.java
Normal file
38
TeamCode/drive/opmode/SplineTest.java
Normal file
@ -0,0 +1,38 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.geometry.Vector2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* This is an example of a more complex path to really test the tuning.
|
||||
*/
|
||||
@Autonomous(group = "drive")
|
||||
public class SplineTest extends LinearOpMode {
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
Trajectory traj = drive.trajectoryBuilder(new Pose2d())
|
||||
.splineTo(new Vector2d(30, 30), 0)
|
||||
.build();
|
||||
|
||||
drive.followTrajectory(traj);
|
||||
|
||||
sleep(2000);
|
||||
|
||||
drive.followTrajectory(
|
||||
drive.trajectoryBuilder(traj.end(), true)
|
||||
.splineTo(new Vector2d(0, 0), Math.toRadians(180))
|
||||
.build()
|
||||
);
|
||||
}
|
||||
}
|
||||
46
TeamCode/drive/opmode/StrafeTest.java
Normal file
46
TeamCode/drive/opmode/StrafeTest.java
Normal file
@ -0,0 +1,46 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* This is a simple routine to test translational drive capabilities.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class StrafeTest extends LinearOpMode {
|
||||
public static double DISTANCE = 60; // in
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
|
||||
.strafeRight(DISTANCE)
|
||||
.build();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
drive.followTrajectory(trajectory);
|
||||
|
||||
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||
telemetry.addData("finalX", poseEstimate.getX());
|
||||
telemetry.addData("finalY", poseEstimate.getY());
|
||||
telemetry.addData("finalHeading", poseEstimate.getHeading());
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested() && opModeIsActive()) ;
|
||||
}
|
||||
}
|
||||
46
TeamCode/drive/opmode/StraightTest.java
Normal file
46
TeamCode/drive/opmode/StraightTest.java
Normal file
@ -0,0 +1,46 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.trajectory.Trajectory;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* This is a simple routine to test translational drive capabilities.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class StraightTest extends LinearOpMode {
|
||||
public static double DISTANCE = 60; // in
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
|
||||
.forward(DISTANCE)
|
||||
.build();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
drive.followTrajectory(trajectory);
|
||||
|
||||
Pose2d poseEstimate = drive.getPoseEstimate();
|
||||
telemetry.addData("finalX", poseEstimate.getX());
|
||||
telemetry.addData("finalY", poseEstimate.getY());
|
||||
telemetry.addData("finalHeading", poseEstimate.getHeading());
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested() && opModeIsActive()) ;
|
||||
}
|
||||
}
|
||||
88
TeamCode/drive/opmode/TrackWidthTuner.java
Normal file
88
TeamCode/drive/opmode/TrackWidthTuner.java
Normal file
@ -0,0 +1,88 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.util.Angle;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.util.MovingStatistics;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||
import org.firstinspires.ftc.teamcode.drive.DriveConstants;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* This routine determines the effective track width. The procedure works by executing a point turn
|
||||
* with a given angle and measuring the difference between that angle and the actual angle (as
|
||||
* indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
|
||||
* given angle / actual angle gives a multiplicative adjustment to the estimated track width
|
||||
* (effective track width = estimated track width * given angle / actual angle). The routine repeats
|
||||
* this procedure a few times and averages the values for additional accuracy. Note: a relatively
|
||||
* accurate track width estimate is important or else the angular constraints will be thrown off.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class TrackWidthTuner extends LinearOpMode {
|
||||
public static double ANGLE = 180; // deg
|
||||
public static int NUM_TRIALS = 5;
|
||||
public static int DELAY = 1000; // ms
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
// TODO: if you haven't already, set the localizer to something that doesn't depend on
|
||||
// drive encoders for computing the heading
|
||||
|
||||
telemetry.addLine("Press play to begin the track width tuner routine");
|
||||
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Running...");
|
||||
telemetry.update();
|
||||
|
||||
MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
|
||||
for (int i = 0; i < NUM_TRIALS; i++) {
|
||||
drive.setPoseEstimate(new Pose2d());
|
||||
|
||||
// it is important to handle heading wraparounds
|
||||
double headingAccumulator = 0;
|
||||
double lastHeading = 0;
|
||||
|
||||
drive.turnAsync(Math.toRadians(ANGLE));
|
||||
|
||||
while (!isStopRequested() && drive.isBusy()) {
|
||||
double heading = drive.getPoseEstimate().getHeading();
|
||||
headingAccumulator += Angle.normDelta(heading - lastHeading);
|
||||
lastHeading = heading;
|
||||
|
||||
drive.update();
|
||||
}
|
||||
|
||||
double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
|
||||
trackWidthStats.add(trackWidth);
|
||||
|
||||
sleep(DELAY);
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Tuning complete");
|
||||
telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
|
||||
trackWidthStats.getMean(),
|
||||
trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
idle();
|
||||
}
|
||||
}
|
||||
}
|
||||
104
TeamCode/drive/opmode/TrackingWheelForwardOffsetTuner.java
Normal file
104
TeamCode/drive/opmode/TrackingWheelForwardOffsetTuner.java
Normal file
@ -0,0 +1,104 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.util.Angle;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.util.MovingStatistics;
|
||||
import com.qualcomm.robotcore.util.RobotLog;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.robotcore.internal.system.Misc;
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
|
||||
|
||||
/**
|
||||
* This routine determines the effective forward offset for the lateral tracking wheel.
|
||||
* The procedure executes a point turn at a given angle for a certain number of trials,
|
||||
* along with a specified delay in milliseconds. The purpose of this is to track the
|
||||
* change in the y position during the turn. The offset, or distance, of the lateral tracking
|
||||
* wheel from the center or rotation allows the wheel to spin during a point turn, leading
|
||||
* to an incorrect measurement for the y position. This creates an arc around around
|
||||
* the center of rotation with an arc length of change in y and a radius equal to the forward
|
||||
* offset. We can compute this offset by calculating (change in y position) / (change in heading)
|
||||
* which returns the radius if the angle (change in heading) is in radians. This is based
|
||||
* on the arc length formula of length = theta * radius.
|
||||
*
|
||||
* To run this routine, simply adjust the desired angle and specify the number of trials
|
||||
* and the desired delay. Then, run the procedure. Once it finishes, it will print the
|
||||
* average of all the calculated forward offsets derived from the calculation. This calculated
|
||||
* forward offset is then added onto the current forward offset to produce an overall estimate
|
||||
* for the forward offset. You can run this procedure as many times as necessary until a
|
||||
* satisfactory result is produced.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group="drive")
|
||||
public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
|
||||
public static double ANGLE = 180; // deg
|
||||
public static int NUM_TRIALS = 5;
|
||||
public static int DELAY = 1000; // ms
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
|
||||
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
|
||||
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
|
||||
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
|
||||
}
|
||||
|
||||
telemetry.addLine("Press play to begin the forward offset tuner");
|
||||
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Running...");
|
||||
telemetry.update();
|
||||
|
||||
MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
|
||||
for (int i = 0; i < NUM_TRIALS; i++) {
|
||||
drive.setPoseEstimate(new Pose2d());
|
||||
|
||||
// it is important to handle heading wraparounds
|
||||
double headingAccumulator = 0;
|
||||
double lastHeading = 0;
|
||||
|
||||
drive.turnAsync(Math.toRadians(ANGLE));
|
||||
|
||||
while (!isStopRequested() && drive.isBusy()) {
|
||||
double heading = drive.getPoseEstimate().getHeading();
|
||||
headingAccumulator += Angle.norm(heading - lastHeading);
|
||||
lastHeading = heading;
|
||||
|
||||
drive.update();
|
||||
}
|
||||
|
||||
double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
|
||||
drive.getPoseEstimate().getY() / headingAccumulator;
|
||||
forwardOffsetStats.add(forwardOffset);
|
||||
|
||||
sleep(DELAY);
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Tuning complete");
|
||||
telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
|
||||
forwardOffsetStats.getMean(),
|
||||
forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested()) {
|
||||
idle();
|
||||
}
|
||||
}
|
||||
}
|
||||
130
TeamCode/drive/opmode/TrackingWheelLateralDistanceTuner.java
Normal file
130
TeamCode/drive/opmode/TrackingWheelLateralDistanceTuner.java
Normal file
@ -0,0 +1,130 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.roadrunner.geometry.Pose2d;
|
||||
import com.acmerobotics.roadrunner.util.Angle;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import com.qualcomm.robotcore.util.RobotLog;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
|
||||
|
||||
/**
|
||||
* Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
|
||||
* LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
|
||||
* wheels.
|
||||
*
|
||||
* Tuning Routine:
|
||||
*
|
||||
* 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
|
||||
* measured value. This need only be an estimated value as you will be tuning it anyways.
|
||||
*
|
||||
* 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
|
||||
* similar mark right below the indicator on your bot. This will be your reference point to
|
||||
* ensure you've turned exactly 360°.
|
||||
*
|
||||
* 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
|
||||
* identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
|
||||
* connect your computer to the RC's WiFi network. In your browser, navigate to
|
||||
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
|
||||
* if you are using the Control Hub.
|
||||
* Ensure the field is showing (select the field view in top right of the page).
|
||||
*
|
||||
* 4. Press play to begin the tuning routine.
|
||||
*
|
||||
* 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
|
||||
*
|
||||
* 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
|
||||
*
|
||||
* 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
|
||||
* on the bot and on the ground you created earlier should be lined up.
|
||||
*
|
||||
* 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
|
||||
* StandardTrackingWheelLocalizer.java class.
|
||||
*
|
||||
* 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
|
||||
* yourself. Read the heading output and follow the advice stated in the note below to manually
|
||||
* nudge the values yourself.
|
||||
*
|
||||
* Note:
|
||||
* It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
|
||||
* a line from the circumference to the center should be present, representing the bot. The line
|
||||
* indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
|
||||
* dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
|
||||
* the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
|
||||
* actual bot, the LATERAL_DISTANCE should be increased.
|
||||
*
|
||||
* If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
|
||||
* position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
|
||||
* effective center of rotation. You can ignore this effect. The center of rotation will be offset
|
||||
* slightly but your heading will still be fine. This does not affect your overall tracking
|
||||
* precision. The heading should still line up.
|
||||
*/
|
||||
@Config
|
||||
@TeleOp(group = "drive")
|
||||
public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
|
||||
public static int NUM_TURNS = 10;
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
|
||||
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
|
||||
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
|
||||
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
|
||||
}
|
||||
|
||||
telemetry.addLine("Prior to beginning the routine, please read the directions "
|
||||
+ "located in the comments of the opmode file.");
|
||||
telemetry.addLine("Press play to begin the tuning routine.");
|
||||
telemetry.addLine("");
|
||||
telemetry.addLine("Press Y/△ to stop the routine.");
|
||||
telemetry.update();
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.update();
|
||||
|
||||
double headingAccumulator = 0;
|
||||
double lastHeading = 0;
|
||||
|
||||
boolean tuningFinished = false;
|
||||
|
||||
while (!isStopRequested() && !tuningFinished) {
|
||||
Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
|
||||
drive.setDrivePower(vel);
|
||||
|
||||
drive.update();
|
||||
|
||||
double heading = drive.getPoseEstimate().getHeading();
|
||||
double deltaHeading = heading - lastHeading;
|
||||
|
||||
headingAccumulator += Angle.normDelta(deltaHeading);
|
||||
lastHeading = heading;
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
|
||||
telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
|
||||
telemetry.addLine();
|
||||
telemetry.addLine("Press Y/△ to conclude routine");
|
||||
telemetry.update();
|
||||
|
||||
if (gamepad1.y)
|
||||
tuningFinished = true;
|
||||
}
|
||||
|
||||
telemetry.clearAll();
|
||||
telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
|
||||
telemetry.addLine("Effective LATERAL_DISTANCE: " +
|
||||
(headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
|
||||
|
||||
telemetry.update();
|
||||
|
||||
while (!isStopRequested()) idle();
|
||||
}
|
||||
}
|
||||
27
TeamCode/drive/opmode/TurnTest.java
Normal file
27
TeamCode/drive/opmode/TurnTest.java
Normal file
@ -0,0 +1,27 @@
|
||||
package org.firstinspires.ftc.teamcode.drive.opmode;
|
||||
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
|
||||
|
||||
/*
|
||||
* This is a simple routine to test turning capabilities.
|
||||
*/
|
||||
@Config
|
||||
@Autonomous(group = "drive")
|
||||
public class TurnTest extends LinearOpMode {
|
||||
public static double ANGLE = 90; // deg
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
drive.turn(Math.toRadians(ANGLE));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user