From 79fe1d26edf4a18b3d673f27dcd60c478d6b55f5 Mon Sep 17 00:00:00 2001 From: weerobots <143899357+weerobots@users.noreply.github.com> Date: Sat, 20 Jan 2024 15:23:20 -0600 Subject: [PATCH] fix round2dp swervemodule, set states on power up --- simgui.json | 13 ++++++++++++- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/elastic-layout.json | 1 - .../java/frc/robot/subsystems/Swerve.java | 15 ++++++--------- .../frc/robot/subsystems/SwerveModule.java | 19 ++++++------------- 6 files changed, 26 insertions(+), 27 deletions(-) delete mode 100644 src/main/java/frc/robot/elastic-layout.json diff --git a/simgui.json b/simgui.json index 69cbf69..11eb44d 100644 --- 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 index 449a1c8..63e26df 100644 --- 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 index a3e0424..bcaab64 100644 --- 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 deleted file mode 100644 index 11eba72..0000000 --- a/src/main/java/frc/robot/elastic-layout.json +++ /dev/null @@ -1 +0,0 @@ -{"tabs":[{"name":"Teleoperated","grid_layout":{"layouts":[],"containers":[]}},{"name":"Autonomous","grid_layout":{"layouts":[],"containers":[]}},{"name":"Swerve","grid_layout":{"layouts":[{"title":"Front Left","x":0.0,"y":0.0,"width":256.0,"height":512.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"FL angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FL angle","period":0.06,"data_type":"double"}},{"title":"FL distance","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FL distance","period":0.06,"data_type":"double"}},{"title":"FL cancoder","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FL cancoder","period":0.06,"data_type":"double"}},{"title":"FL cancoderAbs","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FL cancoderAbs","period":0.06,"data_type":"double"}},{"title":"FL velocity mps","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FL velocity mps","period":0.06,"data_type":"double"}}]},{"title":"Back Right","x":768.0,"y":0.0,"width":256.0,"height":512.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"BR angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BR angle","period":0.06,"data_type":"double"}},{"title":"BR distance","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BR distance","period":0.06,"data_type":"double"}},{"title":"BR cancoder","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BR cancoder","period":0.06,"data_type":"double"}},{"title":"BR cancoderAbs","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BR cancoderAbs","period":0.06,"data_type":"double"}}]},{"title":"Front Right","x":256.0,"y":0.0,"width":256.0,"height":512.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"FR angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FR angle","period":0.06,"data_type":"double"}},{"title":"FR distance","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FR distance","period":0.06,"data_type":"double"}},{"title":"FR cancoder","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FR cancoder","period":0.06,"data_type":"double"}},{"title":"FR cancoderAbs","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/FR cancoderAbs","period":0.06,"data_type":"double"}}]},{"title":"Back Left","x":512.0,"y":0.0,"width":256.0,"height":512.0,"type":"List Layout","properties":{"label_position":"TOP"},"children":[{"title":"BL angle","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BL angle","period":0.06,"data_type":"double"}},{"title":"BL distance","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BL distance","period":0.06,"data_type":"double"}},{"title":"BL cancoder","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BL cancoder","period":0.06,"data_type":"double"}},{"title":"BL cancoderAbs","x":0.0,"y":0.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BL cancoderAbs","period":0.06,"data_type":"double"}}]}],"containers":[{"title":"BL angle","x":1408.0,"y":384.0,"width":128.0,"height":128.0,"type":"Text Display","properties":{"topic":"/SmartDashboard/BL angle","period":0.06,"data_type":"double"}}]}}]} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f2c3979..904a9c2 100644 --- 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 index 62c71ac..4fed7ab 100644 --- 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; } + }