Merge branch 'main' of https://github.com/SkylineSpartabots/Artemis

SkylineSpartabots · Jan 30, 2024 · a09a97c · a09a97c
2 parents 074337f + 97d2933
commit a09a97c
Show file tree

Hide file tree

Showing 7 changed files with 156 additions and 25 deletions.
diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
@@ -0,0 +1,8 @@
+{
+  "download": {
+    "serverTeam": "2976"
+  },
+  "output": {
+    "outputFolder": "C:\\Users\\yunbo\\Sys Id Logs"
+  }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -145,14 +145,9 @@ public static final class SwerveConstants {
     public static final double driveKD = 0.0;
     public static final double driveKF = 0.0;
 
-    /*
-     * Drive Motor Characterization Values
-     * Divide SYSID values by 12 to convert from volts to percent output for CTRE
-     */
-    public static final double driveKS = (0.15932 / 12); 
-    public static final double driveKV = (2.3349 / 12); 
-    public static final double driveKA = (0.47337 / 12);
-
+    /* 1/(( ( (14*15)/(45.0*27) * 5676 ) / 60 ) * 0.31918) */ 
+    public static final double driveFF = 0.19161529689; // 1/(kDriveMotorFreeSpeedRPS * kWheelCircumferenceMeters)  1/ 94.6 * 0.31918 / ---> (1/30.1944) / 6.75
+// 
     /* Swerve Profiling Values */
     /* Meters per Second */
     public static final double maxSpeed = 4.5; 
@@ -179,7 +174,7 @@ public static final class Mod1 {
         public static final int driveMotorID = 53;
         public static final int angleMotorID = 54;
         public static final int canCoderID = 12;
-        public static final Rotation2d angleOffset = Rotation2d.fromDegrees(286.7);
+        public static final Rotation2d angleOffset = Rotation2d.fromDegrees(284.67);
         public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID,
                 canCoderID, angleOffset);
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,6 +17,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...

diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java
@@ -1,22 +1,19 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import com.revrobotics.SparkPIDController;
 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 edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import frc.lib.math.Conversions;
 import frc.lib.util.CTREModuleState;
 import frc.lib.util.SwerveModuleConstants;
 
-import javax.print.attribute.standard.PagesPerMinute;
-
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
-import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.ControlType;
 import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -44,7 +41,7 @@ public Power(double v, double c){
     private CANSparkMax mDriveMotor;
     public CANcoder angleEncoder;
 
-    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.SwerveConstants.driveKS, Constants.SwerveConstants.driveKV, Constants.SwerveConstants.driveKA);
+    private PWMSparkMax test;
 
     public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants){
         this.moduleNumber = moduleNumber;
@@ -133,24 +130,30 @@ private void configAngleMotor(){
         mAngleMotor.setInverted(Constants.SwerveConstants.angleMotorInvert);
         mAngleMotor.setIdleMode(Constants.SwerveConstants.angleNeutralMode);
 
-        mAngleMotor.getPIDController().setP(Constants.SwerveConstants.angleKP);
-        mAngleMotor.getPIDController().setI(Constants.SwerveConstants.angleKI);
-        mAngleMotor.getPIDController().setD(Constants.SwerveConstants.angleKD);
+        SparkPIDController pid = mAngleMotor.getPIDController();
+        pid.setP(Constants.SwerveConstants.angleKP);
+        pid.setI(Constants.SwerveConstants.angleKI);
+        pid.setD(Constants.SwerveConstants.angleKD);
+        pid.setFF(0); // does not need a FF
 
         resetToAbsolute();
     }
 
-    private void configDriveMotor(){   
+    private void configDriveMotor(){
         mDriveMotor.setSmartCurrentLimit(Constants.SwerveConstants.drivePeakCurrentLimit);
         mDriveMotor.setInverted(Constants.SwerveConstants.driveMotorInvert);
         mDriveMotor.setIdleMode(Constants.SwerveConstants.driveNeutralMode);
-        
+
         mDriveMotor.setOpenLoopRampRate(Constants.SwerveConstants.openLoopRamp);
         mDriveMotor.setClosedLoopRampRate(Constants.SwerveConstants.closedLoopRamp);
 
-        mDriveMotor.getPIDController().setP(Constants.SwerveConstants.driveKP);
-        mDriveMotor.getPIDController().setI(Constants.SwerveConstants.driveKI);
-        mDriveMotor.getPIDController().setD(Constants.SwerveConstants.driveKD);
+        SparkPIDController pid = mDriveMotor.getPIDController();
+        pid.setP(Constants.SwerveConstants.driveKP);
+        pid.setI(Constants.SwerveConstants.driveKI);
+        pid.setD(Constants.SwerveConstants.driveKD);
+        pid.setFF(Constants.SwerveConstants.driveFF);
+
+//        mDriveMotor.setFF
 
         mDriveMotor.getEncoder().setPosition(0);
     }
@@ -165,6 +168,23 @@ public SwerveModuleState getState(){
         ); 
     }
 
+    /**
+     * Gets the drive motor of this module. 
+     * @return The drive motor of this module.
+     */
+    public CANSparkMax getDriveMotor() {
+
+        return mDriveMotor;
+    }
+
+    /**
+     * Sets the specified voltage to the drive motor.
+     * @param voltage The desired voltage to set the drive motor to.
+     */
+    public void setDriveVoltage(double voltage) {
+        mDriveMotor.setVoltage(voltage);
+    }
+
     /**
      * Gets the current and voltage going to the angle motor of the module.
      * @return A Power object containing the current current and voltage going to the angle motor.
@@ -178,6 +198,22 @@ public Power getAnglePower(){
         return new Power(mAngleMotor.getBusVoltage(), mAngleMotor.getOutputCurrent());
     }
 
+    /**
+     * Gets the current distance traveled measured by the wheel 
+     * @return The distance traveled in meters
+     */
+    public double getDistance() {
+        return Conversions.motorToMeters(mDriveMotor.getEncoder().getPosition(), Constants.SwerveConstants.wheelCircumference, Constants.SwerveConstants.driveGearRatio);
+    }
+
+    /**
+     * Gets the current velocity of the wheel 
+     * @return The velcity in meters per second. 
+     */
+    public double getVelocity() {
+        return Conversions.motorToMPS(mDriveMotor.getEncoder().getVelocity(), Constants.SwerveConstants.wheelCircumference, Constants.SwerveConstants.driveGearRatio);
+    }
+
     /**
      * Gets the current distance measured by the wheel and rotation of the module.
      * @return A SwerveModulePosition containing the current measured distance and rotation of the module.
@@ -189,4 +225,4 @@ public SwerveModulePosition getPosition(){
         );
     }
 
-}
+}
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems;
 
 
+import com.ctre.phoenix6.SignalLogger;
 import frc.robot.SwerveModule;
 
 import java.util.function.BooleanSupplier;
@@ -25,10 +26,20 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.units.Power;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+
+import edu.wpi.first.units.Units.*;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
 public class Swerve extends SubsystemBase {
@@ -50,7 +61,7 @@ public static Swerve getInstance() {
 
     private SwerveDrivePoseEstimator swerveOdometry;
     private Pigeon2 gyro;
-    private SwerveModule[] mSwerveMods;
+    private SwerveModule[] mSwerveMods ;
     public Supplier<Pose2d> poseSupplier = () -> getPose();
     public Consumer<Pose2d> poseConsumer = a -> {resetOdometry(a);};
     public BooleanSupplier inPosition = () -> inPosition();
@@ -63,13 +74,21 @@ public static Swerve getInstance() {
     public double yTolerance = 1.0;
     public double rotTolerance = 30;
 
+    private final MutableMeasure<Voltage> m_appliedVoltage = edu.wpi.first.units.MutableMeasure.mutable(edu.wpi.first.units.Units.Volts.of(0));
+    private final MutableMeasure<Distance> m_distance = edu.wpi.first.units.MutableMeasure.mutable(edu.wpi.first.units.Units.Meters.of(0));
+    private final MutableMeasure<Velocity<Distance>> m_velocity = edu.wpi.first.units.MutableMeasure.mutable(edu.wpi.first.units.Units.MetersPerSecond.of(0));
+
+    SysIdRoutine routine;
+
     //TODO: kalman filters here
 
     // initializes the swerve modules
     public Swerve() {
         gyro = new Pigeon2(Constants.SwerveConstants.pigeonID);
         gyro.getConfigurator().apply(new Pigeon2Configuration());
 
+
+
         mSwerveMods = new SwerveModule[] {
                 new SwerveModule(0, Constants.SwerveConstants.Mod0.constants),
                 new SwerveModule(1, Constants.SwerveConstants.Mod1.constants),
@@ -80,6 +99,26 @@ public Swerve() {
         swerveOdometry = 
         new SwerveDrivePoseEstimator(
             Constants.SwerveConstants.swerveKinematics, getYaw(), getModulePositions(), new Pose2d());
+        routine = 
+        new SysIdRoutine(
+            // empty config defaults to 1 volt/second ramp rate and 7 volt step voltage
+            new SysIdRoutine.Config(null, null, null, (state) -> SignalLogger.writeString("State", state.toString())),
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                    for (SwerveModule mod : mSwerveMods) {
+                        mod.setDriveVoltage(volts.in(edu.wpi.first.units.Units.Volts));
+                    }
+                },
+                log -> {
+                    for (SwerveModule mod : mSwerveMods) {
+                        log.motor("Mod " + mod.moduleNumber)
+                            .voltage(
+                                m_appliedVoltage.mut_replace(
+                                    mod.getDriveMotor().get() * RobotController.getBatteryVoltage(), edu.wpi.first.units.Units.Volts))
+                            .linearPosition(m_distance.mut_replace(mod.getDistance(), edu.wpi.first.units.Units.Meters))
+                            .linearVelocity(m_velocity.mut_replace(mod.getVelocity(), edu.wpi.first.units.Units.MetersPerSecond));
+                    }
+                } , this));
     }
 
         // used to apply any driving movement to the swerve modules
@@ -237,6 +276,14 @@ public boolean inPosition() {
                         - goalPose.getRotation().getDegrees()) < rotTolerance);
     }
 
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+
     @Override
     public void periodic() {
       // This method will be called once per scheduler run

diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json
@@ -1,3 +1,46 @@
+{
+    "fileName": "ChoreoLib.json",
+    "name": "ChoreoLib",
+    "version": "2024.1.2",
+    "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://SleipnirGroup.github.io/ChoreoLib/dep"
+    ],
+    "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.choreo.lib",
+            "artifactId": "ChoreoLib-java",
+            "version": "2024.1.2"
+        },
+        {
+            "groupId": "com.google.code.gson",
+            "artifactId": "gson",
+            "version": "2.10.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.choreo.lib",
+            "artifactId": "ChoreoLib-cpp",
+            "version": "2024.1.2",
+            "libName": "ChoreoLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
     {
     "fileName": "ChoreoLib.json",
     "name": "ChoreoLib",