diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 62b0311..de2804b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -10,8 +10,11 @@ import com.stuypulse.robot.subsystems.OnBoardIO; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.*; +import com.stuypulse.stuylib.input.gamepads.keyboard.SimKeyGamepad; +import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj2.command.Command; /** @@ -24,7 +27,9 @@ public class RobotContainer { // Assumes a gamepad plugged into channnel 0 - private final Gamepad driver = new AutoGamepad(0); + private final Gamepad driver = + // new AutoGamepad(0); + new SimKeyGamepad(); // The robot's subsystems and commands are defined here... private final Drivetrain drivetrain = new Drivetrain(); @@ -45,6 +50,21 @@ private void configureButtonBindings() { driver.getBottomButton().whileHeld(new DrivetrainSpinCommand(drivetrain)); driver.getRightButton().whenPressed(new DrivetrainResetCommand(drivetrain)); + // Test command + driver.getLeftButton().whileHeld(new DrivetrainGotoCommand( + drivetrain, + + // Target + new SmartNumber("Goto X", 0), + new SmartNumber("Goto Y", 0), + new SmartNumber("Goto (deg)", 0), + + new TrajectoryConfig( + 2, // max velocity + 2 // max acceleration + ) + )); + onBoardIO.getButtonA().whenPressed(getAutonomousCommand()); } diff --git a/src/main/java/com/stuypulse/robot/commands/DrivetrainGotoCommand.java b/src/main/java/com/stuypulse/robot/commands/DrivetrainGotoCommand.java new file mode 100644 index 0000000..a6e5aac --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/DrivetrainGotoCommand.java @@ -0,0 +1,88 @@ +package com.stuypulse.robot.commands; + +import java.util.List; + +import com.stuypulse.robot.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; + +public class DrivetrainGotoCommand extends FollowerCommand { + + private final Pose2d target; + + // this is really messy, i might as well not have a Pose2d target + // and just always take in 3 target values + private final Number targetX, targetY, targetDeg; + + private final TrajectoryConfig config; + + // Base constructor - not good to use + private DrivetrainGotoCommand( + Drivetrain drivetrain, + Pose2d target, + Number targetX, Number targetY, Number targetDeg, + TrajectoryConfig config + ) { + // Don't give the follower command a trajectory (until later) + super(drivetrain, null); + + // Some of these will be used to create a trajectory + this.target = target; + + this.targetX = targetX; + this.targetY = targetY; + this.targetDeg = targetDeg; + + this.config = config; + } + + public DrivetrainGotoCommand(Drivetrain drivetrain, Pose2d target, TrajectoryConfig config) { + this(drivetrain, target, null, null, null, config); + } + + public DrivetrainGotoCommand( + Drivetrain drivetrain, + Number targetX, Number targetY, Number targetDeg, + TrajectoryConfig config + ) { + this(drivetrain, null, targetX, targetY, targetDeg, config); + } + + // Gets a pose2d to go to + private Pose2d getTarget() { + if (target != null) { + return target; + } + + return new Pose2d( + targetX.doubleValue(), + targetY.doubleValue(), + Rotation2d.fromDegrees(targetDeg.doubleValue()) + ); + } + + // Gets a trajectory that starts at the drivetrain's current position + private Trajectory getTrajectory() { + // Create the trajectory in the beginning + Pose2d where = drivetrain.getPose(); + return TrajectoryGenerator.generateTrajectory( + List.of( + where, + getTarget() + ), + config + ); + } + + @Override + public void initialize() { + // Update the trajectory, then initialize the follower command + this.trajectory = getTrajectory(); + super.initialize(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/FollowerCommand.java b/src/main/java/com/stuypulse/robot/commands/FollowerCommand.java new file mode 100644 index 0000000..b594bc4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/FollowerCommand.java @@ -0,0 +1,225 @@ +package com.stuypulse.robot.commands; + +import com.stuypulse.robot.Constants; +import com.stuypulse.robot.Constants.Drivetrain.Motion; + +import com.stuypulse.robot.subsystems.Drivetrain; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.PIDController; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * This is a remake of the edu.wpi.first.wpilibj2.command.RamseteCommand, + * but easier to use with child classes for testing, research, etc. + */ +public class FollowerCommand extends CommandBase { + + // Helper class for storing voltages + private static class DifferentialDriveVoltages { + + public final double leftVoltage, rightVoltage; + + public DifferentialDriveVoltages(double leftVoltage, double rightVoltage) { + this.leftVoltage = leftVoltage; + this.rightVoltage = rightVoltage; + } + + } + + /**************** + * CLASS FIELDS * + ****************/ + + // Drivetrain that follows the path + protected Drivetrain drivetrain; + + // Trajectory to follow, supplies velocities + protected Trajectory trajectory; + + // Calculates fwd, angular velocity based on a goal position + protected RamseteController follower; + + // Converts ideal velocities into wheel speeds + protected DifferentialDriveKinematics kinematics; + + // Converts velocities into voltages + protected SimpleMotorFeedforward feedforward; + + // Used to fix up voltages based on the setpoint + // and what the drivetrain encoders read + protected Controller leftController; + protected Controller rightController; + + // Stopwatch is used to measure time, sample trajectory + protected StopWatch stopwatch; + + // Previous time is used to calculate dT + // dT is used to calculate acceleration + private double previousTime; + + // Previous speeds are used to calculate acceleration + private DifferentialDriveWheelSpeeds previousSpeeds; + + /**************** + * CONSTRUCTORS * + ****************/ + + public FollowerCommand( + Drivetrain drivetrain, + Trajectory trajectory, + RamseteController follower, + SimpleMotorFeedforward feedforward, + DifferentialDriveKinematics kinematics, + Controller leftController, + Controller rightController + ) { + this.drivetrain = drivetrain; + this.trajectory = trajectory; + this.feedforward = feedforward; + this.follower = follower; + this.kinematics = kinematics; + this.leftController = leftController; + this.rightController = rightController; + + this.stopwatch = new StopWatch(); + + this.previousTime = 0.0; + this.previousSpeeds = null; + + addRequirements(drivetrain); + } + + public FollowerCommand( + Drivetrain drivetrain, + Trajectory trajectory + ) { + this( + drivetrain, + trajectory, + new RamseteController(), + Motion.MOTOR_FEED_FORWARD, + Motion.KINEMATICS, + new PIDController(Motion.PID.P, Motion.PID.I, Motion.PID.D), + new PIDController(Motion.PID.P, Motion.PID.I, Motion.PID.D) + ); + } + + + /*************************** + * COMMAND BASED FUNCTIONS * + ***************************/ + + @Override + public void initialize() { + // Initialize the previous time + this.previousTime = -1; + + // Initialize previous speeds + Trajectory.State initial = trajectory.sample(0); + this.previousSpeeds = kinematics.toWheelSpeeds( + new ChassisSpeeds( + initial.velocityMetersPerSecond, + 0, + initial.curvatureRadPerMeter * initial.velocityMetersPerSecond + ) + ); + + // Reset timer + this.stopwatch.reset(); + } + + @Override + public void execute() { + // Get the current time and dT + final double currentTime = stopwatch.getTime(); + final double dt = currentTime - previousTime; + + // If first time in the execute loop, do nothing + if (previousTime < 0) { + drivetrain.tankDriveVolts(0, 0); + previousTime = currentTime; + return; + } + + // Get target wheel speeds (with the ramsete controller) + DifferentialDriveWheelSpeeds targetSpeeds = kinematics.toWheelSpeeds( + follower.calculate( + drivetrain.getPose(), + trajectory.sample(currentTime) + ) + ); + + // Get the voltages for each wheel + DifferentialDriveVoltages voltages = getVoltages( + targetSpeeds, + dt + ); + + // Use voltage outputs + drivetrain.tankDriveVolts( + voltages.leftVoltage, + voltages.rightVoltage + ); + + // Update previous speeds and time + previousSpeeds = targetSpeeds; + previousTime = currentTime; + } + + @Override + public boolean isFinished() { + return stopwatch.getTime() > trajectory.getTotalTimeSeconds(); + } + + /******************** + * HELPER FUNCTIONS * + ********************/ + + private DifferentialDriveVoltages getVoltages( + DifferentialDriveWheelSpeeds target, + double dt + ) { + + // Left, right desired velocity + double left = target.leftMetersPerSecond; + double right = target.rightMetersPerSecond; + + // Left, right (raw) voltage + double leftVoltage = feedforward.calculate( + // velocity + left, + // acceleration + (left - previousSpeeds.leftMetersPerSecond) / dt + ); + double rightVoltage = feedforward.calculate( + // velocity + right, + // acceleration + (right - previousSpeeds.rightMetersPerSecond) / dt + ); + + // Left, right voltages PID corrected + DifferentialDriveWheelSpeeds measurement = drivetrain.getWheelSpeeds(); + leftVoltage += leftController.update( + left - measurement.leftMetersPerSecond + ); + rightVoltage += rightController.update( + right - measurement.rightMetersPerSecond + ); + + return new DifferentialDriveVoltages( + leftVoltage, + rightVoltage + ); + } + + +}