Merge pull request #1 from FRC5190/swerve-drivetrain

Swerve drivetrain

.vscode/settings.json

@@ -25,5 +25,7 @@
       }
     },
   ],
-  "java.test.defaultConfig": "WPIlibUnitTests"
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "editor.detectIndentation": false,
+  "editor.tabSize": 2,
 }

Hardware.md

@@ -0,0 +1,27 @@
+# Hardware
+
+This document details all of the hardware (motors, motor controllers, sensors, etc.) on the robot along with relevant information for programming.
+
+## Motors and Motor Controllers
+<!-- Change for 2024 -->
+(Key: F = Front, B = Back, R = Right, L = Left)
+| ID  |      Mechanism      |   Motor    | Motor Controller | PDH Port |
+| --- | ------------------- | ---------- | ---------------- | -------- |
+| 1   | Drivetrain FL Drive | NEO        | SPARK MAX        | ?        |
+| 2   | Drivetrain FL Steer | NEO        | SPARK MAX        | ?        |
+| 3   | Drivetrain FR Drive | NEO        | SPARK MAX        | ?        |
+| 4   | Drivetrain FR Steer | NEO        | SPARK MAX        | ?        |
+| 5   | Drivetrain BL Drive | NEO        | SPARK MAX        | ?        |
+| 6   | Drivetrain BL Steer | NEO        | SPARK MAX        | ?        |
+| 7   | Drivetrain BR Drive | NEO        | SPARK MAX        | ?        |
+| 8   | Drivetrain BR Steer | NEO        | SPARK MAX        | ?        |
+
+
+
+## Sensors and Limelight
+| ID  |   Sensor    |    Type    | PDH Port |
+| --- | ----------- | ---------- | -------- |
+| 11  | FL CANCoder | Absolute   | ?        |
+| 12  | FR CANCoder | Absolute   | ?        |
+| 13  | BL CANCoder | Absolute   | ?        |
+| 14  | BR CANCoder | Absolute   | ?        | 

build.gradle

@@ -68,7 +68,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 }
 
 test {

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,17 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/java/org/ghrobotics/frc2024/CANCoderSwerve.java

@@ -0,0 +1,110 @@
+package org.ghrobotics.frc2024;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.CANcoderConfigurator;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+
+/**
+ * Swerve Absolute Encoder for CTRE CANCoders.
+ */
+public class CANCoderSwerve {
+
+  /**
+   * CANCoder with WPILib sendable and support.
+   */
+  public CANcoder encoder;
+
+  /**
+   * Initialize the CANCoder on the standard CANBus.
+   *
+   * @param id CAN ID.
+   */
+  public CANCoderSwerve(int id) {
+    // Empty string uses the default canbus for the system
+    encoder = new CANcoder(id);
+  }
+
+
+  /**
+   * Reset the encoder to factory defaults.
+   */
+  public void factoryDefault() {
+    encoder.getConfigurator().apply(new CANcoderConfiguration());
+  }
+
+  /**
+   * Clear sticky faults on the encoder.
+   */
+  public void clearStickyFaults() {
+    encoder.clearStickyFaults();
+  }
+
+  /**
+   * Configure the absolute encoder to read from [0, 360) per second.
+   *
+   * @param inverted Whether the encoder is inverted.
+   */
+  public void configure(boolean inverted) {
+    CANcoderConfigurator cfg                       = encoder.getConfigurator();
+    MagnetSensorConfigs  magnetSensorConfiguration = new MagnetSensorConfigs();
+    cfg.refresh(magnetSensorConfiguration);
+    cfg.apply(magnetSensorConfiguration
+      .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Signed_PlusMinusHalf)
+        .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
+          : SensorDirectionValue.CounterClockwise_Positive));
+  }
+
+  /**
+   * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings.
+   *
+   * @return Absolute position in degrees from [0, 360).
+   */
+
+  public double getAbsolutePosition() {
+    StatusSignal<Double> angle = encoder.getAbsolutePosition();
+
+    return angle.getValue() * 360;
+  }
+
+  /**
+   * Get the instantiated absolute encoder Object.
+   *
+   * @return Absolute encoder object.
+   */
+
+  public Object getAbsoluteEncoder() {
+    return encoder;
+  }
+
+  /**
+   * Sets the Absolute Encoder Offset within the CANcoder's Memory.
+   *
+   * @param offset the offset the Absolute Encoder uses as the zero point in degrees.
+   * @return if setting Absolute Encoder Offset was successful or not.
+   */
+  public boolean setAbsoluteEncoderOffset(double offset) {
+    CANcoderConfigurator cfg = encoder.getConfigurator();
+    MagnetSensorConfigs magCfg = new MagnetSensorConfigs();
+    StatusCode error  = cfg.refresh(magCfg);
+    if (error != StatusCode.OK)
+    {
+      return false;
+    }
+    error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
+    return false;
+  }
+
+  /**
+   * Get the velocity in degrees/sec.
+   *
+   * @return velocity in degrees/sec.
+   */
+  public double getVelocity() {
+    return encoder.getVelocity().getValue() * 360;
+  }
+}

src/main/java/org/ghrobotics/frc2024/Robot.java

@@ -5,51 +5,70 @@
 package org.ghrobotics.frc2024;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import org.ghrobotics.frc2024.commands.DriveTeleop;
+import org.ghrobotics.frc2024.subsystems.Drive;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
- * project.
- */
+* The VM is configured to automatically run this class, and to call the functions corresponding to
+* each mode, as described in the TimedRobot documentation. If you change the name of this class or
+* the package after creating this project, you must also update the build.gradle file in the
+* project.
+*/
 public class Robot extends TimedRobot {
-  /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
-   */
+  // Subsystems
+  private final Drive drive_ = new Drive();
+
+  // Robot State
+  private final RobotState robot_state_ = new RobotState(drive_);
+
+  // Xbox Controller
+  private final CommandXboxController driver_controller_ = new CommandXboxController(0);
+
   @Override
-  public void robotInit() {}
-
+  public void robotInit() {
+    drive_.setDefaultCommand(new DriveTeleop(drive_, robot_state_, driver_controller_));
+  }
+
   @Override
-  public void robotPeriodic() {}
-
+  public void robotPeriodic() {
+    CommandScheduler.getInstance().run();
+    robot_state_.update();
+  }
+
   @Override
   public void autonomousInit() {}
-
+  
   @Override
   public void autonomousPeriodic() {}
-
+
+  // Might need this in the future
+  // robot_state_.reset(robot_state_.getPosition());
+  // robot_state_.update();
   @Override
-  public void teleopInit() {}
-
+  public void teleopInit() {
+    drive_.setBrakeMode(true);
+  }
+
   @Override
   public void teleopPeriodic() {}
-
+  
   @Override
   public void disabledInit() {}
-
+  
   @Override
   public void disabledPeriodic() {}
-
+  
   @Override
   public void testInit() {}
-
+  
   @Override
   public void testPeriodic() {}
-
+  
   @Override
   public void simulationInit() {}
-
+  
   @Override
   public void simulationPeriodic() {}
 }