diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 58adaea..251d3f7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,11 +7,17 @@ package frc4388.robot; +import java.util.List; +import java.util.logging.Level; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.Report; //import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the @@ -125,10 +131,18 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); } - /** - * This function is called periodically during test mode. - */ @Override - public void testPeriodic() { + public void testInit() { + for(int i=0;i subsystems = new ArrayList<>(); + // ! Teleop Commands // ! /* Autos */ @@ -96,6 +103,12 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); + this.subsystems.add(m_robotSwerveDrive); + this.subsystems.add(m_robotMap.leftFront); + this.subsystems.add(m_robotMap.rightFront); + this.subsystems.add(m_robotMap.rightBack); + this.subsystems.add(m_robotMap.leftBack); + // ! Swerve Drive One Module Test // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { // m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft()); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 25bf208..0a03c98 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -57,9 +57,9 @@ public class RobotMap { void configureDriveMotorControllers() { // initialize SwerveModules - this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); - this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); - this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); - this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); + this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET); + this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET); + this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET); + this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET); } } diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java deleted file mode 100644 index c6062e8..0000000 --- a/src/main/java/frc4388/robot/subsystems/Apriltags.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.subsystems; - -//import edu.wpi.first.apriltag.AprilTag; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.networktables.NetworkTable; -//import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Apriltags { - public static class Tag { - public boolean visible = true; - public double x, y, z = 0; - public double ry, rp, rr = 0; - } - - public Tag getTagPosRot() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - final Tag tag = new Tag(); - tag.visible = isAprilTag(); - tag.x = tagTable.getEntry("TagPosX").getDouble(0); - tag.y = tagTable.getEntry("TagPosY").getDouble(0); - tag.z = tagTable.getEntry("TagPosZ").getDouble(0); - tag.ry = tagTable.getEntry("TagRotY").getDouble(0); - tag.rp = tagTable.getEntry("TagRotP").getDouble(0); - tag.rr = tagTable.getEntry("TagRotR").getDouble(0); - - return tag; - } - - public boolean isAprilTag() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - return tagTable.getEntry("IsTag").getBoolean(false); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 91de2e9..feb10e1 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import java.util.logging.Level; + import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; @@ -17,11 +19,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; /** * Add your docs here. */ -public class DiffDrive extends SubsystemBase { +public class DiffDrive extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. @@ -85,4 +90,23 @@ public class DiffDrive extends SubsystemBase { SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); //SmartDashboard.putData(m_gyro); } + + + @Override + public String getSubsystemName() { + return "Diff Drive"; + } + + @Override + public Status queryStatus() { + Status status = new Status(); + // TODO: Add Stuff + return status; + } + + @Override + public Status diagnosticStatus() { + Log("Diagnostic info for this has not been inplemented!"); //TODO + return new Status(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index bd35742..636d9b0 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import java.util.logging.Level; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -17,8 +19,11 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.Constants.SwerveDriveConstants.Conversions; import frc4388.utility.RobotGyro; import frc4388.utility.RobotUnits; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; -public class SwerveDrive extends SubsystemBase { +public class SwerveDrive extends Subsystem { private SwerveModule leftFront; private SwerveModule rightFront; @@ -285,29 +290,23 @@ public class SwerveDrive extends SubsystemBase { public void setToSlow() { this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); + for(int i=0;i<5;i++){ + Log("SLOW"); + } } public void setToFast() { this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); + for(int i=0;i<5;i++){ + Log("FAST"); + } } public void setToTurbo() { this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); + for(int i=0;i<5;i++){ + Log("TURBO"); + } } public void toggleGear(double angle) { @@ -328,6 +327,28 @@ public class SwerveDrive extends SubsystemBase { rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED; } + @Override + public String getSubsystemName() { + return "Swerve Drive Controller"; + } + + @Override + public Status queryStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Gyro Angle: " + this.gyro.getAngle()); + status.addReport(ReportLevel.INFO, "Shift State: " + this.speedAdjust); + //TODO: Add more status things + + return status; + } + + @Override + public Status diagnosticStatus() { + Log("Diagnostic info for this has not been inplemented!"); //TODO + return new Status(); + } + } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index b9895fb..6607979 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import java.util.logging.Level; + import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -33,8 +35,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; -public class SwerveModule extends SubsystemBase { +public class SwerveModule extends Subsystem { + private String name = "Null"; private TalonFX driveMotor; private TalonFX angleMotor; private CANcoder encoder; @@ -47,7 +53,8 @@ public class SwerveModule extends SubsystemBase { /** Creates a new SwerveModule. */ - public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { + public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { + this.name = name; this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; @@ -227,6 +234,29 @@ public class SwerveModule extends SubsystemBase { // encoder.setPosition(0); } + @Override + public String getSubsystemName() { + return this.name; + } + + @Override + public Status queryStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Drive motor speed: " + this.driveMotor.get()); + status.addReport(ReportLevel.INFO, "Angle motor speed: " + this.angleMotor.get()); + //TODO: Add more status things + + + return status; + } + + @Override + public Status diagnosticStatus() { + Log("Diagnostic info for this swereve module has not been inplemented!"); //TODO + return new Status(); + } + // public double getCurrent() { // return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); // } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -} diff --git a/src/main/java/frc4388/utility/Status.java b/src/main/java/frc4388/utility/Status.java new file mode 100644 index 0000000..f4a0791 --- /dev/null +++ b/src/main/java/frc4388/utility/Status.java @@ -0,0 +1,39 @@ +package frc4388.utility; + +import java.util.ArrayList; +import java.util.List; + +public class Status { + public enum ReportLevel { + INFO, + WARNING, + ERROR + } + + public class Report { + public ReportLevel reportLevel; + public String description; + + @Override + public String toString() { + return this.reportLevel.name() + ": " + this.description; + } + } + + public List reports; + + public Status() { + this.reports = new ArrayList<>(); + } + + public void addReport(ReportLevel level, String description) { + Report r = new Report(); + r.reportLevel = level; + r.description = description; + this.reports.add(r); + } + + public boolean hasReport() { + return reports.size() == 0; + } +} diff --git a/src/main/java/frc4388/utility/Subsystem.java b/src/main/java/frc4388/utility/Subsystem.java new file mode 100644 index 0000000..246bb76 --- /dev/null +++ b/src/main/java/frc4388/utility/Subsystem.java @@ -0,0 +1,16 @@ +package frc4388.utility; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Subsystem extends SubsystemBase { + public void Log(String str) { + System.out.println(getSubsystemName() + " - " + str); + } + + // Get name of subsystem, for use in log. + public abstract String getSubsystemName(); + // Get what the subystem is currently doing, such as "Shooter spun up" + public abstract Status queryStatus(); + // Proactivly search for any errors in each subsystem + public abstract Status diagnosticStatus(); +}