diff --git a/.gitattributes b/.gitattributes index 81c99a3d..dbb93265 100644 --- a/.gitattributes +++ b/.gitattributes @@ -3,3 +3,8 @@ src/main/deploy/photon-configs/front-right.zip filter=lfs diff=lfs merge=lfs -text src/main/deploy/photon-configs/back-left.zip filter=lfs diff=lfs merge=lfs -text src/main/deploy/photon-configs/front-left.zip filter=lfs diff=lfs merge=lfs -text +customAssets/Robot_Pumbaa/model_0.glb filter=lfs diff=lfs merge=lfs -text +customAssets/Robot_Pumbaa/model_1.glb filter=lfs diff=lfs merge=lfs -text +customAssets/Robot_Pumbaa/model_2.glb filter=lfs diff=lfs merge=lfs -text +customAssets/Robot_Pumbaa/model_3.glb filter=lfs diff=lfs merge=lfs -text +customAssets/Robot_Pumbaa/model.glb filter=lfs diff=lfs merge=lfs -text diff --git a/checks.xml b/checks.xml index f4f41c54..67cbaef5 100644 --- a/checks.xml +++ b/checks.xml @@ -319,7 +319,7 @@ value="^@return the *|^This method returns |^A [{]@code [_a-zA-Z0-9]+[}]( is a )"/> --> - + diff --git a/customAssets/Robot_Pumbaa/config.json b/customAssets/Robot_Pumbaa/config.json new file mode 100644 index 00000000..564f68cf --- /dev/null +++ b/customAssets/Robot_Pumbaa/config.json @@ -0,0 +1,70 @@ +{ + "name": "Pumbaa", + "sourceUrl": "", + "rotations": [ + { + "axis": "x", + "degrees": 90.0 + } + ], + "position": [ + 0.0, + 0.0, + 0.0 + ], + "cameras": [], + "components": [ + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90.0 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90.0 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90.0 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + }, + { + "zeroedRotations": [ + { + "axis": "x", + "degrees": 90.0 + } + ], + "zeroedPosition": [ + 0.0, + 0.0, + 0.0 + ] + } + ] +} \ No newline at end of file diff --git a/customAssets/Robot_Pumbaa/model.glb b/customAssets/Robot_Pumbaa/model.glb new file mode 100644 index 00000000..f6fda867 --- /dev/null +++ b/customAssets/Robot_Pumbaa/model.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b1434fdffb3655b8016142a134caad95d3a9208c16b73420a95b596fdf48a933 +size 155297860 diff --git a/customAssets/Robot_Pumbaa/model_0.glb b/customAssets/Robot_Pumbaa/model_0.glb new file mode 100644 index 00000000..b83dd5db --- /dev/null +++ b/customAssets/Robot_Pumbaa/model_0.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d2980a266ef0815bc85801cebc8afaf4b2fbe3e0c9ce78147af8ed0586da799a +size 17705300 diff --git a/customAssets/Robot_Pumbaa/model_1.glb b/customAssets/Robot_Pumbaa/model_1.glb new file mode 100644 index 00000000..2f9401ba --- /dev/null +++ b/customAssets/Robot_Pumbaa/model_1.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7159e23ea95b6cb23a26fc7dbfeae2c838f179d863029329e036068c28d9b803 +size 8197628 diff --git a/customAssets/Robot_Pumbaa/model_2.glb b/customAssets/Robot_Pumbaa/model_2.glb new file mode 100644 index 00000000..c5b4fc5f --- /dev/null +++ b/customAssets/Robot_Pumbaa/model_2.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:076f93cc6a59cbd4131e6ec9119fbe0f6a33373a84098c31a6a9cd2701b85a18 +size 12731932 diff --git a/customAssets/Robot_Pumbaa/model_3.glb b/customAssets/Robot_Pumbaa/model_3.glb new file mode 100644 index 00000000..bd16167b --- /dev/null +++ b/customAssets/Robot_Pumbaa/model_3.glb @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:69022c28202e78674de9d5f4f312171c77fd2b3e2ed7e5b734dad12a79b0fdb3 +size 52648 diff --git a/src/main/java/frc/lib/sim/SimulatedArena.java b/src/main/java/frc/lib/sim/SimulatedArena.java new file mode 100644 index 00000000..7ee99f74 --- /dev/null +++ b/src/main/java/frc/lib/sim/SimulatedArena.java @@ -0,0 +1,144 @@ +package frc.lib.sim; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashSet; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; +import java.util.stream.Stream; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import frc.lib.util.FieldConstants; + +/** + * Simulated Arena + */ +public class SimulatedArena { + + /** + * Shooting Note + */ + private static class ShotNote { + public Pose3d pose; + ArrayList trajectory = new ArrayList<>(); + double t = 0.0; + + /** + * Shooting Note + * + * @param startPose Starting Pose + * @param yaw Yaw + * @param pitch Pitch + * @param velocity Velocity + */ + public ShotNote(Pose3d startPose, Rotation2d yaw, Rotation2d pitch, double velocity) { + this.pose = startPose; + double vz = pitch.getSin() * velocity; + double vx = yaw.getCos() * velocity; + double vy = yaw.getSin() * velocity; + double prevZ = startPose.getZ(); + for (double t = 0.0; prevZ > 0.0; t += LoggedRobot.defaultPeriodSecs) { + double x = startPose.getX() + vx * t; + double y = startPose.getY() + vy * t; + double z = startPose.getZ() + vz * t - 9.81 * t * t; + trajectory.add(new Pose3d(x, y, z, new Rotation3d())); + prevZ = z; + } + } + + /** + * Update Viz + */ + public void update(double dt) { + int index = (int) Math.floor(t / LoggedRobot.defaultPeriodSecs); + if (index >= trajectory.size()) { + pose = trajectory.get(trajectory.size() - 1); + pose = new Pose3d(pose.getX(), pose.getY(), 0.0, new Rotation3d()); + } else { + pose = trajectory.get(index); + } + t += dt; + } + } + + private static final double NOTE_HEIGHT = Units.inchesToMeters(1.7); + private Set robots = new HashSet<>(); + private int id = 0; + private static final Rotation3d NO_ROT = new Rotation3d(); + private List notes = Stream + .concat( + Stream.concat(Arrays.stream(FieldConstants.StagingLocations.centerlineTranslations), + Arrays.stream(FieldConstants.StagingLocations.spikeTranslations)), + Arrays.stream(FieldConstants.StagingLocations.spikeTranslations) + .map((p) -> FieldConstants.allianceFlip(p))) + .map((pos) -> new Pose3d(new Translation3d(pos.getX(), pos.getY(), NOTE_HEIGHT), NO_ROT)) + .collect(Collectors.toList()); + public List shotNotes = new ArrayList<>(); + + /** + * Create a new simulated robot + * + * @return Simulated Robot + */ + public SimulatedPumbaa newPumbaa() { + SimulatedPumbaa dt = new SimulatedPumbaa(this.id++); + robots.add(dt); + return dt; + } + + /** + * Update Viz + * + * @param dt Change in time + */ + public void update(double dt) { + robots: for (SimulatedPumbaa robot : this.robots) { + Logger.recordOutput("Viz/Robot" + robot.id, robot.getPose()); + robot.advanceNote(dt, this); + if (robot.couldIntake()) { + System.out.println("checking"); + for (int i = 0; i < notes.size(); i++) { + double distance = notes.get(i).toPose2d().getTranslation() + .minus(robot.getPose().getTranslation()).getNorm(); + if (distance < 0.6) { + notes.remove(i); + robot.intakeOneNote(); + continue robots; + } + } + } + } + for (int i = 0; i < shotNotes.size(); i++) { + shotNotes.get(i).update(dt); + if (shotNotes.get(i).pose.getZ() < NOTE_HEIGHT) { + notes.add(new Pose3d(shotNotes.get(i).pose.getX(), shotNotes.get(i).pose.getY(), + NOTE_HEIGHT, NO_ROT)); + shotNotes.remove(i); + i--; + continue; + } + } + Logger.recordOutput("Viz/Notes", this.notes.toArray(Pose3d[]::new)); + Logger.recordOutput("Viz/ShotNotes", + this.shotNotes.stream().map((shotNote) -> shotNote.pose).toArray(Pose3d[]::new)); + } + + /** + * Shoot note + * + * @param pose Shooting Pose + * @param yaw Shooting Yaw + * @param pitch Shooting Pitch + * @param velocity Shooting Velocity + */ + void shootNote(Pose3d pose, Rotation2d yaw, Rotation2d pitch, double velocity) { + this.shotNotes.add(new ShotNote(pose, yaw, pitch, velocity)); + } + +} diff --git a/src/main/java/frc/lib/sim/SimulatedPumbaa.java b/src/main/java/frc/lib/sim/SimulatedPumbaa.java new file mode 100644 index 00000000..78b61b2f --- /dev/null +++ b/src/main/java/frc/lib/sim/SimulatedPumbaa.java @@ -0,0 +1,124 @@ +package frc.lib.sim; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + +/** + * Simulated Pumbaa + */ +public class SimulatedPumbaa { + + public final int id; + + /** + * Simulated Pumbaa + */ + SimulatedPumbaa(int id) { + this.id = id; + } + + private static final double SHOOTER_DIV = 100.0; + private static final double SHOOTER_FRONT = 0.271162; + private Pose2d pose = new Pose2d(); + private double intake = 0.0; + private double indexer = 0.0; + private double notePosition = 0.9; + private boolean hasNote = true; + private double shooterSpeed = 0.0; + private double height = 24.0; + private Rotation2d wristAngle = new Rotation2d(); + + public double notePosition() { + return this.notePosition; + } + + public void setShooterSpeed(double speed) { + this.shooterSpeed = speed; + } + + public void setElevatorWrist(double height, Rotation2d wristAngle) { + this.height = height; + this.wristAngle = wristAngle; + } + + public Pose2d getPose() { + return pose; + } + + public void setPose(Pose2d pose) { + this.pose = pose; + } + + public boolean couldIntake() { + return intake > 0.2; + } + + public void setIntake(double value) { + this.intake = value; + } + + public void setIndexer(double value) { + this.indexer = value; + } + + public void intakeOneNote() { + this.notePosition = 0.0; + this.hasNote = true; + } + + /** + * Get Shot Pose + * + * @return Pose shooting from + */ + public Pose3d getShootFrom() { + Translation3d t = new Translation3d( + this.getPose().getX() + SHOOTER_FRONT * this.getPose().getRotation().getCos(), + this.getPose().getY() + SHOOTER_FRONT * this.getPose().getRotation().getSin(), + Units.inchesToMeters(height)); + Rotation3d r = + new Rotation3d(0.0, wristAngle.getRadians(), this.getPose().getRotation().getRadians()); + return new Pose3d(t, r); + } + + /** + * Advance Note + * + * @param dt Change in time + * @param arena Simulated Arena + */ + public void advanceNote(double dt, SimulatedArena arena) { + Rotation2d yaw = this.getPose().getRotation(); + Rotation2d pitch = this.wristAngle; + if (hasNote) { + if (notePosition < 0.85) { + notePosition += intake * dt; + } else if (indexer < -0.2) { + this.hasNote = false; + arena.shootNote(new Pose3d(this.getPose()), yaw, Rotation2d.fromDegrees(0.0), 0.0); + } else if (notePosition > 1.0) { + this.hasNote = false; + arena.shootNote(this.getShootFrom(), yaw, pitch, this.shooterSpeed / SHOOTER_DIV); + } else { + notePosition += indexer * dt; + } + } + } + + public boolean hasNote() { + return this.hasNote; + } + + public boolean lowerBeamBreak() { + return hasNote && notePosition < 0.8; + } + + public boolean upperBeamBreak() { + return hasNote && notePosition > 0.9 && notePosition < 1.0; + } + +} diff --git a/src/main/java/frc/lib/util/photon/PhotonIO.java b/src/main/java/frc/lib/util/photon/PhotonIO.java index 31503fdb..a5d7ca90 100755 --- a/src/main/java/frc/lib/util/photon/PhotonIO.java +++ b/src/main/java/frc/lib/util/photon/PhotonIO.java @@ -93,6 +93,12 @@ public Optional> getDistCoeffs() { protected final String name; protected final String ip; + /** + * Photon IO + * + * @param name Camera Name + * @param ip Camera IP + */ public PhotonIO(String name, String ip) { this.name = name; this.ip = ip; diff --git a/src/main/java/frc/lib/util/photon/PhotonReal.java b/src/main/java/frc/lib/util/photon/PhotonReal.java index bf1b9609..994351f4 100755 --- a/src/main/java/frc/lib/util/photon/PhotonReal.java +++ b/src/main/java/frc/lib/util/photon/PhotonReal.java @@ -81,6 +81,12 @@ public PhotonPipelineResultIntermediate unpack(Packet packet) { } + /** + * Upload Settings + * + * @param ip Camera IP + * @param file Camera settings file + */ public boolean uploadSettings(String ip, File file) throws IOException { try (final CloseableHttpClient httpClient = HttpClients.createDefault()) { HttpPost postReq = new HttpPost("http://" + ip + "/api/settings"); diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index 0378b215..492e9055 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -1,13 +1,10 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.math.Conversions; import frc.robot.Constants; import frc.robot.Robot; @@ -23,27 +20,13 @@ public class SwerveModule { private SwerveModuleIO io; private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged(); - private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward( - Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA); - - /* drive motor control requests */ - private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); - private final VelocityVoltage driveVelocity = new VelocityVoltage(0); - - /* angle motor control requests */ - private final PositionVoltage anglePosition = new PositionVoltage(0); - /** * Swerve Module * * @param moduleNumber Module Number - * @param driveMotorID CAN ID of the Drive Motor - * @param angleMotorID CAN ID of the Angle Motor - * @param cancoderID CAN ID of the CANCoder * @param angleOffset Angle Offset of the CANCoder to align the wheels */ - public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, - Rotation2d angleOffset, SwerveModuleIO io) { + public SwerveModule(int moduleNumber, Rotation2d angleOffset, SwerveModuleIO io) { this.io = io; this.moduleNumber = moduleNumber; @@ -75,8 +58,12 @@ public void periodic() { */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { desiredState = SwerveModuleState.optimize(desiredState, getState().angle); - io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations())); + io.setAngleMotor(desiredState.angle.getRotations()); setSpeed(desiredState, isOpenLoop); + SmartDashboard.putNumber("desired state speed/" + moduleNumber, + desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("desired state angle/" + moduleNumber, + desiredState.angle.getDegrees()); } /** @@ -87,16 +74,10 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) */ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { if (isOpenLoop) { - driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; - io.setDriveMotor(driveDutyCycle); - inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + double power = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed; + io.setDriveMotorPower(power); } else { - driveVelocity.Velocity = Conversions.metersPerSecondToRotationPerSecond( - desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference); - driveVelocity.FeedForward = - driveFeedForward.calculate(desiredState.speedMetersPerSecond); - io.setDriveMotor(driveVelocity); - inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output; + io.setDriveMotor(desiredState.speedMetersPerSecond); } } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index cbfb1530..011a051c 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -1,7 +1,6 @@ package frc.lib.util.swerve; import org.littletonrobotics.junction.AutoLog; -import com.ctre.phoenix6.controls.ControlRequest; /** IO Class for SwerveModule */ public interface SwerveModuleIO { @@ -12,20 +11,23 @@ public static class SwerveModuleInputs { public double driveMotorSelectedSensorVelocity; public double angleMotorSelectedPosition; public double absolutePositionAngleEncoder; + public double[] odometryTimestamps; // public double driveMotorTemp; // public double angleMotorTemp; } + public default void setModNumber(int number) {} + public default void updateInputs(SwerveModuleInputs inputs) {} - public default void setDriveMotor(ControlRequest request) {} + public default void setDriveMotor(double mps) {} + + public default void setDriveMotorPower(double power) {} - public default void setAngleMotor(ControlRequest request) {} + public default void setAngleMotor(double angle) {} public default void setAngleSelectedSensorPosition(double angle) {} public default void setPositionAngleMotor(double absolutePosition) {} - - } diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 2084f621..517f1077 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -4,11 +4,13 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import edu.wpi.first.math.geometry.Rotation2d; +import frc.lib.math.Conversions; import frc.robot.Constants; /** @@ -28,9 +30,15 @@ public class SwerveModuleReal implements SwerveModuleIO { private StatusSignal angleMotorSelectedPosition; private StatusSignal absolutePositionAngleEncoder; + /* drive motor control requests */ + private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); + private final VelocityVoltage driveVelocity = new VelocityVoltage(0); + + /* angle motor control requests */ + private final PositionVoltage anglePosition = new PositionVoltage(0); + /** Instantiating motors and Encoders */ - public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, - Rotation2d angleOffset) { + public SwerveModuleReal(int driveMotorID, int angleMotorID, int cancoderID) { angleEncoder = new CANcoder(cancoderID, "canivore"); mDriveMotor = new TalonFX(driveMotorID, "canivore"); @@ -55,7 +63,6 @@ private void configAngleMotor() { /* Gear Ratio and Wrapping Config */ swerveAngleFXConfig.Feedback.FeedbackRemoteSensorID = angleEncoder.getDeviceID(); swerveAngleFXConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - // swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; swerveAngleFXConfig.Feedback.SensorToMechanismRatio = 1.0; swerveAngleFXConfig.Feedback.RotorToSensorRatio = Constants.Swerve.angleGearRatio; swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; @@ -99,6 +106,9 @@ private void configDriveMotor() { swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; + swerveDriveFXConfig.Slot0.kS = Constants.Swerve.driveKS; + swerveDriveFXConfig.Slot0.kV = Constants.Swerve.driveKV; + swerveDriveFXConfig.Slot0.kA = Constants.Swerve.driveKA; /* Open and Closed Loop Ramping */ swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = @@ -122,13 +132,17 @@ private void configAngleEncoder() { } @Override - public void setAngleMotor(ControlRequest request) { - mAngleMotor.setControl(request); + public void setAngleMotor(double angle) { + mAngleMotor.setControl(anglePosition.withPosition(angle)); } @Override - public void setDriveMotor(ControlRequest request) { - mDriveMotor.setControl(request); + public void setDriveMotor(double mps) { + // driveVelocity.FeedForward = feedforward; + double driveRPS = Conversions.metersPerSecondToRotationPerSecond(mps, + Constants.Swerve.wheelCircumference); + driveVelocity.Velocity = driveRPS; + mDriveMotor.setControl(driveVelocity); } @Override diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java new file mode 100644 index 00000000..7cc8166b --- /dev/null +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleSim.java @@ -0,0 +1,92 @@ +package frc.lib.util.swerve; + +import org.littletonrobotics.junction.LoggedRobot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.lib.math.Conversions; +import frc.robot.Constants; + +/** + * Swerve Module Sim + */ +public class SwerveModuleSim implements SwerveModuleIO { + public int moduleNumber; + + private FlywheelSim driveSim = + new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, 0.025); + + private double angle; + private double distance; + + private double driveAppliedVolts = 0.0; + private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + private PIDController driveFeedback = new PIDController(0.5, 0.0, 0.0); + + /** + * Swerve Module Sim + */ + public SwerveModuleSim() {} + + @Override + public void setModNumber(int number) { + this.moduleNumber = number; + } + + @Override + public void updateInputs(SwerveModuleInputs inputs) { + driveSim.update(LoggedRobot.defaultPeriodSecs); + double driveSpeed = Units.radiansToRotations(driveSim.getAngularVelocityRadPerSec()); + this.distance += driveSpeed * LoggedRobot.defaultPeriodSecs; + inputs.driveMotorSelectedPosition = this.distance; + inputs.driveMotorSelectedSensorVelocity = driveSpeed; + + inputs.angleMotorSelectedPosition = angle; + + inputs.absolutePositionAngleEncoder = angle; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + } + + /** + * Set drive motor in Meter per Sec + * + * @param mps Meters per Second + */ + public void setDriveMotor(double mps) { + double rpm = Conversions.metersPerSecondToRotationPerSecond(mps, + Constants.Swerve.wheelCircumference); + driveFeedback.setSetpoint(rpm); + double driveFF = driveFeedforward.calculate(mps); + SmartDashboard.putNumber("ff/" + moduleNumber, driveFF); + double volts = driveFeedback.calculate(mps) + driveFF; + if (rpm == 0) { + volts = 0; + } + SmartDashboard.putNumber("Drive volts/" + moduleNumber, volts); + setDriveVoltage(volts); + } + + /** + * Set Angle for steering motor + * + * @param angle Angle to set + */ + public void setAngleMotor(double angle) { + this.angle = angle; + } + + /** + * Set Drive Voltage + * + * @param volts Voltage + */ + public void setDriveVoltage(double volts) { + driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + driveSim.setInputVoltage(driveAppliedVolts); + } +} diff --git a/src/main/java/frc/lib/viz/PumbaaViz.java b/src/main/java/frc/lib/viz/PumbaaViz.java new file mode 100644 index 00000000..08b997db --- /dev/null +++ b/src/main/java/frc/lib/viz/PumbaaViz.java @@ -0,0 +1,186 @@ +package frc.lib.viz; + +import java.util.ArrayList; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import frc.lib.sim.SimulatedPumbaa; +import frc.robot.Constants; + +/** + * Pumbaa Viz + */ +public class PumbaaViz { + + private final String prefix; + private Pose3d elevatorBottomPose; + private Pose3d elevatorTopPose; + private Pose3d shooterPose; + private Pose3d notePose; + private NoteLocation noteLocation = NoteLocation.None; + private final SimulatedPumbaa sim; + private Rotation2d wristAngle; + private double height; + private Pose2d robotPose = new Pose2d(); + + private final Rotation3d no_rot = new Rotation3d(); + private final Pose3d no_note_pose = new Pose3d(0, 0, -100.0, no_rot); + private final Rotation3d intake_rotation = + new Rotation3d(0.0, Units.degreesToRadians(-73.2153), 0.0); + private final Pose3d intake_pose = new Pose3d(-0.342037, 0.0, 0.205429, + new Rotation3d(0.0, Units.degreesToRadians(-73.2153), 0.0)); + private final Translation3d intake_top_pose = new Translation3d(-0.295609, 0.0, 0.376448); + private final Translation3d intake_bottom_pose = new Translation3d(-0.380825, 0.0, 0.071435); + private final Translation3d intake_dir = intake_top_pose.minus(intake_bottom_pose); + + private final double shooterHeight = 0.538339; + private final double indexerBack = 0.150388; + + /** + * Pumba Viz + * + * @param prefix Prefix + * @param sim Simulation + */ + public PumbaaViz(String prefix, SimulatedPumbaa sim) { + this.prefix = prefix; + this.sim = sim; + this.elevatorBottomPose = new Pose3d(); + this.elevatorTopPose = new Pose3d(); + } + + /** + * Set Elevator and Wrist Position + * + * @param height Height of elevator + * @param wristAngle Wrist angle + */ + public void setElevatorWrist(double height, Rotation2d wristAngle) { + this.height = height; + height = + Units.inchesToMeters(height - Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT); + this.wristAngle = wristAngle; + shooterPose = new Pose3d(new Translation3d(0.0, 0.0, shooterHeight + height), + new Rotation3d(0.0, -wristAngle.getRadians(), 0.0)); + elevatorBottomPose = new Pose3d(0, 0, height / 2.0, no_rot); + elevatorTopPose = new Pose3d(0, 0, height, no_rot); + if (noteLocation == NoteLocation.Shooter) { + double indexerBackX = indexerBack * wristAngle.getCos(); + notePose = shooterPose.plus(new Transform3d(-indexerBackX, 0, 0, no_rot)); + } + } + + /** + * Set Robot Pose + * + * @param pose Position of robot + */ + public void setPose(Pose2d pose) { + this.robotPose = pose; + } + + /** + * Note Location + */ + public static enum NoteLocation { + Intake, Shooter, None + } + + /** + * Set Note Location + */ + public void setNoteLocation(NoteLocation location) { + if (sim != null) { + return; + } + switch (location) { + case Intake: + this.notePose = intake_pose; + break; + case Shooter: + break; + default: + this.notePose = no_note_pose; + break; + } + this.noteLocation = location; + } + + private static final double SHOOTER_FRONT = 0.271162; + + /** + * Get Shotting Position + * + * @return Shooting Pose + */ + public Pose3d getShootFrom() { + Translation3d t = new Translation3d( + this.robotPose.getX() + SHOOTER_FRONT * this.robotPose.getRotation().getCos(), + this.robotPose.getY() + SHOOTER_FRONT * this.robotPose.getRotation().getSin(), + Units.inchesToMeters(height)); + Rotation3d r = + new Rotation3d(0.0, wristAngle.getRadians(), this.robotPose.getRotation().getRadians()); + return new Pose3d(t, r); + } + + ArrayList trajectory = new ArrayList<>(); + + /** + * Update Viz + */ + public void update() { + if (this.sim != null) { + if (this.sim.hasNote()) { + double notePosition = this.sim.notePosition(); + if (notePosition < 0.8) { + double notePositionPercent = notePosition / 0.85; + notePose = + new Pose3d(intake_bottom_pose.plus(intake_dir.times(notePositionPercent)), + intake_rotation); + } else if (notePosition < 0.9) { + double notePositionPercent = (notePosition - 0.8) / 0.1; + double indexerBackX = indexerBack * wristAngle.getCos(); + Pose3d topPose = shooterPose.plus(new Transform3d(-indexerBackX, 0, 0, no_rot)); + Pose3d bottomPose = new Pose3d(intake_top_pose, intake_rotation); + notePose = bottomPose.interpolate(topPose, notePositionPercent); + } else { + double notePositionPercent = (notePosition - 0.9) / 0.1; + double indexerBackX = + indexerBack * -(notePositionPercent * 2.0 - 1.0) * wristAngle.getCos(); + notePose = shooterPose.plus(new Transform3d(-indexerBackX, 0, 0, no_rot)); + } + } else { + this.notePose = no_note_pose; + } + } + recalculateTrajectory(); + Logger.recordOutput(prefix + "/trajectory", trajectory.toArray(Pose3d[]::new)); + Logger.recordOutput(prefix + "/components", + new Pose3d[] {shooterPose, elevatorBottomPose, elevatorTopPose, notePose}); + } + + private static final double SHOOT_SPEED = 33.0; + + void recalculateTrajectory() { + trajectory.clear(); + Pose3d startPose = getShootFrom(); + double vz = wristAngle.getSin() * SHOOT_SPEED; + double vx = robotPose.getRotation().getCos() * SHOOT_SPEED; + double vy = robotPose.getRotation().getSin() * SHOOT_SPEED; + double prevZ = startPose.getZ(); + for (double t = 0.0; prevZ > 0.0; t += LoggedRobot.defaultPeriodSecs) { + double x = startPose.getX() + vx * t; + double y = startPose.getY() + vy * t; + double z = startPose.getZ() + vz * t - 9.81 * t * t; + trajectory.add(new Pose3d(x, y, z, new Rotation3d())); + prevZ = z; + } + } + +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69736909..b00d16d6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,7 +21,6 @@ * Constants file. */ public final class Constants { - /** * Stick Deadband */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a6afb6b8..3ec1211f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -160,6 +160,8 @@ public void robotPeriodic() { if (gcTimer.advanceIfElapsed(5)) { System.gc(); } + profiler.swap("viz"); + robotContainer.updateViz(); profiler.pop(); profiler.pop(); } @@ -222,6 +224,11 @@ public void testInit() { @Override public void testPeriodic() {} + @Override + public void simulationPeriodic() { + robotContainer.updateSimulation(); + } + private static final String environmentVariable = "AKIT_LOG_PATH"; private static final String advantageScopeFileName = "akit-log-path.txt"; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 520ee01e..ce70e993 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,8 +1,10 @@ package frc.robot; import java.util.Map; +import org.littletonrobotics.junction.LoggedRobot; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -11,17 +13,24 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.lib.sim.SimulatedArena; +import frc.lib.sim.SimulatedPumbaa; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.photon.PhotonReal; +import frc.lib.viz.PumbaaViz; import frc.robot.Robot.RobotRunType; import frc.robot.autos.JustShoot1; import frc.robot.autos.P123; @@ -36,16 +45,20 @@ import frc.robot.subsystems.LEDs; import frc.robot.subsystems.elevator_wrist.ElevatorWrist; import frc.robot.subsystems.elevator_wrist.ElevatorWristIO; +import frc.robot.subsystems.elevator_wrist.ElevatorWristIOSim; import frc.robot.subsystems.elevator_wrist.ElevatorWristReal; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOFalcon; +import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterSim; import frc.robot.subsystems.shooter.ShooterVortex; import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.SwerveIO; import frc.robot.subsystems.swerve.SwerveReal; +import frc.robot.subsystems.swerve.SwerveSim; /** @@ -55,6 +68,16 @@ * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { + public static Mechanism2d mech = + new Mechanism2d(Constants.Swerve.trackWidth, Constants.Swerve.wheelBase); + public static MechanismRoot2d root = + mech.getRoot("climber", Units.inchesToMeters(12), Constants.Swerve.wheelBase / 2); + public static MechanismLigament2d m_elevator = root.append(new MechanismLigament2d("elevator", + Units.inchesToMeters(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT), 90)); + public static MechanismLigament2d m_wrist = m_elevator.append(new MechanismLigament2d("wrist", + 0.5, Constants.ElevatorWristConstants.SetPoints.MIN_ANGLE.getRadians(), 6, + new Color8Bit(Color.kPurple))); + /* Shuffleboard */ public static ShuffleboardTab mainDriverTab = Shuffleboard.getTab("Main Driver"); @@ -118,53 +141,54 @@ public class RobotContainer { private Trigger noteInIntake = new Trigger(() -> this.intake.getintakeBeamBrakeStatus()) .debounce(0.25, Debouncer.DebounceType.kRising); + /** Viz */ + private final PumbaaViz viz; + + /** Simulation */ + private final SimulatedArena arena; + /** */ public RobotContainer(RobotRunType runtimeType) { + if (runtimeType == RobotRunType.kSimulation) { + arena = new SimulatedArena(); + } else { + arena = null; + } numNoteChooser.setDefaultOption("0", 0); for (int i = 0; i < 7; i++) { numNoteChooser.addOption(String.valueOf(i), i); } - cameras = - /* - * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right - */ - new PhotonCameraWrapper[] { - // new PhotonCameraWrapper( - // new PhotonReal(Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_NAME, - // Constants.CameraConstants.FrontLeftFacingCamera.CAMERA_IP), - // Constants.CameraConstants.FrontLeftFacingCamera.KCAMERA_TO_ROBOT), - new PhotonCameraWrapper( - new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, - Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), - Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT), - // new PhotonCameraWrapper( - // new PhotonReal(Constants.CameraConstants.BackLeftFacingCamera.CAMERA_NAME, - // Constants.CameraConstants.BackLeftFacingCamera.CAMERA_IP), - // Constants.CameraConstants.BackLeftFacingCamera.KCAMERA_TO_ROBOT) - }; - // new PhotonCameraWrapper( - // new PhotonReal(Constants.CameraConstants.BackRightFacingCamera.CAMERA_NAME), - // Constants.CameraConstants.BackRightFacingCamera.KCAMERA_TO_ROBOT)}; + /* + * Camera Order: 0 - Front Left 1 - Front RIght 2 - Back Left 3 - Back Right + */ + cameras = new PhotonCameraWrapper[] {new PhotonCameraWrapper( + new PhotonReal(Constants.CameraConstants.FrontRightFacingCamera.CAMERA_NAME, + Constants.CameraConstants.FrontRightFacingCamera.CAMERA_IP), + Constants.CameraConstants.FrontRightFacingCamera.KCAMERA_TO_ROBOT)}; switch (runtimeType) { case kReal: + viz = new PumbaaViz("Viz", null); shooter = new Shooter(new ShooterVortex()); - intake = new Intake(new IntakeIOFalcon()); - s_Swerve = new Swerve(new SwerveReal(), cameras); - elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator); + intake = new Intake(new IntakeIOFalcon(), viz); + s_Swerve = new Swerve(new SwerveReal(), cameras, viz); + elevatorWrist = new ElevatorWrist(new ElevatorWristReal(), operator, viz); break; case kSimulation: - s_Swerve = new Swerve(new SwerveIO() {}, cameras); - shooter = new Shooter(new ShooterIO() {}); - intake = new Intake(new IntakeIO() {}); - elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); + SimulatedPumbaa pumbaa = arena.newPumbaa(); + viz = new PumbaaViz("Viz", pumbaa); + s_Swerve = new Swerve(new SwerveSim(pumbaa), cameras, viz); + shooter = new Shooter(new ShooterSim(pumbaa)); + intake = new Intake(new IntakeIOSim(pumbaa), viz); + elevatorWrist = new ElevatorWrist(new ElevatorWristIOSim(pumbaa), operator, viz); break; default: - s_Swerve = new Swerve(new SwerveIO() {}, cameras); + viz = new PumbaaViz("Viz", null); + s_Swerve = new Swerve(new SwerveIO() {}, cameras, viz); shooter = new Shooter(new ShooterIO() {}); - intake = new Intake(new IntakeIO() {}); - elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator); + intake = new Intake(new IntakeIO() {}, viz); + elevatorWrist = new ElevatorWrist(new ElevatorWristIO() {}, operator, viz); } autoChooser.setDefaultOption("Nothing", Commands.none()); @@ -301,4 +325,20 @@ public Command getAutonomousCommand() { return autocommand; } + /** + * Update viz + */ + public void updateViz() { + this.viz.update(); + } + + /** + * Update simulation + */ + public void updateSimulation() { + if (this.arena != null) { + this.arena.update(LoggedRobot.defaultPeriodSecs); + } + } + } diff --git a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java index 358d52be..3ee5e47f 100644 --- a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java +++ b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; +import frc.lib.viz.PumbaaViz; import frc.robot.Constants; import frc.robot.OperatorState; import frc.robot.Robot; @@ -72,12 +73,15 @@ public class ElevatorWrist extends SubsystemBase { public Trigger elevatorAtAmp = new Trigger(() -> elevatorAtAmp()); + private final PumbaaViz viz; + /** * Create new ElevatorWrist. */ - public ElevatorWrist(ElevatorWristIO io, CommandXboxController operator) { + public ElevatorWrist(ElevatorWristIO io, CommandXboxController operator, PumbaaViz viz) { this.operator = operator; this.io = io; + this.viz = viz; io.updateInputs(inputs); estimatedWristAngle = getWristAngleMeasurement().getRotations(); wristPIDController @@ -221,6 +225,8 @@ && getHeight() > Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT)) { // Logger.recordOutput("/ElevatorWrist/Wrist/Combined Voltage", // wristFeedForwardValue + wristPIDValue); Logger.recordOutput("/ElevatorWrist/Wrist/Combined Voltage", wristPIDValue); + Robot.profiler.swap("viz"); + viz.setElevatorWrist(calculatedHeight, calculatedWristAngle); Robot.profiler.pop(); Robot.profiler.pop(); } diff --git a/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIOSim.java b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIOSim.java new file mode 100644 index 00000000..fbee2ba1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWristIOSim.java @@ -0,0 +1,116 @@ +package frc.robot.subsystems.elevator_wrist; + +import org.littletonrobotics.junction.LoggedRobot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.lib.sim.SimulatedPumbaa; + +/** + * Elevator Wrist IO Sim + */ +public class ElevatorWristIOSim implements ElevatorWristIO { + + private final SimulatedPumbaa sim; + + /** + * Elevator Wrist IO Sim + */ + public ElevatorWristIOSim(SimulatedPumbaa sim) { + this.sim = sim; + } + + private static final double ELEVATOR_START_HEIGHT = + frc.robot.Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT; + private static final double ELEVATOR_MIN_HEIGHT = + frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_1_HEIGHT; + private static final double ELEVATOR_MAX_HEIGHT = + frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_2_HEIGHT; + private static final double ELEVATOR_TIME_RANGE = 3.0; + + private static final double ELEVATOR_MAX_SPEED = + (ELEVATOR_MAX_HEIGHT - ELEVATOR_MIN_HEIGHT) / ELEVATOR_TIME_RANGE; + + public static final double ELEVATOR_M; + public static final double ELEVATOR_B; + + static { + // + ELEVATOR_M = + (frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_2_ANGLE_MEASURED.getRotations() + - frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_1_ANGLE_MEASURED + .getRotations()) + / (frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_2_HEIGHT + - frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_1_HEIGHT); + // meas_1 * m + b = act_1 + // b = act_1 - meas_1 * m + ELEVATOR_B = + frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_1_ANGLE_MEASURED.getRotations() + - frc.robot.Constants.ElevatorWristConstants.ELEVATOR_REF_1_HEIGHT * ELEVATOR_M; + } + + private static final double WRIST_START_ANGLE = 0.0; + private static final double WRIST_MAX_ANGLE = 55.0; + private static final double WRIST_MIN_ANGLE = -30.0; + private static final double WRIST_TIME_RANGE = 0.4; + + + private static final double WRIST_MAX_SPEED = + (WRIST_MAX_ANGLE - WRIST_MIN_ANGLE) / WRIST_TIME_RANGE; + + public static final double WRIST_M; + public static final double WRIST_B; + + static { + WRIST_M = (frc.robot.Constants.ElevatorWristConstants.WRIST_REF_2_ANGLE_MEASURED + .getRotations() + - frc.robot.Constants.ElevatorWristConstants.WRIST_REF_1_ANGLE_MEASURED.getRotations()) + / (frc.robot.Constants.ElevatorWristConstants.WRIST_REF_2_ANGLE_ACTUAL.getRotations() + - frc.robot.Constants.ElevatorWristConstants.WRIST_REF_1_ANGLE_ACTUAL + .getRotations()); + // meas_1 * m + b = act_1 + // b = act_1 - meas_1 * m + WRIST_B = + frc.robot.Constants.ElevatorWristConstants.WRIST_REF_1_ANGLE_MEASURED.getRotations() + - frc.robot.Constants.ElevatorWristConstants.WRIST_REF_1_ANGLE_ACTUAL.getRotations() + * WRIST_M; + } + + private double elevatorCurrentSpeed = 0.0; + private double elevatorCurrentPosition = ELEVATOR_START_HEIGHT; + private double wristCurrentSpeed = 0.0; + private double wristCurrentPosition = WRIST_START_ANGLE; + + @Override + public void setElevatorVoltage(double voltage) { + voltage = MathUtil.clamp(-voltage, -12.0, 12.0); + elevatorCurrentSpeed = ELEVATOR_MAX_SPEED * (voltage / 12.0); + } + + @Override + public void setWristVoltage(double voltage) { + voltage = MathUtil.clamp(voltage, -12.0, 12.0); + wristCurrentSpeed = WRIST_MAX_SPEED * (voltage / 12.0); + } + + @Override + public void updateInputs(ElevatorWristInputs inputs) { + elevatorCurrentPosition += elevatorCurrentSpeed * LoggedRobot.defaultPeriodSecs; + elevatorCurrentPosition = + MathUtil.clamp(elevatorCurrentPosition, ELEVATOR_MIN_HEIGHT, ELEVATOR_MAX_HEIGHT); + double elevatorMeas = ELEVATOR_M * elevatorCurrentPosition + ELEVATOR_B; + inputs.leftElevatorRelativeEncRawValue = elevatorMeas; + inputs.rightElevatorRelativeEncRawValue = elevatorMeas; + + wristCurrentPosition += wristCurrentSpeed * LoggedRobot.defaultPeriodSecs; + wristCurrentPosition = + MathUtil.clamp(wristCurrentPosition, WRIST_MIN_ANGLE, WRIST_MAX_ANGLE); + double wristMeas = WRIST_M * Units.degreesToRotations(wristCurrentPosition) + WRIST_B; + + inputs.wristAbsoluteEncRawValue = wristMeas; + + this.sim.setElevatorWrist(elevatorCurrentPosition, + Rotation2d.fromDegrees(wristCurrentPosition)); + } + +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index ffb9409c..40f73402 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.viz.PumbaaViz; +import frc.lib.viz.PumbaaViz.NoteLocation; import frc.robot.Constants; import frc.robot.RobotContainer; @@ -23,8 +25,17 @@ public class Intake extends SubsystemBase { private GenericEntry haveNote = RobotContainer.mainDriverTab.add("Have Note", noNote) .withWidget("Single Color View").withPosition(9, 4).withSize(3, 2).getEntry(); - public Intake(IntakeIO io) { + private final PumbaaViz viz; + + /** + * Intake Subsystem + * + * @param io IO Layer + * @param viz Sim Visualization + */ + public Intake(IntakeIO io, PumbaaViz viz) { this.io = io; + this.viz = viz; io.updateInputs(intakeAutoLogged); } @@ -41,6 +52,13 @@ public void periodic() { } else { haveNote.setString(noNote); } + if (getintakeBeamBrakeStatus()) { + viz.setNoteLocation(NoteLocation.Intake); + } else if (getIndexerBeamBrakeStatus()) { + viz.setNoteLocation(NoteLocation.Shooter); + } else { + viz.setNoteLocation(NoteLocation.None); + } } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java index 3a8387d0..9365eb03 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -18,8 +18,8 @@ public static class IntakeInputs { // public double indexerMotorVoltage; // public double intakeAmps; // public double indexerAmps; - // public double intakeRPM; - // public double indexerRPM; + public double intakeRPM; + public double indexerRPM; public boolean indexerBeamBrake; public boolean intakeBeamBrake; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java new file mode 100644 index 00000000..36035001 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -0,0 +1,55 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.LoggedRobot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.lib.sim.SimulatedPumbaa; + +/** + * Intake IO Sim + */ +public class IntakeIOSim implements IntakeIO { + + private FlywheelSim intakeSim = new FlywheelSim(DCMotor.getNEO(2), 1, 0.025); + private FlywheelSim indexerSim = new FlywheelSim(DCMotor.getFalcon500(1), 1, 0.025); + + private double intakeAppliedVolts = 0.0; + private double indexerAppliedVolts = 0.0; + + private final SimulatedPumbaa pumbaa; + + /** + * Intake IO Layer with simulated motors and sensors + * + * @param pumbaa Simulated Robot Viz + */ + public IntakeIOSim(SimulatedPumbaa pumbaa) { + this.pumbaa = pumbaa; + } + + @Override + public void updateInputs(IntakeInputs inputs) { + intakeSim.update(LoggedRobot.defaultPeriodSecs); + indexerSim.update(LoggedRobot.defaultPeriodSecs); + + inputs.intakeBeamBrake = pumbaa.lowerBeamBreak(); // true == game piece + inputs.intakeRPM = intakeSim.getAngularVelocityRPM(); + inputs.indexerBeamBrake = pumbaa.upperBeamBreak(); // true == game piece + inputs.indexerRPM = indexerSim.getAngularVelocityRPM(); + } + + @Override + public void setIntakeMotorPercentage(double percent) { + intakeAppliedVolts = MathUtil.clamp(percent * 12.0, -12.0, 12.0); + intakeSim.setInputVoltage(intakeAppliedVolts); + pumbaa.setIntake(percent); + } + + @Override + public void setIndexerMotorPercentage(double percent) { + indexerAppliedVolts = MathUtil.clamp(percent * 12.0, -12.0, 12.0); + indexerSim.setInputVoltage(indexerAppliedVolts); + pumbaa.setIndexer(percent); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java new file mode 100644 index 00000000..da82fa28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSim.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.LoggedRobot; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.lib.sim.SimulatedPumbaa; +import frc.robot.Constants; + +/** + * Class for ShooterSim + */ +public class ShooterSim implements ShooterIO { + + private FlywheelSim topShooterMotor = + new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); + private FlywheelSim bottomShooterMotor = + new FlywheelSim(DCMotor.getNeoVortex(1), Constants.ShooterConstants.GEAR_RATIO, 0.01); + + private double topAppliedVolts = 0.0; + private double bottomAppliedVolts = 0.0; + + private final SimulatedPumbaa sim; + + /** + * Constructor Shooter Subsystem - sets motor and encoder preferences + */ + public ShooterSim(SimulatedPumbaa sim) { + this.sim = sim; + } + + public void setTopMotor(double power) { + topAppliedVolts = MathUtil.clamp(power, -12.0, 12.0); + topShooterMotor.setInputVoltage(topAppliedVolts); + } + + public void setBottomMotor(double power) { + bottomAppliedVolts = MathUtil.clamp(power, -12.0, 12.0); + bottomShooterMotor.setInputVoltage(bottomAppliedVolts); + } + + + @Override + public void updateInputs(ShooterIOInputsAutoLogged inputs) { + topShooterMotor.update(LoggedRobot.defaultPeriodSecs); + bottomShooterMotor.update(LoggedRobot.defaultPeriodSecs); + inputs.topShooterVelocityRotPerMin = topShooterMotor.getAngularVelocityRPM(); + inputs.bottomShooterVelocityRotPerMin = bottomShooterMotor.getAngularVelocityRPM(); + sim.setShooterSpeed(topShooterMotor.getAngularVelocityRPM()); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index cf8f1c2c..368e3d00 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -26,6 +26,7 @@ import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.swerve.SwerveModule; +import frc.lib.viz.PumbaaViz; import frc.robot.Constants; import frc.robot.Robot; import frc.robot.RobotContainer; @@ -49,26 +50,17 @@ public class Swerve extends SubsystemBase { .withProperties(Map.of("Color when true", "green", "Color when false", "red")) .withPosition(11, 0).withSize(2, 2).getEntry(); + private final PumbaaViz viz; + /** * Swerve Subsystem */ - public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras) { + public Swerve(SwerveIO swerveIO, PhotonCameraWrapper[] cameras, PumbaaViz viz) { this.swerveIO = swerveIO; this.cameras = cameras; + this.viz = viz; + swerveMods = swerveIO.createModules(); fieldOffset = getGyroYaw().getDegrees(); - swerveMods = new SwerveModule[] { - swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID, - Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, - Constants.Swerve.Mod0.angleOffset), - swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID, - Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID, - Constants.Swerve.Mod1.angleOffset), - swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID, - Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID, - Constants.Swerve.Mod2.angleOffset), - swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID, - Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, - Constants.Swerve.Mod3.angleOffset)}; swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions(), new Pose2d()); @@ -123,7 +115,7 @@ rotation, getFieldRelativeHeading()) */ public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); - + Logger.recordOutput("/Swerve/DesiredStates", desiredStates); for (SwerveModule mod : swerveMods) { mod.setDesiredState(desiredStates[mod.moduleNumber], false); } @@ -193,6 +185,7 @@ public Pose2d getPose() { */ public void resetOdometry(Pose2d pose) { swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), pose); + this.swerveIO.setPose(pose); } /** @@ -317,8 +310,11 @@ public void periodic() { Robot.profiler.swap("simple"); SmartDashboard.putBoolean("Has Initialized", hasInitialized); SmartDashboard.putNumber("Gyro Yaw", getGyroYaw().getDegrees()); + Logger.recordOutput("/Swerve/ActualStates", getModuleStates()); Robot.profiler.pop(); Robot.profiler.pop(); + Robot.profiler.swap("viz"); + viz.setPose(getPose()); Robot.profiler.pop(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java index 37175e2e..b0ae91bb 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -3,9 +3,7 @@ import java.util.Optional; import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import frc.lib.util.swerve.SwerveModule; -import frc.lib.util.swerve.SwerveModuleIO; /** IO Class for Swerve */ public interface SwerveIO { @@ -21,17 +19,21 @@ public static class SwerveInputs { public default void updateInputs(SwerveInputs inputs) {} - /** Instantiating SwerveModules */ - public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, - int angleMotorID, int cancoderID, Rotation2d angleOffset) { - return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, - new SwerveModuleIO() {}); + // /** Instantiating SwerveModules */ + // public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, + // int angleMotorID, int cancoderID, Rotation2d angleOffset) { + // return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, + // new SwerveModuleIO() {}); + // } + + public default SwerveModule[] createModules() { + return new SwerveModule[] {}; } public default Optional getInitialPose() { return Optional.empty(); } - public default void update(int i, Pose2d pose) {} + public default void setPose(Pose2d pose) {} } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java index 9155a0cb..410e8ca3 100755 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -22,12 +22,27 @@ public void updateInputs(SwerveInputs inputs) { } - @Override public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID, Rotation2d angleOffset) { - return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset, - new SwerveModuleReal(moduleNumber, driveMotorID, angleMotorID, cancoderID, - angleOffset)); + return new SwerveModule(moduleNumber, angleOffset, + new SwerveModuleReal(driveMotorID, angleMotorID, cancoderID)); + } + + @Override + public SwerveModule[] createModules() { + return new SwerveModule[] { + createSwerveModule(0, Constants.Swerve.Mod0.driveMotorID, + Constants.Swerve.Mod0.angleMotorID, Constants.Swerve.Mod0.canCoderID, + Constants.Swerve.Mod0.angleOffset), + createSwerveModule(1, Constants.Swerve.Mod1.driveMotorID, + Constants.Swerve.Mod1.angleMotorID, Constants.Swerve.Mod1.canCoderID, + Constants.Swerve.Mod1.angleOffset), + createSwerveModule(2, Constants.Swerve.Mod2.driveMotorID, + Constants.Swerve.Mod2.angleMotorID, Constants.Swerve.Mod2.canCoderID, + Constants.Swerve.Mod2.angleOffset), + createSwerveModule(3, Constants.Swerve.Mod3.driveMotorID, + Constants.Swerve.Mod3.angleMotorID, Constants.Swerve.Mod3.canCoderID, + Constants.Swerve.Mod3.angleOffset)}; } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java new file mode 100644 index 00000000..23b1f482 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java @@ -0,0 +1,100 @@ +package frc.robot.subsystems.swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; +import frc.lib.sim.SimulatedPumbaa; +import frc.lib.util.swerve.SwerveModule; +import frc.lib.util.swerve.SwerveModuleSim; +import frc.robot.Constants; + +/** + * Real Class for Swerve + */ +public class SwerveSim implements SwerveIO { + + private SwerveModule[] swerveMods; + private final AnalogGyro m_gyro = new AnalogGyro(0); + private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro); + private Rotation2d rawGyroRotation = new Rotation2d(); + private SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; + private SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] {new SwerveModulePosition(), new SwerveModulePosition(), + new SwerveModulePosition(), new SwerveModulePosition()}; + + private Pose2d currentPose = new Pose2d(); + private final SimulatedPumbaa pumbaa; + + /** + * Real Swerve Initializer + */ + public SwerveSim(SimulatedPumbaa pumbaa) { + this.pumbaa = pumbaa; + } + + @Override + public void updateInputs(SwerveInputs inputs) { + m_gyroSim.setAngle(getHeading().getDegrees()); + if (Constants.Swerve.invertGyro) { + inputs.yaw = (float) -m_gyroSim.getAngle(); + } else { + inputs.yaw = (float) m_gyroSim.getAngle(); + } + // inputs.pitch = 0; + // inputs.roll = 0; + + } + + /** + * Create Swerve Module + * + * @param moduleNumber Module Number + * @return Swerve Module + */ + public SwerveModule createSwerveModule(int moduleNumber) { + SwerveModuleSim modIO = new SwerveModuleSim(); + modIO.setModNumber(moduleNumber); + SwerveModule mod = + new SwerveModule(moduleNumber, Rotation2d.fromRotations(Math.random()), modIO); + return mod; + } + + @Override + public SwerveModule[] createModules() { + swerveMods = new SwerveModule[] {createSwerveModule(0), createSwerveModule(1), + createSwerveModule(2), createSwerveModule(3)}; + return swerveMods; + } + + /** + * Get Heading + * + * @return Get robot heading + */ + private Rotation2d getHeading() { + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = swerveMods[moduleIndex].getPosition(); + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters + - lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + Twist2d twist = Constants.Swerve.swerveKinematics.toTwist2d(moduleDeltas); + this.currentPose = this.currentPose.exp(twist); + this.pumbaa.setPose(this.currentPose); + rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + return rawGyroRotation; + } + + @Override + public void setPose(Pose2d pose) { + this.currentPose = pose; + this.pumbaa.setPose(this.currentPose); + } + +}