diff --git a/CommandRobotBase.java b/CommandRobotBase.java index 88bb3fea..c4c9aaf2 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -6,6 +6,7 @@ import org.usfirst.frc4904.standard.humaninput.Operator; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/commands/CancelCurrent.java b/commands/CancelCurrent.java new file mode 100644 index 00000000..fa91ce3b --- /dev/null +++ b/commands/CancelCurrent.java @@ -0,0 +1,32 @@ +package org.usfirst.frc4904.standard.commands; + +import org.usfirst.frc4904.standard.LogKitten; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * This class cancels a command + */ +public class CancelCurrent extends CommandBase { + protected final SubsystemBase subsystem; + + public CancelCurrent(SubsystemBase subsystem) { + super(); + setName("Cancel " + subsystem.getName() + "'s current command."); + this.subsystem = subsystem; + } + + public void initialize() { + LogKitten.v("Initializing " + getName()); + if(subsystem.getCurrentCommand() != null){ + subsystem.getCurrentCommand().cancel(); + } + } + + public boolean isFinished() { + return true; + } +} diff --git a/commands/RunFor.java b/commands/RunFor.java index f8271b99..f6d38fd9 100644 --- a/commands/RunFor.java +++ b/commands/RunFor.java @@ -25,7 +25,6 @@ public RunFor(String name, CommandBase command, double duration) { this.duration = duration; this.command = command; firstTick = true; - addRequirements((Subsystem[]) this.command.getRequirements().toArray()); } /** diff --git a/commands/RunIfElse.java b/commands/RunIfElse.java index 64835f7a..7cd3499f 100644 --- a/commands/RunIfElse.java +++ b/commands/RunIfElse.java @@ -18,9 +18,6 @@ public RunIfElse(String name, CommandBase ifCommand, CommandBase elseCommand, this.ifCommand = ifCommand; this.elseCommand = elseCommand; this.booleanSuppliers = booleanSuppliers; - - addRequirements((Subsystem[]) ifCommand.getRequirements().toArray()); - addRequirements((Subsystem[]) elseCommand.getRequirements().toArray()); } public RunIfElse(CommandBase ifCommand, CommandBase elseCommand, BooleanSupplier... booleanSuppliers) { diff --git a/commands/RunUntil.java b/commands/RunUntil.java index b9799396..75e44f75 100644 --- a/commands/RunUntil.java +++ b/commands/RunUntil.java @@ -7,17 +7,18 @@ public class RunUntil extends CommandBase { protected final CommandBase command; protected final Supplier stopCondition; + protected final boolean cancelOnEnd; - public RunUntil(String name, CommandBase command, Supplier stopCondition) { + public RunUntil(String name, CommandBase command, Supplier stopCondition, boolean cancelOnEnd) { super(); setName(name); this.command = command; this.stopCondition = stopCondition; - addRequirements((Subsystem[]) command.getRequirements().toArray()); + this.cancelOnEnd = cancelOnEnd; } public RunUntil(CommandBase command, Supplier stopCondition) { - this("RunUntil", command, stopCondition); + this("RunUntil", command, stopCondition, true); } @Override @@ -32,6 +33,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - command.cancel(); + if (cancelOnEnd){ + command.cancel(); + } } } diff --git a/commands/RunWhile.java b/commands/RunWhile.java index 4217831e..3d84da8d 100644 --- a/commands/RunWhile.java +++ b/commands/RunWhile.java @@ -13,7 +13,6 @@ public RunWhile(String name, CommandBase command, Supplier stopConditio setName(name); this.command = command; this.stopCondition = stopCondition; - addRequirements((Subsystem[]) command.getRequirements().toArray()); } public RunWhile(CommandBase command, Supplier stopCondition) { diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index 687112fd..5125dd0b 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -2,10 +2,8 @@ package org.usfirst.frc4904.standard.commands.chassis; -import java.util.List; - -import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.standard.subsystems.chassis.SensorDrive; +import org.usfirst.frc4904.standard.subsystems.chassis.SplinesDrive; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -13,87 +11,114 @@ import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; -import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +/** + * This command creates a simple spline to follow a Trajectory using a SensorDrive. Note that while the nextCommand to run after the robot finishes driving and the initialPos are both configurable, they will almost always be set automatically. + */ public class SimpleSplines extends SequentialCommandGroup { - public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos, double maxVoltage, Command nextCommand){ - super(new RamseteCommand( - TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, new TrajectoryConfig(robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond, - robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared) - .setKinematics(robotDrive.getDriveConstants().kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, - robotDrive.getDriveConstants().kvVoltSecondsPerMeter, - robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), - robotDrive.getDriveConstants().kDriveKinematics, - maxVoltage))), + /** + * @param robotDrive the SensorDrive used to follow the trajectory. + * @param trajectory the Trajectory to follow. + * @param nextCommand the command to run immediately following the spline completion. In most cases, this should be setting the chassis voltages all to 0. + * @param initialPos the initial pose to reset the robot's odometry to. + */ + public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Command nextCommand, Pose2d initialPos){ + super(new InstantCommand(() -> robotDrive.resetOdometry(initialPos)), + new RamseteCommand( + trajectory, robotDrive::getPose, - new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta), - new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, - robotDrive.getDriveConstants().kvVoltSecondsPerMeter, - robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), - robotDrive.getDriveConstants().kDriveKinematics, + new RamseteController(robotDrive.getAutoConstants().RAMSETE_B, robotDrive.getAutoConstants().RAMSETE_ZETA), + new SimpleMotorFeedforward(robotDrive.getDriveConstants().KS, + robotDrive.getDriveConstants().KV, + robotDrive.getDriveConstants().KA), + robotDrive.getDriveConstants().DRIVE_KINEMATICS, robotDrive::getWheelSpeeds, - new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0), - new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0), - // RamseteCommand passes volts to the callback - robotDrive::tankDriveVolts, robotDrive), nextCommand); + new PIDController(robotDrive.getDriveConstants().KP_DRIVE_VEL, 0, 0), + new PIDController(robotDrive.getDriveConstants().KP_DRIVE_VEL, 0, 0), + robotDrive::tankDriveVolts, robotDrive.getDriveBase().getMotors()), nextCommand); + addRequirements(robotDrive.getDriveBase()); } - public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos, double maxVoltage){ - this(robotDrive, init_pos, inter_points, final_pos, maxVoltage, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0))); + /** + * @param robotDrive the SensorDrive used to follow the trajectory. + * @param trajectory the Trajectory to follow. + * @param initialPos the initial pose to reset the robot's odometry to. + */ + public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Pose2d initialPos){ + this(robotDrive, trajectory, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0)), initialPos); } - public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos){ - this(robotDrive, init_pos, inter_points, final_pos, 10); + /** + * @param robotDrive the SensorDrive used to follow the trajectory. + * @param trajectory the Trajectory to follow. + * @param nextCommand the command to run immediately following the spline completion. In most cases, this should be setting the chassis voltages all to 0. + */ + public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory, Command nextCommand){ + this(robotDrive, trajectory, nextCommand, trajectory.getStates().get(0).poseMeters); } - public SimpleSplines(SensorDrive robotDrive, Pose2d init_pos, Pose2d final_pos){ - this(robotDrive, init_pos, List.of(), final_pos); + /** + * @param robotDrive the SensorDrive used to follow the trajectory. + * @param trajectory the Trajectory to follow. + */ + public SimpleSplines(SplinesDrive robotDrive, Trajectory trajectory){ + this(robotDrive, trajectory, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0))); } - /** * Class to store autonomous constants used for Ramsete Pathing. */ - public static class AutoConstants { - public double kMaxSpeedMetersPerSecond; - public double kMaxAccelerationMetersPerSecondSquared; - public double kRamseteB; - public double kRamseteZeta; - - public AutoConstants(double kMaxSpeedMetersPerSecond, double kMaxAccelerationMetersPerSecondSquared, double kRamseteB, double kRamseteZeta) { - this.kMaxSpeedMetersPerSecond = kMaxSpeedMetersPerSecond; - this.kMaxAccelerationMetersPerSecondSquared = kMaxAccelerationMetersPerSecondSquared; - this.kRamseteB = kRamseteB; - this.kRamseteZeta = kRamseteZeta; + public static class SplineAutoConstants { + public double MAX_SPEED; + public double MAX_ACCEL; + public double RAMSETE_B; + public double RAMSETE_ZETA; + + /** + * Constructor for the auto constants. + * @param MAX_SPEED_MS max speed of the robot. Meters per second. + * @param MAX_ACCEL_MSS max acceleration of the robot. Meters per second squared. + * @param RAMSETE_B B coefficient. Should be 2.0 for most contexts. + * @param kRamseteZeta Zeta coefficient. Should be 0.7 for most contexts. + */ + public SplineAutoConstants(double MAX_SPEED, double MAX_ACCEL, double RAMSETE_B, double RAMSETE_ZETA) { + this.MAX_SPEED = MAX_SPEED; + this.MAX_ACCEL = MAX_ACCEL; + this.RAMSETE_B = RAMSETE_B; + this.RAMSETE_ZETA = RAMSETE_ZETA; } } /** * Class to store drive constants used for Ramsete pathing. */ - public static class DriveConstants { - public double ksVolts; - public double kvVoltSecondsPerMeter; - public double kaVoltSecondsSquaredPerMeter; - public double kTrackwidthMeters; - public DifferentialDriveKinematics kDriveKinematics; - public double kPDriveVel; + public static class SplineDriveConstants { + public double KS; + public double KV; + public double KA; + public double TRACK_WIDTH; + public DifferentialDriveKinematics DRIVE_KINEMATICS; + public double KP_DRIVE_VEL; - public DriveConstants(double ksVolts, double kvVoltSecondsPerMeter, double kaVoltSecondsSquaredPerMeter, double kTrackwidthMeters, double kPDriveVel){ - this.ksVolts = ksVolts; - this.kvVoltSecondsPerMeter = kvVoltSecondsPerMeter; - this.kaVoltSecondsSquaredPerMeter = kaVoltSecondsSquaredPerMeter; - this.kTrackwidthMeters = kTrackwidthMeters; - this.kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters); - this.kPDriveVel = kPDriveVel; + /** + * Constructor for the drive constants. + * @param KS ksVolts (from characterization). + * @param KV kvVoltSecondsPerMeter (from characterization). + * @param KA kaVoltSecondsSquaredPerMeter (from characterization). + * @param TRACK_WIDTH kTrackwidthMeters (from characterization). + * @param KP_DRIVE_VEL kPDriveVel (from characterization). + */ + public SplineDriveConstants(double KS, double KV, double KA, double TRACK_WIDTH, double KP_DRIVE_VEL){ + this.KS = KS; + this.KV = KV; + this.KA = KA; + this.TRACK_WIDTH = TRACK_WIDTH; + this.DRIVE_KINEMATICS = new DifferentialDriveKinematics(TRACK_WIDTH); + this.KP_DRIVE_VEL = KP_DRIVE_VEL; } } } diff --git a/commands/motor/MotorVelocitySet.java b/commands/motor/MotorVelocitySet.java index 285b1535..9cd8889d 100644 --- a/commands/motor/MotorVelocitySet.java +++ b/commands/motor/MotorVelocitySet.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.motor; +import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.commands.Noop; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; @@ -27,6 +28,7 @@ public MotorVelocitySet(String name, VelocitySensorMotor motor, double velocity, super(); setName(name); addRequirements(motor); + addRequirements(RobotMap.Component.flywheelMotorA, RobotMap.Component.flywheelMotorB); // TODO: @Daniel-E-B requirements should be done in the classes that extend this this.motor = motor; this.fallbackCommand = fallbackCommand; this.setVelocity(velocity); @@ -63,26 +65,26 @@ public void setVelocity(double velocity) { @Override public void initialize() { - try { - motor.reset(); + // try { // TODO: @Daniel-E-B uncomment and test + // motor.reset(); motor.enableMotionController(); motor.set(velocity); - } catch (InvalidSensorException e) { - cancel(); - fallbackCommand.schedule(); - } + // } catch (InvalidSensorException e) { + // cancel(); + // fallbackCommand.schedule(); + // } } @Override public void execute() { motor.set(velocity); - Exception potentialSensorException = motor.checkSensorException(); - if (potentialSensorException != null) { - cancel(); - if (!fallbackCommand.isScheduled()) { - fallbackCommand.schedule(); - } - } + // Exception potentialSensorException = motor.checkSensorException(); // TODO: @Daniel-E-B uncomment and test + // if (potentialSensorException != null) { + // cancel(); + // if (!fallbackCommand.isScheduled()) { + // fallbackCommand.schedule(); + // } + // } } @Override @@ -92,6 +94,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { + motor.disableMotionController(); + motor.set(0.0); if (!interrupted) { motor.set(0.0); } diff --git a/custom/motioncontrollers/CustomPIDController.java b/custom/motioncontrollers/CustomPIDController.java index a8ae27df..292b88e9 100644 --- a/custom/motioncontrollers/CustomPIDController.java +++ b/custom/motioncontrollers/CustomPIDController.java @@ -272,7 +272,7 @@ public double getError() { */ private double feedForward() { if(sensor.getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement){ - return F * Math.signum(setpoint); + return F * Math.signum(lastError); } else { return F * setpoint; } diff --git a/custom/sensors/CANTalonEncoder.java b/custom/sensors/CANTalonEncoder.java index 66deceba..83e12d57 100644 --- a/custom/sensors/CANTalonEncoder.java +++ b/custom/sensors/CANTalonEncoder.java @@ -184,4 +184,8 @@ public boolean getStoppedSafely() { public double getRateSafely() { return getRate(); } + + public BaseTalon getTalon() { + return talon; + } } diff --git a/subsystems/.DS_Store b/subsystems/.DS_Store deleted file mode 100644 index 8656e0e5..00000000 Binary files a/subsystems/.DS_Store and /dev/null differ diff --git a/subsystems/chassis/.DS_Store b/subsystems/chassis/.DS_Store deleted file mode 100644 index 3bd6c21b..00000000 Binary files a/subsystems/chassis/.DS_Store and /dev/null differ diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index dd3d61b6..f77dde6c 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -7,6 +7,8 @@ package org.usfirst.frc4904.standard.subsystems.chassis; +import java.util.List; + import com.ctre.phoenix.sensors.CANCoder; import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; @@ -14,10 +16,20 @@ import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.spline.Spline; +import edu.wpi.first.wpilibj.spline.SplineHelper; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator.ControlVectorList; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -26,20 +38,16 @@ public class SensorDrive implements Subsystem { // Based largely on private final TankDrive driveBase; private final CANCoder leftEncoder; private final CANCoder rightEncoder; - private final SimpleSplines.AutoConstants autoConstants; - private final SimpleSplines.DriveConstants driveConstants; private final IMU gyro; private final DifferentialDriveOdometry odometry; /** * Creates a new DriveSubsystem. */ - public SensorDrive(TankDrive driveBase, SimpleSplines.AutoConstants autoConstants, SimpleSplines.DriveConstants driveConstants, CANCoder leftEncoder, CANCoder rightEncoder, IMU gyro) { + public SensorDrive(TankDrive driveBase, CANCoder leftEncoder, CANCoder rightEncoder, IMU gyro) { this.driveBase = driveBase; this.leftEncoder = leftEncoder; this.rightEncoder = rightEncoder; - this.autoConstants = autoConstants; - this.driveConstants = driveConstants; this.gyro = gyro; resetEncoders(); @@ -52,12 +60,12 @@ public void periodic() { odometry.update(Rotation2d.fromDegrees(getHeading()), leftEncoder.getPosition(), rightEncoder.getPosition()); } - public SimpleSplines.AutoConstants getAutoConstants(){ - return autoConstants; - } - public SimpleSplines.DriveConstants getDriveConstants(){ - return driveConstants; + /** + * Gets the drive base. + */ + public TankDrive getDriveBase(){ + return this.driveBase; } /** diff --git a/subsystems/chassis/SplinesDrive.java b/subsystems/chassis/SplinesDrive.java new file mode 100644 index 00000000..e4180379 --- /dev/null +++ b/subsystems/chassis/SplinesDrive.java @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4904.standard.subsystems.chassis; + +import java.util.List; + +import com.ctre.phoenix.sensors.CANCoder; + +import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; +import org.usfirst.frc4904.standard.custom.sensors.IMU; +import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; +import org.usfirst.frc4904.standard.subsystems.motor.Motor; + +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class SplinesDrive extends SensorDrive { + private final SimpleSplines.SplineAutoConstants autoConstants; + private final SimpleSplines.SplineDriveConstants driveConstants; + private TrajectoryConfig pathConfig; + + /** + * Creates a new DriveSubsystem. + */ + public SplinesDrive(TankDrive driveBase, SimpleSplines.SplineAutoConstants autoConstants, SimpleSplines.SplineDriveConstants driveConstants, CANCoder leftEncoder, CANCoder rightEncoder, IMU gyro) { + super(driveBase,leftEncoder, rightEncoder, gyro); + this.autoConstants = autoConstants; + this.driveConstants = driveConstants; + configuratePath(10); + } + + public SimpleSplines.SplineAutoConstants getAutoConstants(){ + return autoConstants; + } + + public SimpleSplines.SplineDriveConstants getDriveConstants(){ + return driveConstants; + } + + /** + * Sets the pathConfig based on the maxVoltage allowed. + * + * @param maxVoltage the max voltage to be allowed in the chassis. + */ + public void configuratePath(double maxVoltage){ + pathConfig = new TrajectoryConfig(autoConstants.MAX_SPEED, autoConstants.MAX_ACCEL) + .setKinematics(driveConstants.DRIVE_KINEMATICS) + .addConstraint(new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward(driveConstants.KS, + driveConstants.KV, + driveConstants.KA), + driveConstants.DRIVE_KINEMATICS, + maxVoltage)); + } + + /** + * Generates a trajectory using clamped cubic splines, allowing the input of a starting and ending Pose2d and intermediate Translation2d Points. + * @param init_pos the initial pose to start at - unless otherwise specified, RamseteCommand will automatically assume this to be the robot's initial position. + * @param inter_points the intermediate (x,y) Translation2d waypoints. Can create using List.of(). + * @param final_pos the final pose of the robot. + * @return the trajectory. + */ + public Trajectory generateSimpleTrajectory(Pose2d init_pos, List inter_points, Pose2d final_pos){ + return TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, pathConfig); + } + + /** + * Generates a trajectory using quintic splines, allowing the input of a list of Pose2d waypoints. + * @param waypoints the waypoints that the spline should intersect. + * @return the trajectory. + */ + public Trajectory generateQuinticTrajectory(List waypoints){ + return TrajectoryGenerator.generateTrajectory(waypoints, pathConfig); + } +}