diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 6fb5335..f8acad3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,7 +15,9 @@ import java.util.logging.Level; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.CanDevice; @@ -49,6 +51,26 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + + + new Thread() { + public void run() { + try{ + while(!this.isInterrupted() && this.isAlive()){ + Thread.sleep(500); + for(int i=0;i subsystems = new ArrayList<>(); + // public List subsystems = new ArrayList<>(); // ! Teleop Commands @@ -103,11 +103,11 @@ 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); + // 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(() -> { diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index feb10e1..0cffe0b 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -44,6 +44,8 @@ public class DiffDrive extends Subsystem { */ public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor, TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { + + super(); m_leftFrontMotor = leftFrontMotor; m_rightFrontMotor = rightFrontMotor; @@ -98,10 +100,8 @@ public class DiffDrive extends Subsystem { } @Override - public Status queryStatus() { - Status status = new Status(); + public void queryStatus() { // TODO: Add Stuff - return status; } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0f9d8e4..cafe872 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -55,6 +55,7 @@ public class SwerveDrive extends Subsystem { /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + super(); this.leftFront = leftFront; this.rightFront = rightFront; this.leftBack = leftBack; @@ -334,14 +335,17 @@ public class SwerveDrive extends Subsystem { } @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); + public void queryStatus() { + + SmartDashboard.putNumber("[" + getSubsystemName() + "] Gyro angle", this.gyro.getAngle()); + SmartDashboard.putNumber("[" + getSubsystemName() + "] Shift State", this.speedAdjust); + + // this.leftFront.queryStatus(); + // this.leftBack.queryStatus(); + // this.rightFront.queryStatus(); + // this.rightBack.queryStatus(); + //TODO: Add more status things - - return status; } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 3de23be..2ee9ca6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -33,6 +33,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; @@ -56,6 +57,7 @@ public class SwerveModule extends Subsystem { /** Creates a new SwerveModule. */ public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) { + super(); this.name = name; this.driveMotor = driveMotor; this.angleMotor = angleMotor; @@ -242,15 +244,10 @@ public class SwerveModule extends Subsystem { } @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()); + public void queryStatus() { + SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get()); + SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble()); //TODO: Add more status things - - - return status; } public boolean motorsAlive() { diff --git a/src/main/java/frc4388/utility/CanDevice.java b/src/main/java/frc4388/utility/CanDevice.java index 6a6db56..ec05fdb 100644 --- a/src/main/java/frc4388/utility/CanDevice.java +++ b/src/main/java/frc4388/utility/CanDevice.java @@ -52,4 +52,6 @@ public class CanDevice { return s; } + + } diff --git a/src/main/java/frc4388/utility/Subsystem.java b/src/main/java/frc4388/utility/Subsystem.java index 246bb76..961900d 100644 --- a/src/main/java/frc4388/utility/Subsystem.java +++ b/src/main/java/frc4388/utility/Subsystem.java @@ -1,16 +1,25 @@ package frc4388.utility; +import java.util.ArrayList; +import java.util.List; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public abstract class Subsystem extends SubsystemBase { + public static List subsystems = new ArrayList<>(); + + public Subsystem(){ + subsystems.add(this); + } + 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(); + // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard + public abstract void queryStatus(); // Proactivly search for any errors in each subsystem public abstract Status diagnosticStatus(); }