From e73e2296d1a0df9fc246ba59097f1984cf1f931f Mon Sep 17 00:00:00 2001 From: Wilson Watson Date: Tue, 14 Nov 2023 12:02:40 -0600 Subject: [PATCH] fix linting errors --- build.gradle | 9 +++++++-- checker/build.gradle | 0 .../frc5572/robotools/CompilationData.java | 0 .../org/frc5572/robotools/RobotProcessor.java | 2 +- .../org/frc5572/robotools/checks/Check.java | 0 .../org/frc5572/robotools/checks/IOCheck.java | 0 .../frc/lib/util/swerve/SwerveModuleIO.java | 2 ++ .../frc/lib/util/swerve/SwerveModuleReal.java | 6 ++++++ src/main/java/frc/robot/BuildConstants.java | 19 ------------------- src/main/java/frc/robot/Constants.java | 3 +++ src/main/java/frc/robot/Robot.java | 12 +++++++++++- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- .../java/frc/robot/autos/CrossAndDock.java | 2 +- .../java/frc/robot/autos/DoubleScore.java | 2 +- .../java/frc/robot/autos/DoubleScore6.java | 5 +---- .../frc/robot/autos/MiddleScoreEngage.java | 2 +- src/main/java/frc/robot/autos/Score1.java | 2 +- src/main/java/frc/robot/autos/Score1Dock.java | 2 +- .../java/frc/robot/autos/SecondGamePiece.java | 2 +- .../frc/robot/autos/SecondGamePiece6.java | 2 +- .../frc/robot/autos/SecondGamePieceScore.java | 2 +- .../java/frc/robot/autos/TripleScore.java | 2 +- .../java/frc/robot/autos/TripleScore6.java | 2 +- .../java/frc/robot/commands/arm/DockArm.java | 2 +- .../java/frc/robot/commands/arm/ScoreArm.java | 2 +- .../robot/commands/drive/MoveToEngage.java | 14 +------------- .../frc/robot/commands/drive/MoveToScore.java | 2 +- .../java/frc/robot/commands/test/TestArm.java | 2 -- .../frc/robot/commands/wrist/AutoWrist.java | 2 +- .../robot/commands/wrist/VariableIntake.java | 2 +- .../robot/commands/wrist/WristIntakeIn.java | 2 +- .../commands/wrist/WristIntakeRelease.java | 2 +- .../java/frc/robot/subsystems/arm/Arm.java | 1 + .../java/frc/robot/subsystems/arm/ArmIO.java | 2 ++ .../robot/subsystems/arm/ArmIOSparkMax.java | 2 ++ .../frc/robot/subsystems/swerve2/Swerve.java | 19 ++++++++++--------- .../robot/subsystems/swerve2/SwerveIO.java | 2 ++ .../robot/subsystems/swerve2/SwerveReal.java | 6 ++++++ .../WristIntake.java | 16 ++++++++++------ .../WristIntakeIO.java | 14 +++++++------- .../WristIntakeIOFalcon500.java | 8 +++++--- 41 files changed, 102 insertions(+), 86 deletions(-) mode change 100755 => 100644 checker/build.gradle mode change 100755 => 100644 checker/src/main/java/org/frc5572/robotools/CompilationData.java mode change 100755 => 100644 checker/src/main/java/org/frc5572/robotools/checks/Check.java mode change 100755 => 100644 checker/src/main/java/org/frc5572/robotools/checks/IOCheck.java delete mode 100644 src/main/java/frc/robot/BuildConstants.java rename src/main/java/frc/robot/subsystems/{wristIntake => wrist_intake}/WristIntake.java (73%) rename src/main/java/frc/robot/subsystems/{wristIntake => wrist_intake}/WristIntakeIO.java (58%) rename src/main/java/frc/robot/subsystems/{wristIntake => wrist_intake}/WristIntakeIOFalcon500.java (75%) diff --git a/build.gradle b/build.gradle index d6e02c1b..68d92e98 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2023.4.3" + id "checkstyle" } sourceCompatibility = JavaVersion.VERSION_11 @@ -83,7 +84,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' - // annotationProcessor project(":checker") + annotationProcessor project(":checker") def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } @@ -117,8 +118,8 @@ tasks.withType(JavaCompile) { // only run in GH actions if (System.getenv("GITHUB_ACTOR") != null && !System.getenv("GITHUB_ACTOR").equals("")) { options.compilerArgs << "-Xplugin:rchk" - outputs.upToDateWhen { false } // Force recompile, even if source file hasn't changed } + outputs.upToDateWhen { false } // Force recompile, even if source file hasn't changed } javadoc { @@ -130,3 +131,7 @@ javadoc { "" } } + +checkstyle { + config = project.resources.text.fromFile("checks.xml") +} diff --git a/checker/build.gradle b/checker/build.gradle old mode 100755 new mode 100644 diff --git a/checker/src/main/java/org/frc5572/robotools/CompilationData.java b/checker/src/main/java/org/frc5572/robotools/CompilationData.java old mode 100755 new mode 100644 diff --git a/checker/src/main/java/org/frc5572/robotools/RobotProcessor.java b/checker/src/main/java/org/frc5572/robotools/RobotProcessor.java index d6cf86a9..7eee7689 100644 --- a/checker/src/main/java/org/frc5572/robotools/RobotProcessor.java +++ b/checker/src/main/java/org/frc5572/robotools/RobotProcessor.java @@ -69,7 +69,7 @@ public boolean process(Set arg0, RoundEnvironment roundEn if (fatal) { throw new RuntimeException("Checks failed!"); } - return true; + return false; } } diff --git a/checker/src/main/java/org/frc5572/robotools/checks/Check.java b/checker/src/main/java/org/frc5572/robotools/checks/Check.java old mode 100755 new mode 100644 diff --git a/checker/src/main/java/org/frc5572/robotools/checks/IOCheck.java b/checker/src/main/java/org/frc5572/robotools/checks/IOCheck.java old mode 100755 new mode 100644 diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java index a46ca613..aebfe628 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java @@ -4,8 +4,10 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; +/** IO Class for SwerveModule */ public interface SwerveModuleIO { + /** Inputs Class for SwerveModule */ @AutoLog public static class SwerveModuleInputs { public double angleEncoderValue; diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java index 0161b20c..cabc7aa7 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java @@ -9,6 +9,9 @@ import frc.robot.Constants; import frc.robot.Robot; +/** + * Implementation for the real robot + */ public class SwerveModuleReal implements SwerveModuleIO { private TalonFX angleMotor; // Output @@ -17,6 +20,9 @@ public class SwerveModuleReal implements SwerveModuleIO { private SwerveModuleConstants constants; + /** + * Initializer for real swerve module. Configures with defaults. + */ public SwerveModuleReal(SwerveModuleConstants constants) { this.constants = constants; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index c8ec0750..00000000 --- a/src/main/java/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "FRC2023"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 79; - public static final String GIT_SHA = "da48d549ec11ee4bac295346b92a67a91eae1de2"; - public static final String GIT_DATE = "2023-06-08 11:15:18 CDT"; - public static final String GIT_BRANCH = "adding-advantagekit-logging"; - public static final String BUILD_DATE = "2023-09-05 08:08:29 CDT"; - public static final long BUILD_UNIX_TIME = 1693919309083L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index de834728..0b45c278 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,6 +22,9 @@ public final class Constants { public static final int DRIVER_ID = 0; public static final int OPERATOR_ID = 1; + /** number of seconds between each loop iteration. */ + public static final double kDefaultPeriod = 0.02; + /** * Motor CAN id's. PID constants for Swerve Auto Holonomic Drive Controller. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 686b0868..f3148419 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,6 +37,8 @@ public class Robot extends LoggedRobot { public static int column = 0; public static UsbCamera camera = CameraServer.startAutomaticCapture("Magazine Camera", 0); + PowerDistribution powerDistribution = null; + // private Ultrasonic ultrasonic = new Ultrasonic(); /** * This function is run when the robot is first started up and should be used for any @@ -48,7 +50,7 @@ public void robotInit() { if (isReal()) { logger.addDataReceiver(new WPILOGWriter("/media/sda1")); logger.addDataReceiver(new NT4Publisher()); - new PowerDistribution(1, ModuleType.kRev); + powerDistribution = new PowerDistribution(1, ModuleType.kRev); } else { String path = LogFileUtil.findReplayLog(); logger.setReplaySource(new WPILOGReader(path)); @@ -134,4 +136,12 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override public void testPeriodic() {} + + @Override + public void close() { + super.close(); + if (powerDistribution != null) { + powerDistribution.close(); + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62b17d4c..595fddfe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,7 +50,9 @@ import frc.robot.subsystems.swerve2.Swerve; import frc.robot.subsystems.swerve2.SwerveIO; import frc.robot.subsystems.swerve2.SwerveReal; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntakeIO; +import frc.robot.subsystems.wrist_intake.WristIntakeIOFalcon500; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -127,7 +129,7 @@ public class RobotContainer { public final Swerve s_Swerve; // private final DropIntake s_dIntake = new DropIntake(); private final Arm s_Arm; - private final WristIntake s_wristIntake = new WristIntake(); + private final WristIntake s_wristIntake; private ArmIO armIO; @@ -138,10 +140,14 @@ public RobotContainer(boolean isReal) { if (isReal) { armIO = new ArmIOSparkMax(ph); s_Swerve = new Swerve(new SwerveReal()); + s_wristIntake = new WristIntake(new WristIntakeIOFalcon500()); } else { armIO = new ArmIO() {}; s_Swerve = new Swerve(new SwerveIO() { + }); + s_wristIntake = new WristIntake(new WristIntakeIO() { + }); } s_Arm = new Arm(armIO); diff --git a/src/main/java/frc/robot/autos/CrossAndDock.java b/src/main/java/frc/robot/autos/CrossAndDock.java index 73df1e74..8e7c3584 100644 --- a/src/main/java/frc/robot/autos/CrossAndDock.java +++ b/src/main/java/frc/robot/autos/CrossAndDock.java @@ -16,7 +16,7 @@ import frc.robot.commands.drive.TurnToAngle; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Leaves community, and docks. diff --git a/src/main/java/frc/robot/autos/DoubleScore.java b/src/main/java/frc/robot/autos/DoubleScore.java index a79c93ce..fc72b459 100644 --- a/src/main/java/frc/robot/autos/DoubleScore.java +++ b/src/main/java/frc/robot/autos/DoubleScore.java @@ -20,7 +20,7 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score a second game piece diff --git a/src/main/java/frc/robot/autos/DoubleScore6.java b/src/main/java/frc/robot/autos/DoubleScore6.java index 55841219..28d586d7 100644 --- a/src/main/java/frc/robot/autos/DoubleScore6.java +++ b/src/main/java/frc/robot/autos/DoubleScore6.java @@ -14,15 +14,12 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score a second game piece */ public class DoubleScore6 extends TrajectoryBase { - - private double maxVel = 6; - private double maxAccel = 4; // private double armAngle = -60.0; // private double wristAngle = 3.0; Pose2d aprilTag7 = FieldConstants.aprilTags.get(7).toPose2d(); diff --git a/src/main/java/frc/robot/autos/MiddleScoreEngage.java b/src/main/java/frc/robot/autos/MiddleScoreEngage.java index e4fe482a..8b7335ea 100644 --- a/src/main/java/frc/robot/autos/MiddleScoreEngage.java +++ b/src/main/java/frc/robot/autos/MiddleScoreEngage.java @@ -17,7 +17,7 @@ import frc.robot.commands.wrist.WristIntakeRelease; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score one game piece then engage from community side. diff --git a/src/main/java/frc/robot/autos/Score1.java b/src/main/java/frc/robot/autos/Score1.java index 439c5469..96c5d850 100644 --- a/src/main/java/frc/robot/autos/Score1.java +++ b/src/main/java/frc/robot/autos/Score1.java @@ -10,7 +10,7 @@ import frc.robot.commands.wrist.WristIntakeRelease; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score1Dock Auto. diff --git a/src/main/java/frc/robot/autos/Score1Dock.java b/src/main/java/frc/robot/autos/Score1Dock.java index 0ab15550..ffcfffb6 100644 --- a/src/main/java/frc/robot/autos/Score1Dock.java +++ b/src/main/java/frc/robot/autos/Score1Dock.java @@ -17,7 +17,7 @@ import frc.robot.commands.wrist.WristIntakeRelease; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score1Dock Auto. diff --git a/src/main/java/frc/robot/autos/SecondGamePiece.java b/src/main/java/frc/robot/autos/SecondGamePiece.java index 329513c8..035307d4 100644 --- a/src/main/java/frc/robot/autos/SecondGamePiece.java +++ b/src/main/java/frc/robot/autos/SecondGamePiece.java @@ -15,7 +15,7 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** diff --git a/src/main/java/frc/robot/autos/SecondGamePiece6.java b/src/main/java/frc/robot/autos/SecondGamePiece6.java index 2cc7e3b6..8173326b 100644 --- a/src/main/java/frc/robot/autos/SecondGamePiece6.java +++ b/src/main/java/frc/robot/autos/SecondGamePiece6.java @@ -16,7 +16,7 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** diff --git a/src/main/java/frc/robot/autos/SecondGamePieceScore.java b/src/main/java/frc/robot/autos/SecondGamePieceScore.java index af3adf75..509f080c 100644 --- a/src/main/java/frc/robot/autos/SecondGamePieceScore.java +++ b/src/main/java/frc/robot/autos/SecondGamePieceScore.java @@ -10,7 +10,7 @@ import frc.robot.commands.wrist.WristIntakeRelease; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Acquire a second game piece after scoring diff --git a/src/main/java/frc/robot/autos/TripleScore.java b/src/main/java/frc/robot/autos/TripleScore.java index a7ebc373..239008df 100644 --- a/src/main/java/frc/robot/autos/TripleScore.java +++ b/src/main/java/frc/robot/autos/TripleScore.java @@ -11,7 +11,7 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score three game pieces diff --git a/src/main/java/frc/robot/autos/TripleScore6.java b/src/main/java/frc/robot/autos/TripleScore6.java index 34b9c1b7..fb57ccc5 100644 --- a/src/main/java/frc/robot/autos/TripleScore6.java +++ b/src/main/java/frc/robot/autos/TripleScore6.java @@ -12,7 +12,7 @@ import frc.robot.commands.wrist.AutoWrist; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Score three game pieces diff --git a/src/main/java/frc/robot/commands/arm/DockArm.java b/src/main/java/frc/robot/commands/arm/DockArm.java index 6c37dc1d..776c1177 100644 --- a/src/main/java/frc/robot/commands/arm/DockArm.java +++ b/src/main/java/frc/robot/commands/arm/DockArm.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.lib.util.ArmPosition; import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Command to dock the arm in the robot diff --git a/src/main/java/frc/robot/commands/arm/ScoreArm.java b/src/main/java/frc/robot/commands/arm/ScoreArm.java index ca2be325..72487d5c 100644 --- a/src/main/java/frc/robot/commands/arm/ScoreArm.java +++ b/src/main/java/frc/robot/commands/arm/ScoreArm.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.lib.util.Scoring; import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Command to move arm into scoring position diff --git a/src/main/java/frc/robot/commands/drive/MoveToEngage.java b/src/main/java/frc/robot/commands/drive/MoveToEngage.java index d0deb408..989ccaac 100644 --- a/src/main/java/frc/robot/commands/drive/MoveToEngage.java +++ b/src/main/java/frc/robot/commands/drive/MoveToEngage.java @@ -1,11 +1,9 @@ package frc.robot.commands.drive; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.lib.util.FieldConstants; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** @@ -25,14 +23,4 @@ public MoveToEngage(Swerve swerve, Arm arm, WristIntake wristIntake) { ClimbPlatform climbPlatform = new ClimbPlatform(swerve); addCommands(climbPlatform); } - - /** - * Check if the robot's position is inside the community or center of the field - * - * @return True if the robot is in the middle of the field. False if in the community - */ - private boolean centerField() { - Pose2d pose = FieldConstants.allianceFlip(swerve.getPose()); - return pose.getX() > FieldConstants.Community.cableBumpInnerX; - } } diff --git a/src/main/java/frc/robot/commands/drive/MoveToScore.java b/src/main/java/frc/robot/commands/drive/MoveToScore.java index 6ba5dca7..47ba49b7 100644 --- a/src/main/java/frc/robot/commands/drive/MoveToScore.java +++ b/src/main/java/frc/robot/commands/drive/MoveToScore.java @@ -6,7 +6,7 @@ import frc.robot.commands.arm.ScoreArm; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.swerve2.Swerve; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** diff --git a/src/main/java/frc/robot/commands/test/TestArm.java b/src/main/java/frc/robot/commands/test/TestArm.java index f0772dc2..ac1b7e5d 100644 --- a/src/main/java/frc/robot/commands/test/TestArm.java +++ b/src/main/java/frc/robot/commands/test/TestArm.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.lib.util.ArmPosition; import frc.lib.util.Scoring; import frc.lib.util.Scoring.GamePiece; import frc.robot.Robot; @@ -25,7 +24,6 @@ public void initialize() { SmartDashboard.putNumber("Targeted Column", Robot.column); GamePiece gamePiece = Scoring.getGamePiece(); SmartDashboard.putString("Targeted Game Piece", gamePiece.toString()); - ArmPosition position = Scoring.getScoreParameters(); // arm.enablePID(); // arm.setArmGoal(position.getArmAngle()); arm.setArmGoal(60); diff --git a/src/main/java/frc/robot/commands/wrist/AutoWrist.java b/src/main/java/frc/robot/commands/wrist/AutoWrist.java index 3a5e8184..b2489617 100644 --- a/src/main/java/frc/robot/commands/wrist/AutoWrist.java +++ b/src/main/java/frc/robot/commands/wrist/AutoWrist.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Consistent wrist operation for Autonomous. diff --git a/src/main/java/frc/robot/commands/wrist/VariableIntake.java b/src/main/java/frc/robot/commands/wrist/VariableIntake.java index 9355c4b2..392992a9 100644 --- a/src/main/java/frc/robot/commands/wrist/VariableIntake.java +++ b/src/main/java/frc/robot/commands/wrist/VariableIntake.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Variable controlled intake speed. diff --git a/src/main/java/frc/robot/commands/wrist/WristIntakeIn.java b/src/main/java/frc/robot/commands/wrist/WristIntakeIn.java index dc6bfb02..1388d8c2 100644 --- a/src/main/java/frc/robot/commands/wrist/WristIntakeIn.java +++ b/src/main/java/frc/robot/commands/wrist/WristIntakeIn.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Command to raise the Drop Down Intake to the top position diff --git a/src/main/java/frc/robot/commands/wrist/WristIntakeRelease.java b/src/main/java/frc/robot/commands/wrist/WristIntakeRelease.java index 8a3370bb..56932335 100644 --- a/src/main/java/frc/robot/commands/wrist/WristIntakeRelease.java +++ b/src/main/java/frc/robot/commands/wrist/WristIntakeRelease.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.lib.util.Scoring; import frc.robot.Constants; -import frc.robot.subsystems.wristIntake.WristIntake; +import frc.robot.subsystems.wrist_intake.WristIntake; /** * Command to raise the Drop Down Intake to the top position diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 16af1c10..e9b5e694 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -38,6 +38,7 @@ public class Arm extends SubsystemBase { */ public Arm(ArmIO armIO) { io = armIO; + io.updateInputs(armInputs); this.wristPIDController.setIntegratorRange(Constants.Wrist.PID.MIN_INTEGRAL, diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java index 8a71fa17..904ab0f7 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -2,8 +2,10 @@ import org.littletonrobotics.junction.AutoLog; +/** IO Class for Arm */ public class ArmIO { + /** Inputs Class for Arm */ @AutoLog public static class ArmInputs { public double armAngleRad; diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index d6c39732..8face861 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.PneumaticHub; import frc.robot.Constants; +/** Real Class for Arm */ public class ArmIOSparkMax extends ArmIO { private final CANSparkMax armMotor1 = @@ -23,6 +24,7 @@ public class ArmIOSparkMax extends ArmIO { new CANSparkMax(Constants.Wrist.WRIST_MOTOR_ID, MotorType.kBrushless); private final AbsoluteEncoder wristEncoder = wristMotor.getAbsoluteEncoder(Type.kDutyCycle); + /** Initializer for real Arm */ public ArmIOSparkMax(PneumaticHub ph) { // ARM armMotor1.restoreFactoryDefaults(); diff --git a/src/main/java/frc/robot/subsystems/swerve2/Swerve.java b/src/main/java/frc/robot/subsystems/swerve2/Swerve.java index 3d111103..7a2a53a6 100644 --- a/src/main/java/frc/robot/subsystems/swerve2/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve2/Swerve.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.swerve2; -import java.util.Map; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -11,8 +10,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,6 +18,7 @@ import frc.robot.Constants; import frc.robot.RobotContainer; +/** Swerve drivebase subsystem */ public class Swerve extends SubsystemBase { private SwerveDrivePoseEstimator swerveOdometry; @@ -31,15 +29,18 @@ public class Swerve extends SubsystemBase { // private ComplexWidget fieldWidget = RobotContainer.mainDriverTab.add("Field Pos", field) // .withWidget(BuiltInWidgets.kField).withSize(8, 4) // make the widget 2x1 // .withPosition(0, 0); // place it in the top-left corner - private GenericEntry aprilTagTarget = RobotContainer.autoTab - .add("Currently Seeing April Tag", false).withWidget(BuiltInWidgets.kBooleanBox) - .withProperties(Map.of("Color when true", "green", "Color when false", "red")) - .withPosition(8, 4).withSize(2, 2).getEntry(); + + /* + * private GenericEntry aprilTagTarget = RobotContainer.autoTab + * .add("Currently Seeing April Tag", false).withWidget(BuiltInWidgets.kBooleanBox) + * .withProperties(Map.of("Color when true", "green", "Color when false", "red")) + * .withPosition(8, 4).withSize(2, 2).getEntry(); + */ private SwerveIO io; private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); - private SwerveModule swerveMods[] = new SwerveModule[4]; + private SwerveModule[] swerveMods = new SwerveModule[4]; /** * Initializes swerve modules. @@ -84,7 +85,7 @@ public void wheelsIn() { public void driveWithTwist(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { - double dt = TimedRobot.kDefaultPeriod; + double dt = Constants.kDefaultPeriod; ChassisSpeeds chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), diff --git a/src/main/java/frc/robot/subsystems/swerve2/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve2/SwerveIO.java index af9ce368..08d85bc2 100644 --- a/src/main/java/frc/robot/subsystems/swerve2/SwerveIO.java +++ b/src/main/java/frc/robot/subsystems/swerve2/SwerveIO.java @@ -4,8 +4,10 @@ import frc.lib.util.swerve.SwerveModule; import frc.lib.util.swerve.SwerveModuleIO; +/** IO Class for Swerve */ public interface SwerveIO { + /** Inputs Class for Swerve */ @AutoLog public static class SwerveInputs { public float yaw; diff --git a/src/main/java/frc/robot/subsystems/swerve2/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve2/SwerveReal.java index a953be5f..1a768f24 100644 --- a/src/main/java/frc/robot/subsystems/swerve2/SwerveReal.java +++ b/src/main/java/frc/robot/subsystems/swerve2/SwerveReal.java @@ -4,8 +4,14 @@ import frc.lib.util.swerve.SwerveModuleConstants; import frc.lib.util.swerve.SwerveModuleReal; +/** Real Class for Swerve */ public class SwerveReal implements SwerveIO { + /** Real Swerve Initializer */ + public SwerveReal() { + + } + @Override public void updateInputs(SwerveInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/wristIntake/WristIntake.java b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntake.java similarity index 73% rename from src/main/java/frc/robot/subsystems/wristIntake/WristIntake.java rename to src/main/java/frc/robot/subsystems/wrist_intake/WristIntake.java index 049c5fff..b8304419 100644 --- a/src/main/java/frc/robot/subsystems/wristIntake/WristIntake.java +++ b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntake.java @@ -1,5 +1,6 @@ -package frc.robot.subsystems.wristIntake; +package frc.robot.subsystems.wrist_intake; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -10,7 +11,7 @@ public class WristIntake extends SubsystemBase { private boolean invertForCone = true; - private WristIntakeIOFalcon500 io; + private WristIntakeIO io; private WristIntakeInputsAutoLogged wristIntakeAutoLogged = new WristIntakeInputsAutoLogged(); @@ -18,13 +19,16 @@ public class WristIntake extends SubsystemBase { /** * Create Wrist Intake Subsystem */ - public WristIntake() { - io = new WristIntakeIOFalcon500(); + public WristIntake(WristIntakeIO io) { + this.io = io; + io.updateInputs(wristIntakeAutoLogged); } @Override public void periodic() { - SmartDashboard.putNumber("Wrist Intake Current", io.getOutputCurrentAmps()); + io.updateInputs(wristIntakeAutoLogged); + Logger.getInstance().processInputs("WristIntake", wristIntakeAutoLogged); + SmartDashboard.putNumber("Wrist Intake Current", wristIntakeAutoLogged.outputCurrentAmps); } // Runs wrist intake motor to intake a game piece. @@ -62,7 +66,7 @@ public void panic() { * Get if motor is stalling by checking if the stator current exceeds a constant */ public boolean getVoltageSpike(boolean passedTime) { - return io.getOutputCurrentAmps() > Constants.Wrist.STALL_CURRENT; + return wristIntakeAutoLogged.outputCurrentAmps > Constants.Wrist.STALL_CURRENT; } public void setInvert(boolean invert) { diff --git a/src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIO.java b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIO.java similarity index 58% rename from src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIO.java rename to src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIO.java index 49140ff8..15f867a9 100644 --- a/src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIO.java +++ b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIO.java @@ -1,23 +1,23 @@ -package frc.robot.subsystems.wristIntake; +package frc.robot.subsystems.wrist_intake; import org.littletonrobotics.junction.AutoLog; +/** IO Class for WristIntake */ public interface WristIntakeIO { + /** Inputs Class for WristIntake */ @AutoLog public static class WristIntakeInputs { public double wristAngleRad; public double wristVelocityRotPerSecond; + public double voltage; + public double outputCurrentAmps; } + public default void updateInputs(WristIntakeInputs inputs) {} + public default void setMotorVoltage(double voltage) {} public default void setMotorPercentage(double percent) {} - public default void getVoltage() {} - - public default double getOutputCurrentAmps() { - return 0.0; - } - } diff --git a/src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIOFalcon500.java b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIOFalcon500.java similarity index 75% rename from src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIOFalcon500.java rename to src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIOFalcon500.java index ddebd808..2fa50769 100644 --- a/src/main/java/frc/robot/subsystems/wristIntake/WristIntakeIOFalcon500.java +++ b/src/main/java/frc/robot/subsystems/wrist_intake/WristIntakeIOFalcon500.java @@ -1,22 +1,24 @@ -package frc.robot.subsystems.wristIntake; +package frc.robot.subsystems.wrist_intake; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import frc.robot.Constants; +/** Real Class for WristIntake */ public class WristIntakeIOFalcon500 implements WristIntakeIO { private final WPI_TalonFX wristIntakeMotor = new WPI_TalonFX(Constants.Wrist.WRIST_INTAKE_ID); + /** Initializer for real WristIntake */ public WristIntakeIOFalcon500() { wristIntakeMotor.setInverted(false); wristIntakeMotor.setNeutralMode(NeutralMode.Brake); } @Override - public double getOutputCurrentAmps() { - return wristIntakeMotor.getStatorCurrent(); + public void updateInputs(WristIntakeInputs inputs) { + inputs.outputCurrentAmps = wristIntakeMotor.getStatorCurrent(); } @Override