fix round2dp swervemodule, set states on power up

fondyfire2194 · Jan 20, 2024 · 79fe1d2 · 79fe1d2
1 parent 4728da1
commit 79fe1d2
Show file tree

Hide file tree

Showing 6 changed files with 26 additions and 27 deletions.
diff --git a/simgui.json b/simgui.json
@@ -2,7 +2,11 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
-      "/SmartDashboard/Field": "Field2d"
+      "/SmartDashboard/Auto Mode": "String Chooser",
+      "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/On-the-fly path": "Command",
+      "/SmartDashboard/Pathfind to Pickup Pos": "Command",
+      "/SmartDashboard/Pathfind to Scoring Pos": "Command"
     },
     "windows": {
       "/SmartDashboard/Field": {
@@ -13,6 +17,13 @@
       }
     }
   },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "open": true
+      }
+    }
+  },
   "NetworkTables Info": {
     "visible": true
   }

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -85,8 +85,8 @@ public static final class Swerve {
     public static final double angleConversionFactor = 360.0 / angleGearRatio;
 
     /* Neutral Modes */
-    public static final IdleMode angleNeutralMode = IdleMode.kBrake;
-    public static final IdleMode driveNeutralMode = IdleMode.kBrake;
+    public static final IdleMode angleNeutralMode = IdleMode.kCoast;
+    public static final IdleMode driveNeutralMode = IdleMode.kCoast;
 
     /* Motor Inverts */
     public static final boolean driveInvert = false;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -8,7 +8,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;

diff --git a/src/main/java/frc/robot/elastic-layout.json b/src/main/java/frc/robot/elastic-layout.json
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.PathPlannerLogging;
@@ -93,18 +94,14 @@ public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) {
 
   public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) {
     SwerveModuleState[] swerveModuleStates = Constants.Swerve.swerveKinematics.toSwerveModuleStates(
-
         fieldRelative
             ? ChassisSpeeds.fromFieldRelativeSpeeds(
                 translation.getX(), translation.getY(), rotation, getYaw())
             : new ChassisSpeeds(translation.getX(), translation.getY(), rotation));
-
     SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed);
-
     for (SwerveModule mod : mSwerveMods) {
       mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop);
     }
-
   }
 
   /* Used by SwerveControllerCommand in Auto */
@@ -120,6 +117,10 @@ public void resetModuleEncoders() {
     mSwerveMods[1].resetAngleToAbsolute();
     mSwerveMods[2].resetAngleToAbsolute();
     mSwerveMods[3].resetAngleToAbsolute();
+    mSwerveMods[0].setDesiredState(new SwerveModuleState(),false);
+    mSwerveMods[1].setDesiredState(new SwerveModuleState(),false);
+    mSwerveMods[2].setDesiredState(new SwerveModuleState(),false);
+    mSwerveMods[3].setDesiredState(new SwerveModuleState(),false);
   }
 
   public Pose2d getPose() {
@@ -221,11 +222,7 @@ private void putStates() {
 
   public static double round2dp(double number, int dp) {
     double temp = Math.pow(10, dp);
-    SmartDashboard.putNumber("temp", temp);
-    double temp1 = Math.round(number * temp);
-    SmartDashboard.putNumber("temp1", temp1);
-
-    return temp1 / temp;
+    return Math.round(number * temp) / temp;
   }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -74,7 +74,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
     setSpeed(desiredState, isOpenLoop);
   }
 
-  public SwerveModuleState getDesiredState(){
+  public SwerveModuleState getDesiredState() {
     return currentDesiredState;
   }
 
@@ -128,7 +128,6 @@ private void configDriveMotor() {
 
   private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
 
-    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "Set Speed", desiredState.speedMetersPerSecond);
     if (isOpenLoop) {
       double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
       driveMotor.setVoltage(percentOutput * RobotController.getBatteryVoltage());
@@ -164,23 +163,17 @@ public SwerveModuleState getState() {
 
   @Override
   public void periodic() {
-    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "angle deg",
-        round2dp(getAngle().getDegrees(), 2));
-    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "distance m",
-        round2dp(driveEncoder.getPosition(), 2));
-    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "velocity mps",
-        round2dp(driveEncoder.getVelocity(), 2));
 
     SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "cancoder",
-        round2dp(m_turnCancoder.getPosition().getValueAsDouble(), 2));
-    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "cancoderAbs",
         round2dp(m_turnCancoder.getAbsolutePosition().getValueAsDouble(), 2));
+    SmartDashboard.putNumber(Constants.Swerve.modNames[moduleNumber] + "intcoder",
+        round2dp(integratedAngleEncoder.getPosition(), 2));
 
   }
 
   public static double round2dp(double number, int dp) {
-    number = Math.round(number * dp);
-    number /= 100;
-    return number;
+    double temp = Math.pow(10, dp);
+    return Math.round(number * temp)/ temp;
   }
+
 }