Merge pull request #14 from ddjoshua01/main

Swerve
fairviewrobotics · Jan 26, 2024 · 6accd84 · 6accd84
2 parents fbe0397 + 27593e1
commit 6accd84
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java
@@ -15,7 +15,7 @@ public class DrivetrainConstants {
     public static final double rotationalSlewRate = 12.0; // percent/second (1 = 100%) - rotation
 
     public static final double drivingSpeedScalar = -1.0;
-    public static final double rotationSpeedScalar = -2.0;
+    public static final double rotationSpeedScalar = 2.0;
 
     public static final double trackWidth = Units.inchesToMeters(22);
     public static final double wheelBase = Units.inchesToMeters(22.5);
@@ -27,20 +27,20 @@ public class DrivetrainConstants {
             new Translation2d(-wheelBase / 2, -trackWidth / 2)
     );
 
-    public static final double frontLeftChassisAngularOffset = 5.772;
-    public static final double frontRightChassisAngularOffset = 6.099;
-    public static final double rearLeftChassisAngularOffset = 0.871 + Math.PI;
-    public static final double rearRightChassisAngularOffset = 3.650;
+    public static final double frontLeftChassisAngularOffset = -0.57 + Math.PI;
+    public static final double frontRightChassisAngularOffset = 0.874 + Math.PI;
+    public static final double rearLeftChassisAngularOffset = 6.089;
+    public static final double rearRightChassisAngularOffset = 2.428 + Math.PI/2;
 
-    public static final int frontLeftDrivingPort = 3;
-    public static final int rearLeftDrivingPort = 4;
-    public static final int frontRightDrivingPort = 10;
-    public static final int rearRightDrivingPort = 11;
+    public static final int frontLeftDrivingPort = 1;
+    public static final int rearLeftDrivingPort = 5;
+    public static final int frontRightDrivingPort = 3;
+    public static final int rearRightDrivingPort = 7;
 
-    public static final int frontLeftTurningPort = 8;
+    public static final int frontLeftTurningPort = 2;
     public static final int rearLeftTurningPort = 6;
-    public static final int frontRightTurningPort = 18;
-    public static final int rearRightTurningPort = 2;
+    public static final int frontRightTurningPort = 4;
+    public static final int rearRightTurningPort = 8;
 
     public static final boolean gyroReversed = false;
     public static final boolean turningEncoderReversed = true;

diff --git a/src/main/java/frc/robot/controllers/SwerveModuleControlller.java b/src/main/java/frc/robot/controllers/SwerveModuleControlller.java
@@ -12,10 +12,9 @@ public class SwerveModuleControlller {
     private final CANSparkMax turningMotor;
     private final RelativeEncoder drivingEncoder;
     private final AbsoluteEncoder turningEncoder;
-    private final SparkMaxPIDController drivingPID;
-    private final SparkMaxPIDController turningPID;
+    private final SparkPIDController drivingPID;
+    private final SparkPIDController turningPID;
     private SwerveModuleState m_desiredState;
-
     private double chassisAngularOffset;
 
     /**

diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java
@@ -104,6 +104,12 @@ public double heading() {
     private final DoubleEntry frontleftpos = NetworkTableInstance.getDefault()
             .getTable("Swerve").getDoubleTopic("flpos").getEntry(frontLeft.getPosition().angle.getRadians());
 
+    private final DoubleEntry rearrightpos = NetworkTableInstance.getDefault()
+            .getTable("Swerve").getDoubleTopic("rrpos").getEntry(rearRight.getPosition().angle.getRadians());
+
+    private final DoubleEntry rearleftpos = NetworkTableInstance.getDefault()
+            .getTable("Swerve").getDoubleTopic("rlpos").getEntry(rearLeft.getPosition().angle.getRadians());
+
     // Periodic
 
     /**
@@ -133,6 +139,8 @@ public void periodic() {
 
         frontrightpos.set(frontRight.getPosition().angle.getRadians());
         frontleftpos.set(frontLeft.getPosition().angle.getRadians());
+        rearrightpos.set(rearRight.getPosition().angle.getRadians());
+        rearleftpos.set(rearLeft.getPosition().angle.getRadians());
 
         // Set Network Tables Telemetry
         actualTelemetry.set(new double[]{