SwerveModule done

src/main/java/frc4388/robot/Constants.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
 
 /**
@@ -18,12 +19,6 @@ import frc4388.utility.LEDPatterns;
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final class DriveConstants {
-        public static final int DRIVE_PIGEON_ID = 6;
-
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-    }
-
     public static final class SwerveDriveConstants {
         public static final class IDs {
             public static final int DRIVE_PIGEON_ID = -1;
@@ -44,7 +39,21 @@ public final class Constants {
             public static final int RIGHT_BACK_ENCODER_ID = -1;
         }
 
+        public static final class PIDConstants {
+            public static final int SWERVE_SLOT_IDX = 0;
+            public static final int SWERVE_PID_LOOP_IDX = 1;
+            public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
+        }
 
+        public static final class Conversions {
+            public static final int CANCODER_TICKS_PER_ROTATION = 4096;
+        }
+
+
+        public static final double MAX_SPEED_FEET_PER_SECOND = 16; // TODO: find the actual value
+
+        public static final int SWERVE_TIMEOUT_MS = 30;
+        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
     
     public static final class LEDConstants {

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -1,85 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import frc4388.robot.Constants.DriveConstants;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotTime;
-
-/**
- * Add your docs here.
- */
-public class DiffDrive extends SubsystemBase {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private WPI_TalonFX m_leftFrontMotor;
-  private WPI_TalonFX m_rightFrontMotor;
-  private WPI_TalonFX m_leftBackMotor;
-  private WPI_TalonFX m_rightBackMotor;
-  private DifferentialDrive m_driveTrain;
-  private RobotGyro m_gyro;
-
-  /**
-   * Add your docs here.
-   */
-  public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
-      WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
-
-    m_leftFrontMotor = leftFrontMotor;
-    m_rightFrontMotor = rightFrontMotor;
-    m_leftBackMotor = leftBackMotor;
-    m_rightBackMotor = rightBackMotor;
-    m_driveTrain = driveTrain;
-    m_gyro = gyro;
-  }
-
-  @Override
-  public void periodic() {
-    m_gyro.updatePigeonDeltas();
-
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
-      updateSmartDashboard();
-    }
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void tankDriveWithInput(double leftMove, double rightMove) {
-    m_leftFrontMotor.set(leftMove);
-    m_rightFrontMotor.set(rightMove);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  private void updateSmartDashboard() {
-
-    // Examples of the functionality of RobotGyro
-    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    // SmartDashboard.putData(m_gyro);
-  }
-}

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -4,18 +4,28 @@
 
 package frc4388.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
 
 public class SwerveModule extends SubsystemBase {
     private WPI_TalonFX driveMotor;
     private WPI_TalonFX angleMotor;
     private CANCoder canCoder;
 
+    public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
+  
   
     /** Creates a new SwerveModule. */
     public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
@@ -24,11 +34,51 @@ public class SwerveModule extends SubsystemBase {
         this.canCoder = canCoder;
 
         TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        angleConfig.slot0.kP = swerveGains.kP;
+        angleConfig.slot0.kI = swerveGains.kI;
+        angleConfig.slot0.kD = swerveGains.kD;
 
+        // use the CANcoder as the remote sensor for the primary TalonFX PID
+        angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
+        angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+        angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+        angleMotor.configAllSettings(angleConfig);
+
+        CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
+        canCoderConfig.magnetOffsetDegrees = offset;
+        canCoder.configAllSettings(canCoderConfig);
     }
 
-    @Override
+    /**
-    public void periodic() {
+     * Get the angle of a SwerveModule as a Rotation2d
-        // This method will be called once per scheduler run
+     * @return the angle of a SwerveModule as a Rotation2d
+     */
+    public Rotation2d getAngle() {
+        // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. 
+        return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
     }
+
+    /**
+     * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
+     * @param desiredState a SwerveModuleState representing the desired new state of the module
+     */
+    public void setDesiredState(SwerveModuleState desiredState) {
+        Rotation2d currentRotation = this.getAngle();
+
+        SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+        // calculate the difference between our current rotational position and our new rotational position
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+        // calculate the new absolute position of the SwerveModule based on the difference in rotation
+        double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
+
+        // convert the CANCoder from its position reading to ticks
+        double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
+        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+        driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
+    }
+
 }