From 679b13969d0c9506152ace4470ef490b91928055 Mon Sep 17 00:00:00 2001 From: Justin Date: Fri, 11 Feb 2022 01:02:18 -0500 Subject: [PATCH] Add a basic waypoint based auto, using 6328s FieldConstants --- src/main/java/frc/robot/Constants.java | 16 ++- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/RamsetA.java | 26 +++- .../java/frc/robot/commands/auto/TwoBall.java | 33 +++++ .../frc/robot/subsystems/DriveSubsystem.java | 1 + .../java/frc/robot/util/FieldConstants.java | 136 ++++++++++++++++++ src/main/java/frc/robot/util/Util.java | 124 ++++++++++++++++ 7 files changed, 333 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/commands/auto/TwoBall.java create mode 100644 src/main/java/frc/robot/util/FieldConstants.java create mode 100644 src/main/java/frc/robot/util/Util.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbfb8c1..76d86a5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -47,6 +47,13 @@ public static final class DriveConstants { public static final double gearRatio = 60.0 / 10.0; public static final double distPerPulse = (1.0 / gearRatio) * Units.inchesToMeters(wheelDiameter) * Math.PI; + + private static final double bumperlessRobotLength = Units.inchesToMeters(26); + private static final double bumperlessRobotWidth = Units.inchesToMeters(24); + private static final double bumperThickness = Units.inchesToMeters(3); + + public static final double fullRobotWidth = bumperlessRobotWidth + bumperThickness * 2; + public static final double fullRobotLength = bumperlessRobotLength + bumperThickness * 2; } public static final class IntakeConstants { @@ -73,7 +80,7 @@ public static final class AutoConstants { // FF and FB gains; NEED TO BE DETERMINED ON THE FULLY BUILT ROBOT, WILL CHANGE // WITH WEIGHT public static final double ksVolts = 0.20541; - public static final double ksVoltSecondsPerMeter = 2.4361; + public static final double kvVoltSecondsPerMeter = 2.4361; public static final double kaVoltSecondsSquaredPerMeter = 0.25946; public static final double kPDriveVel = 3.95; @@ -83,12 +90,15 @@ public static final class AutoConstants { public static final DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(trackWidth); + public static final double maxCentripetalAcceleration = 100; + // Ramsete constants; generally the same on all robots public static final double RamseteZeta = 0.7; public static final double RamseteB = 2; // Max speeds - public static final double maxSpeed = 8; - public static final double maxAccel = 5; + public static final double maxSpeed = 1; + public static final double maxAccel = 1; + public static final double maxVoltageApplied = 6; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e05603c..23c85fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,7 +96,7 @@ public Command getAutonomousCommand() { Constants.AutoConstants.RamseteB, Constants.AutoConstants.RamseteZeta), new SimpleMotorFeedforward( Constants.AutoConstants.ksVolts, - Constants.AutoConstants.ksVoltSecondsPerMeter, + Constants.AutoConstants.kvVoltSecondsPerMeter, Constants.AutoConstants.kaVoltSecondsSquaredPerMeter), Constants.AutoConstants.kinematics, driveSubsystem::getWheelSpeeds, diff --git a/src/main/java/frc/robot/commands/RamsetA.java b/src/main/java/frc/robot/commands/RamsetA.java index 2599ca6..fd6f886 100644 --- a/src/main/java/frc/robot/commands/RamsetA.java +++ b/src/main/java/frc/robot/commands/RamsetA.java @@ -3,18 +3,42 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; +import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; import frc.robot.subsystems.DriveSubsystem; +import java.util.List; public class RamsetA extends SequentialCommandGroup { protected Trajectory autoTrajectory; + public static Trajectory makeTrajectory( + double startVelocity, List waypoints, double endVelocity, boolean reversed) { + CentripetalAccelerationConstraint centripetalAccelerationConstraint = + new CentripetalAccelerationConstraint(Constants.AutoConstants.maxCentripetalAcceleration); + return TrajectoryGenerator.generateTrajectory( + waypoints, + new TrajectoryConfig(Constants.AutoConstants.maxSpeed, Constants.AutoConstants.maxAccel) + .addConstraint(centripetalAccelerationConstraint) + .addConstraint( + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + Constants.AutoConstants.ksVolts, + Constants.AutoConstants.kvVoltSecondsPerMeter, + Constants.AutoConstants.kaVoltSecondsSquaredPerMeter), + Constants.AutoConstants.kinematics, + Constants.AutoConstants.maxVoltageApplied))); + } + public static Command RamseteSchmoove(Trajectory autoTrajectory, DriveSubsystem driveSubsystem) { RamseteCommand ramsete = new RamseteCommand( @@ -24,7 +48,7 @@ public static Command RamseteSchmoove(Trajectory autoTrajectory, DriveSubsystem Constants.AutoConstants.RamseteB, Constants.AutoConstants.RamseteZeta), new SimpleMotorFeedforward( Constants.AutoConstants.ksVolts, - Constants.AutoConstants.ksVoltSecondsPerMeter, + Constants.AutoConstants.kvVoltSecondsPerMeter, Constants.AutoConstants.kaVoltSecondsSquaredPerMeter), Constants.AutoConstants.kinematics, driveSubsystem::getWheelSpeeds, diff --git a/src/main/java/frc/robot/commands/auto/TwoBall.java b/src/main/java/frc/robot/commands/auto/TwoBall.java new file mode 100644 index 0000000..046f4ae --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/TwoBall.java @@ -0,0 +1,33 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.DeployIntake; +import frc.robot.commands.RamsetA; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.IntakeFourBar; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.util.FieldConstants; +import java.util.List; + +public class TwoBall extends SequentialCommandGroup { + public TwoBall( + DriveSubsystem driveSubsystem, IntakeSubsystem intakeSubsystem, IntakeFourBar fourBar) { + addCommands( + sequence( + new DeployIntake(intakeSubsystem, fourBar), + RamsetA.RamseteSchmoove( + RamsetA.makeTrajectory( + 0, + List.of(FieldConstants.StartingPoints.tarmacD, FieldConstants.cargoE), + 0.0, + false), + driveSubsystem), + RamsetA.RamseteSchmoove( + RamsetA.makeTrajectory( + 0.0, + List.of(FieldConstants.cargoE, FieldConstants.StartingPoints.fenderB), + 0.0, + false), + driveSubsystem))); + } +} diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 603304f..09880bf 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -95,6 +95,7 @@ public Pose2d getPose() { public void resetOdometry(Pose2d pose) { roboOdometry.resetPosition(pose, gyro.getRotation2d()); + resetEncoders(); } public DifferentialDriveWheelSpeeds getWheelSpeeds() { diff --git a/src/main/java/frc/robot/util/FieldConstants.java b/src/main/java/frc/robot/util/FieldConstants.java new file mode 100644 index 0000000..ff8aa79 --- /dev/null +++ b/src/main/java/frc/robot/util/FieldConstants.java @@ -0,0 +1,136 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + +public class FieldConstants { + // Field dimensions + public static final double fieldLength = Units.inchesToMeters(54.0 * 12.0); + public static final double fieldWidth = Units.inchesToMeters(27.0 * 12.0); + public static final double hangarLength = Units.inchesToMeters(128.75); + public static final double hangarWidth = Units.inchesToMeters(116.0); + + // Vision target + public static final double visionTargetDiameter = Units.inchesToMeters(4.0 * 12.0 + 5.375); + + /* Bottom of tape */ + public static final double visionTargetHeightLower = Units.inchesToMeters(8.0 * 12 + 5.625); + + /* Top of tape */ + public static final double visionTargetHeightUpper = + visionTargetHeightLower + Units.inchesToMeters(2.0); + + // Dimensions of hub and tarmac + public static final Rotation2d centerLineAngle = Rotation2d.fromDegrees(66.0); + public static final Translation2d hubCenter = + new Translation2d(fieldLength / 2.0, fieldWidth / 2.0); + public static final double tarmacInnerDiameter = Units.inchesToMeters(219.25); + public static final double tarmacOuterDiameter = Units.inchesToMeters(237.31); + public static final double tarmacFenderToTip = Units.inchesToMeters(84.75); + + /* If the tarmac formed a full octagon */ + public static final double tarmacFullSideLength = tarmacInnerDiameter * (Math.sqrt(2.0) - 1.0); + + /* Length of tape marking outside of tarmac */ + public static final double tarmacMarkedSideLength = Units.inchesToMeters(82.83); + + /* Length removed because of corner cutoff */ + public static final double tarmacMissingSideLength = + tarmacFullSideLength - tarmacMarkedSideLength; + + public static final double hubSquareLength = tarmacOuterDiameter - (tarmacFenderToTip * 2.0); + + // Reference rotations (angle from hub to each reference point and fender side) + public static final Rotation2d referenceARotation = + Rotation2d.fromDegrees(180.0) + .minus(centerLineAngle) + .plus(Rotation2d.fromDegrees(360.0 / 16.0)); + public static final Rotation2d referenceBRotation = + referenceARotation.rotateBy(Rotation2d.fromDegrees(360.0 / 8.0)); + public static final Rotation2d referenceCRotation = + referenceBRotation.rotateBy(Rotation2d.fromDegrees(360.0 / 8.0)); + public static final Rotation2d referenceDRotation = + referenceCRotation.rotateBy(Rotation2d.fromDegrees(360.0 / 8.0)); + public static final Rotation2d fenderARotation = + referenceARotation.rotateBy(Rotation2d.fromDegrees(360.0 / 16.0)); + public static final Rotation2d fenderBRotation = + fenderARotation.rotateBy(Rotation2d.fromDegrees(90.0)); + + // Reference points (centered of the sides of the tarmac if they formed a + // complete octagon, plus + // edges of fender) + public static final Pose2d referenceA = + new Pose2d(hubCenter, referenceARotation) + .transformBy(Util.Geometry.transformFromTranslation(tarmacInnerDiameter / 2.0, 0.0)); + public static final Pose2d referenceB = + new Pose2d(hubCenter, referenceBRotation) + .transformBy(Util.Geometry.transformFromTranslation(tarmacInnerDiameter / 2.0, 0.0)); + public static final Pose2d referenceC = + new Pose2d(hubCenter, referenceCRotation) + .transformBy(Util.Geometry.transformFromTranslation(tarmacInnerDiameter / 2.0, 0.0)); + public static final Pose2d referenceD = + new Pose2d(hubCenter, referenceDRotation) + .transformBy(Util.Geometry.transformFromTranslation(tarmacInnerDiameter / 2.0, 0.0)); + private static final Pose2d fenderA = + new Pose2d(hubCenter, fenderARotation) + .transformBy(Util.Geometry.transformFromTranslation(hubSquareLength / 2.0, 0.0)); + private static final Pose2d fenderB = + new Pose2d(hubCenter, fenderBRotation) + .transformBy(Util.Geometry.transformFromTranslation(hubSquareLength / 2.0, 0.0)); + + // Cargo points + public static final double cornerToCargoY = Units.inchesToMeters(15.56); + public static final double referenceToCargoY = (tarmacFullSideLength / 2.0) - cornerToCargoY; + public static final double referenceToCargoX = Units.inchesToMeters(40.44); + public static final Pose2d cargoA = + referenceA.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, -referenceToCargoY)); + public static final Pose2d cargoB = + referenceA.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, referenceToCargoY)); + public static final Pose2d cargoC = + referenceB.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, referenceToCargoY)); + public static final Pose2d cargoD = + referenceC.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, -referenceToCargoY)); + public static final Pose2d cargoE = + referenceD.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, -referenceToCargoY)); + public static final Pose2d cargoF = + referenceD.transformBy( + Util.Geometry.transformFromTranslation(referenceToCargoX, referenceToCargoY)); + + // Terminal cargo point + public static final Rotation2d terminalOuterRotation = Rotation2d.fromDegrees(133.75); + public static final double terminalLength = Units.inchesToMeters(324.0 - 256.42); + public static final double terminalWidth = + Math.tan(Rotation2d.fromDegrees(180.0).minus(terminalOuterRotation).getRadians()) + * terminalLength; + public static final Pose2d terminalCenter = + new Pose2d( + new Translation2d(terminalLength / 2.0, terminalWidth / 2.0), + terminalOuterRotation.minus(Rotation2d.fromDegrees(90.0))); + public static final double terminalCargoOffset = Units.inchesToMeters(10.43); + public static final Pose2d cargoG = + terminalCenter.transformBy(Util.Geometry.transformFromTranslation(terminalCargoOffset, 0.0)); + + // Starting points + public static class StartingPoints { + public static final Pose2d tarmacA = + referenceA.transformBy(Util.Geometry.transformFromTranslation(-0.5, 0.7)); + public static final Pose2d tarmacB = + referenceB.transformBy(Util.Geometry.transformFromTranslation(-0.5, -0.2)); + public static final Pose2d tarmacC = + referenceC.transformBy(Util.Geometry.transformFromTranslation(-0.5, -0.1)); + public static final Pose2d tarmacD = + referenceD.transformBy(Util.Geometry.transformFromTranslation(-0.5, -0.7)); + + public static final Pose2d fenderA = + FieldConstants.fenderA.transformBy(Util.Geometry.transformFromTranslation(0.5, 0.0)); + public static final Pose2d fenderB = + Util.Geometry.offsetDrivetrainFromPose(FieldConstants.fenderB); + } +} diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java new file mode 100644 index 0000000..e8e62c6 --- /dev/null +++ b/src/main/java/frc/robot/util/Util.java @@ -0,0 +1,124 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import frc.robot.Constants; + +public class Util { + public static class Geometry { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d transformFromTranslation(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x componenet of the translation + * @param y The y componenet of the translation + * @return The resulting transform + */ + public static Transform2d transformFromTranslation(double x, double y) { + return new Transform2d(new Translation2d(x, y), new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d transformFromRotation(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d poseFromTranslation(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d poseFromRotation(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d poseToTransform(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d transformToPose(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Interpolates between two poses based on the scale factor t. For example, t=0 would result in + * the first pose, t=1 would result in the last pose, and t=0.5 would result in a pose which is + * exactly halfway between the two poses. Values of t less than zero return the first pose, and + * values of t greater than 1 return the last pose. + * + * @param lhs The left hand side, or first pose to use for interpolation + * @param rhs The right hand side, or last pose to use for interpolation + * @param t The scale factor, 0 <= t <= 1 + * @return The pose which represents the interpolation. For t <= 0, the "lhs" parameter is + * returned directly. For t >= 1, the "rhs" parameter is returned directly. + */ + public static Pose2d interpolate(Pose2d lhs, Pose2d rhs, double t) { + if (t <= 0) { + return lhs; + } else if (t >= 1) { + return rhs; + } + Twist2d twist = lhs.log(rhs); + Twist2d scaled = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t); + return lhs.exp(scaled); + } + + /** + * Returns the direction that this translation makes with the origin as a Rotation2d + * + * @param translation The translation + * @return The direction of the translation + */ + public static Rotation2d direction(Translation2d translation) { + return new Rotation2d(translation.getX(), translation.getY()); + } + + public static Pose2d offsetDrivetrainFromPose(Pose2d pose) { + return pose.transformBy( + new Transform2d( + new Translation2d(Constants.DriveConstants.fullRobotLength / 2, 0), + new Rotation2d())); + } + } +}