diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4d990c86..301b46e8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,7 @@ public static final class RobotMap { public static final int DRIVER_PORT = 0; public static final int OPERATOR_PORT = 1; - + public static final int SHOOTER_LEFT_FLYWHEEL_ID = 4110; public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 4540; @@ -99,6 +99,8 @@ public static final class IntakeConstants { } public static final class FeederConstants { + public static final PIDFFGains FEEDER_GAINS = + PIDFFGains.builder().name("Feeder Controller").kP(0.0).kD(0.0).build(); // TODO: FIX public static final double GERING = 1.; public static final double MAX_RPM = 5000; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6c43a126..df4e9576 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,15 +23,15 @@ import frc.robot.commands.otf.RotateScore; import frc.robot.subsystems.elevatorIO.Elevator; import frc.robot.subsystems.elevatorIO.ElevatorIOSim; -import frc.robot.subsystems.shooterIO.Shooter; -import frc.robot.subsystems.shooterIO.ShooterIOSim; -import frc.robot.subsystems.shooterIO.ShooterIOVortex; import frc.robot.subsystems.feederIO.Feeder; import frc.robot.subsystems.feederIO.FeederIOSim; import frc.robot.subsystems.feederIO.FeederIOSparks; import frc.robot.subsystems.intakeIO.Intake; import frc.robot.subsystems.intakeIO.IntakeIOSim; import frc.robot.subsystems.intakeIO.IntakeIOSparks; +import frc.robot.subsystems.shooterIO.Shooter; +import frc.robot.subsystems.shooterIO.ShooterIOSim; +import frc.robot.subsystems.shooterIO.ShooterIOVortex; import frc.robot.subsystems.shooterPivot.ShooterPivot; import frc.robot.subsystems.shooterPivot.ShooterPivotIOSim; import frc.robot.subsystems.swerveIO.SwerveIOPigeon2; diff --git a/src/main/java/frc/robot/subsystems/feederIO/Feeder.java b/src/main/java/frc/robot/subsystems/feederIO/Feeder.java index 035e7b13..782631d0 100644 --- a/src/main/java/frc/robot/subsystems/feederIO/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feederIO/Feeder.java @@ -1,45 +1,39 @@ package frc.robot.subsystems.feederIO; -import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Robot; -import frc.robot.rhr.RHRPIDFFController; -import frc.robot.util.PIDFFGains; +import frc.robot.util.LoggableMotor; +import lombok.Getter; import org.littletonrobotics.junction.Logger; public class Feeder extends SubsystemBase { - private FeederIO io; + @Getter private LoggableMotor feederMotor; + + private FeederIO IO; private double target; private FeederInputsAutoLogged inputs; - private RHRPIDFFController controller = - new RHRPIDFFController(PIDFFGains.builder().kP(0.04).kD(0.).build()); - public Feeder(FeederIO io) { - this.io = io; + public Feeder(FeederIO IO) { + this.IO = IO; this.inputs = new FeederInputsAutoLogged(); + this.feederMotor = new LoggableMotor("Feeder", DCMotor.getNEO(1)); } public void setTarget(double targetRPM) { target = targetRPM; - // io.setSetpoint(targetRPM); + IO.setSetpoint(targetRPM); } @Override public void periodic() { - io.updateInputs(inputs); - var volts = controller.calculate(inputs.velocityRPM, target); - volts = MathUtil.clamp(volts, -12, 12); - - Logger.recordOutput("Feeder/Sent Volts", volts); + IO.updateInputs(inputs); Logger.recordOutput("Feeder/TargetRPM", target); Logger.recordOutput("Feeder/atTarget", atTarget()); - - io.setVoltage(volts); - Logger.processInputs("Feeder", inputs); } diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java index fe196ad0..36c56ec2 100644 --- a/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feederIO/FeederIO.java @@ -13,11 +13,11 @@ public static class FeederInputs { public double positionDeg = 0.0; } - // public void setSetpoint(double setpointRPM); + public void setSetpoint(double setpointRPM); public void setVoltage(double volts); - // public boolean atTarget(); + public boolean atTarget(); public void updateInputs(FeederInputsAutoLogged inputs); } diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java index 7cf3c97a..5cd0ad5a 100644 --- a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java +++ b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSim.java @@ -4,24 +4,32 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.Constants; +import frc.robot.Constants.FeederConstants; +import frc.robot.rhr.RHRPIDFFController; public class FeederIOSim implements FeederIO { + private double setpointRPM; DCMotorSim sim = new DCMotorSim( DCMotor.getNEO(1), Constants.FeederConstants.GERING, Constants.FeederConstants.MOI); private double volts; + RHRPIDFFController feederController; - public FeederIOSim() {} + public FeederIOSim() { + feederController = FeederConstants.FEEDER_GAINS.createRHRController(); + } @Override public void setVoltage(double volts) { this.volts = volts; sim.setInputVoltage(volts); } - // @Override - // public boolean atTarget() { - // return Math.abs(sim.getAngularVelocityRPM() -) > 0.01; - // } + + @Override + public boolean atTarget() { + return Math.abs(sim.getAngularVelocityRPM() - setpointRPM) < 0.01; + } + @Override public void updateInputs(FeederInputsAutoLogged inputs) { sim.update(0.02); @@ -32,4 +40,10 @@ public void updateInputs(FeederInputsAutoLogged inputs) { inputs.velocityRPM = sim.getAngularVelocityRPM(); inputs.positionDeg = Units.rotationsToDegrees(sim.getAngularPositionRotations()); } + + @Override + public void setSetpoint(double setpointRPM) { + feederController.setSetpoint(setpointRPM); + this.setpointRPM = setpointRPM; + } } diff --git a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java index 0175c704..b02bba50 100644 --- a/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java +++ b/src/main/java/frc/robot/subsystems/feederIO/FeederIOSparks.java @@ -1,32 +1,36 @@ package frc.robot.subsystems.feederIO; +import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.util.Units; +import frc.robot.Constants.FeederConstants; import frc.robot.Constants.RobotMap; public class FeederIOSparks implements FeederIO { CANSparkMax motor; - // double setpoint = 0; + double setpoint = 0; public FeederIOSparks() { motor = new CANSparkMax(RobotMap.FEEDER_CAN_ID, MotorType.kBrushless); + motor.getPIDController().setP(FeederConstants.FEEDER_GAINS.getKP()); + motor.getPIDController().setD(FeederConstants.FEEDER_GAINS.getKD()); } - // @Override - // public void setSetpoint(double setpointRPM) { - // setpoint = setpointRPM; - // } + @Override + public void setSetpoint(double setpointRPM) { + motor.getPIDController().setReference(setpointRPM, ControlType.kVelocity, 0); + } @Override public void setVoltage(double volts) { motor.setVoltage(volts); } - // @Override - // public boolean atTarget() { - // return Math.abs(motor.getEncoder().getVelocity() - setpoint) < 0.1; - // } + @Override + public boolean atTarget() { + return Math.abs(motor.getEncoder().getVelocity() - setpoint) < 0.1; + } @Override public void updateInputs(FeederInputsAutoLogged inputs) { diff --git a/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java index f6c43115..9c737563 100644 --- a/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java @@ -1,13 +1,19 @@ package frc.robot.subsystems.swerveIO; +import com.choreo.lib.Choreo; +import com.choreo.lib.ChoreoControlFunction; +import com.choreo.lib.ChoreoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; 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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; @@ -17,12 +23,21 @@ import edu.wpi.first.networktables.TimestampedDoubleArray; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; +import frc.robot.Robot; +import frc.robot.rhr.auto.RHRPathPlannerAuto; import frc.robot.subsystems.swerveIO.module.SwerveModule; import frc.robot.subsystems.swerveIO.module.SwerveModuleIO; +import frc.robot.util.ErrorTracker; import frc.robot.util.MotionHandler; +import frc.robot.util.PIDFFGains; +import frc.robot.util.SwerveHeadingController; import org.littletonrobotics.junction.Logger; public class SwerveSubsystem extends SubsystemBase { @@ -399,5 +414,88 @@ public void periodic() { Logger.recordOutput("Swerve/Desired speeds/r-radps", desiredSpeeds.omegaRadiansPerSecond); } - public static class Commands {} + public static class Commands { + public ChoreoControlFunction modifiedChoreoSwerveController( + PIDController xController, PIDController yController, PIDController rotationController) { + rotationController.enableContinuousInput(-Math.PI, Math.PI); + ErrorTracker errorTracker = + new ErrorTracker( + 10, + PIDFFGains.fromPIDGains(xController), + PIDFFGains.fromPIDGains(rotationController)); + return (pose, referenceState) -> { + Logger.recordOutput( + "Choreo/Target Pose", + new Pose2d( + new Translation2d(referenceState.x, referenceState.y), + Rotation2d.fromRadians(referenceState.heading))); + + var error = + new Pose2d( + new Translation2d(referenceState.x - pose.getX(), referenceState.y - pose.getY()), + Rotation2d.fromRadians(referenceState.heading - pose.getRotation().getRadians())); + + errorTracker.addObservation(error); + double xFF = referenceState.velocityX; + double yFF = referenceState.velocityY; + double rotationFF = referenceState.angularVelocity; + + double xFeedback = xController.calculate(pose.getX(), referenceState.x); + double yFeedback = yController.calculate(pose.getY(), referenceState.y); + double rotationFeedback = + rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading); + + return ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation()); + }; + } + + public static Command setHeading(Rotation2d degrees) { + return new InstantCommand(() -> SwerveHeadingController.getInstance().setSetpoint(degrees)); + } + + public static Command setHeadingandWait(Rotation2d degrees) { + return new SequentialCommandGroup( + setHeading(degrees), + new WaitUntilCommand(SwerveHeadingController.getInstance()::atSetpoint)); + } + + public static Command getAutonomousCommand(String trajectory) { + return new RHRPathPlannerAuto(trajectory); + } + + public static Command resetOdometry(Pose2d pose) { + return new InstantCommand(() -> Robot.swerveDrive.resetOdometry(pose)); + } + + public static Command resetOdometry(String trajectory) { + PathPlannerPath p = PathPlannerPath.fromChoreoTrajectory(trajectory); + return new InstantCommand( + () -> Robot.swerveDrive.resetOdometry(p.getPreviewStartingHolonomicPose())); + } + + public Command choreoCommandBuilder(ChoreoTrajectory traj) { + var alliance = DriverStation.getAlliance(); + boolean useAllianceColour = false; + if (alliance.isPresent()) { + useAllianceColour = alliance.get() == DriverStation.Alliance.Red; + } + + return new SequentialCommandGroup( + Choreo.choreoSwerveCommand( + traj, // + Robot.swerveDrive::getUsablePose, // + modifiedChoreoSwerveController( + new PIDController(3, 0.0, 0.0), // + new PIDController(3, 0.0, 0.0), // + new PIDController(3, 0.0, 0.0)), + (ChassisSpeeds speeds) -> { + Robot.swerveDrive.setDesiredChassisSpeeds(speeds); + }, + useAllianceColour, + Robot.swerveDrive // + ), + new InstantCommand(() -> Robot.swerveDrive.setDesiredChassisSpeeds(new ChassisSpeeds()))); + } + } } diff --git a/src/main/java/frc/robot/util/SwerveHeadingController.java b/src/main/java/frc/robot/util/SwerveHeadingController.java index 2ea36581..2a516375 100644 --- a/src/main/java/frc/robot/util/SwerveHeadingController.java +++ b/src/main/java/frc/robot/util/SwerveHeadingController.java @@ -64,6 +64,9 @@ public Rotation2d getSetpoint() { return setpoint; } + public boolean atSetpoint() { + return this.error < 0.1; + } /** * Updates the heading controller PID with the setpoint and calculates output. * @@ -77,6 +80,7 @@ public double update() { Logger.recordOutput("Heading Controller/setpoint", setpoint.getDegrees()); double output = 0; + if (!controller.atSetpoint()) { Rotation2d currentHeading = Robot.swerveDrive.getYaw(); output = controller.calculate(currentHeading.getDegrees(), setpoint.getDegrees());