Merge branch 'main' of https://github.com/FRC5190/2024CompetitionSeason…

… into superstructure

.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 {

simgui.json

@@ -4,6 +4,13 @@
       "/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,77 +5,70 @@
 package org.ghrobotics.frc2024;
 
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
-import org.ghrobotics.frc2024.subsystems.Arm;
-import org.ghrobotics.frc2024.subsystems.Climber;
-import org.ghrobotics.frc2024.subsystems.Intake;
-import org.ghrobotics.frc2024.subsystems.Shooter;
-
-
+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 Arm arm_ = new Arm();
-  private final Climber climber_ = new Climber();
-  private final Intake intake_ = new Intake();
-  private final Shooter shooter_ = new Shooter();
-
-  // Superstructure
-  private final Superstructure superstructure_ = new Superstructure(arm_, climber_, intake_, shooter_);
-
-  // Xbox Controllers
+  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);
-  private final CommandXboxController operator_controller_ = new CommandXboxController(1);
-
-
+
   @Override
-  public void robotInit() {}
-
+  public void robotInit() {
+    drive_.setDefaultCommand(new DriveTeleop(drive_, robot_state_, driver_controller_));
+  }
+
   @Override
   public void robotPeriodic() {
-    SmartDashboard.putString("Current State", superstructure_.getState());
+    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() {}
 }

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

@@ -0,0 +1,58 @@
+package org.ghrobotics.frc2024;
+
+import org.ghrobotics.frc2024.subsystems.Drive;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class RobotState {
+  // Swerve Drive
+  private final Drive drive_;
+
+  // Position Estimator
+  private final SwerveDrivePoseEstimator pose_estimator_;
+
+  // Constructor
+  public RobotState(Drive drive) {
+    // Assign member variables
+    drive_ = drive;
+
+    // Initialize pose estimator
+    pose_estimator_ = new SwerveDrivePoseEstimator(
+      drive_.getKinematics(), drive.getAngle(), drive.getSwerveModulePositions(),
+      new Pose2d());
+  }
+
+  // Update
+  public void update() {
+    // Update pose estimator with new drive measurements
+    pose_estimator_.update(drive_.getAngle(), drive_.getSwerveModulePositions());
+    SmartDashboard.putNumber("Robot Pose X", getPosition().getTranslation().getX());
+    SmartDashboard.putNumber("Robot Pose Y", getPosition().getTranslation().getY());
+    SmartDashboard.putNumber("Actual Robot Heading", drive_.getAngle().getDegrees());
+    SmartDashboard.putNumber("Estimated Robot Heading", getDegree());
+  }
+
+  // Get Position
+  public Pose2d getPosition() {
+    return pose_estimator_.getEstimatedPosition();
+  }
+
+  // Get Degree
+  public double getDegree() {
+    return pose_estimator_.getEstimatedPosition().getRotation().getDegrees();
+  }
+
+  // Get Rotation2d
+  public Rotation2d getRotation2d() {
+    return pose_estimator_.getEstimatedPosition().getRotation();
+  }
+
+  // Reset Position
+  // Add this after testing
+  public void reset(Pose2d pose) {
+    pose_estimator_.resetPosition(drive_.getAngle(), drive_.getSwerveModulePositions(), pose);
+  }
+}