From f9413c5920d745047f52b919c23b92a9f23402e1 Mon Sep 17 00:00:00 2001
From: Alex Resnick <adr8292@gmail.com>
Date: Sun, 15 Dec 2024 17:37:37 -0600
Subject: [PATCH] go the Rev stuff working

---
 src/main/java/frc/robot/RobotContainer.java   |  2 ++
 .../frc/robot/subsystems/swerve/Swerve.java   |  6 +++++
 .../frc/robot/subsystems/swerve/SwerveIO.java |  5 +++++
 .../robot/subsystems/swerve/SwerveReal.java   | 22 ++++++++++++++++++-
 4 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a21125d..920b089 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -67,6 +67,8 @@ private void configureButtonBindings() {
 
         /* Driver Buttons */
         driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
+
+        driver.x().whileTrue(s_Swerve.runNeo(1));
     }
 
     /**
diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
index b243698..2f2445f 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -16,6 +16,8 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.util.FieldConstants;
 import frc.lib.util.swerve.SwerveModule;
@@ -323,4 +325,8 @@ public double distanceFromSpeaker() {
                     - getPose().getX());
         return distance;
     }
+
+    public Command runNeo(double power) {
+        return Commands.runEnd(() -> swerveIO.runNeo(power), () -> swerveIO.runNeo(0), this);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
index 6b1b469..00b699b 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
@@ -19,6 +19,9 @@ public static class SwerveInputs {
         public double newyaw;
         public double newroll;
         public double newpitch;
+
+        public double neoPosition;
+        public double neoVelocity;
     }
 
     public default void updateInputs(SwerveInputs inputs) {}
@@ -40,4 +43,6 @@ public default Optional<Pose2d> getInitialPose() {
 
     public default void setPose(Pose2d pose) {}
 
+    public default void runNeo(double power) {}
+
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
index 63c937b..d8c034c 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
@@ -1,6 +1,14 @@
 package frc.robot.subsystems.swerve;
 
 import com.reduxrobotics.sensors.canandgyro.Canandgyro;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
 import com.studica.frc.AHRS;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.lib.util.swerve.SwerveModule;
@@ -13,8 +21,14 @@ public class SwerveReal implements SwerveIO {
     private AHRS gyro = new AHRS(Constants.Swerve.navXID);
     private Canandgyro newGyro = new Canandgyro(1);
 
+    private SparkMax neo = new SparkMax(60, MotorType.kBrushless);
+    private RelativeEncoder neoEncoder = neo.getEncoder();
+
     /** Real Swerve Initializer */
-    public SwerveReal() {}
+    public SwerveReal() {
+        SparkBaseConfig neoConfig = new SparkMaxConfig().inverted(true).idleMode(IdleMode.kCoast);
+        neo.configure(neoConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
 
     @Override
     public void updateInputs(SwerveInputs inputs) {
@@ -25,6 +39,8 @@ public void updateInputs(SwerveInputs inputs) {
         inputs.newpitch = newGyro.getPitch();
         inputs.newroll = newGyro.getRoll();
 
+        inputs.neoPosition = neoEncoder.getPosition();
+        inputs.neoVelocity = neoEncoder.getVelocity();
     }
 
     public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
@@ -50,4 +66,8 @@ public SwerveModule[] createModules() {
                 Constants.Swerve.Mod3.angleOffset)};
     }
 
+    public void runNeo(double power) {
+        neo.set(power);
+    }
+
 }