From bb1c58764e9b5430fc0f8eb04d97077437ff06d7 Mon Sep 17 00:00:00 2001 From: nathanrsxtn <37890449+nathanrsxtn@users.noreply.github.com> Date: Sun, 30 Jan 2022 00:39:17 -0700 Subject: [PATCH] Cache swerve module CANCoder feedback coefficients - Add a proper logging system --- build.gradle | 1 + src/main/java/frc4388/robot/Main.java | 5 +- src/main/java/frc4388/robot/Robot.java | 18 ++- .../java/frc4388/robot/subsystems/LED.java | 8 +- .../robot/subsystems/SwerveModule.java | 5 +- .../java/frc4388/utility/AnsiLogging.java | 107 ++++++++++++++++++ 6 files changed, 136 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc4388/utility/AnsiLogging.java diff --git a/build.gradle b/build.gradle index c16e3c3..e453dcb 100644 --- a/build.gradle +++ b/build.gradle @@ -55,6 +55,7 @@ def includeDesktopSupport = true dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation 'org.fusesource.jansi:jansi:2.4.0' roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 544abf0..21f4236 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -4,7 +4,9 @@ package frc4388.robot; + import edu.wpi.first.wpilibj.RobotBase; +import frc4388.utility.AnsiLogging; /** * Do NOT add any static variables to this class, or any initialization at all. @@ -21,9 +23,10 @@ public final class Main { *

If you change your main robot class, change the parameter type. */ public static void main(String... args) { + AnsiLogging.systemInstall(); RobotBase.startRobot(Robot::new); } } -// hi ryan \ No newline at end of file +// hi ryan \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index bc80766..f313d27 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -4,9 +4,11 @@ package frc4388.robot; +import java.util.logging.Level; +import java.util.logging.Logger; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -20,6 +22,7 @@ import frc4388.utility.RobotTime; * project. */ public class Robot extends TimedRobot { + private static final Logger LOGGER = Logger.getLogger(Robot.class.getName()); Command m_autonomousCommand; private RobotTime m_robotTime = RobotTime.getInstance(); @@ -31,6 +34,16 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + if (org.fusesource.jansi.Ansi.isEnabled()) { + LOGGER.log(Level.ALL, "Logging Test 1/7"); + LOGGER.log(Level.WARNING, "Logging Test 2/7"); + LOGGER.log(Level.INFO, "Logging Test 3/7"); + LOGGER.log(Level.CONFIG, "Logging Test 4/7"); + LOGGER.log(Level.FINE, "Logging Test 5/7"); + LOGGER.log(Level.FINER, "Logging Test 6/7"); + LOGGER.log(Level.FINEST, "Logging Test 7/7"); + } + LOGGER.fine("robotInit()"); // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -64,6 +77,7 @@ public class Robot extends TimedRobot { */ @Override public void disabledInit() { + LOGGER.fine("disabledInit()"); m_robotTime.endMatchTime(); } @@ -79,6 +93,7 @@ public class Robot extends TimedRobot { */ @Override public void autonomousInit() { + LOGGER.fine("autonomousInit()"); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); /*String autoSelected = SmartDashboard.getString("Auto Selector", "Default"); @@ -108,6 +123,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + LOGGER.fine("teleopInit()"); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 341671f..eaab9dd 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -4,10 +4,10 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.logging.Logger; +import edu.wpi.first.wpilibj.motorcontrol.Spark; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; @@ -27,7 +27,7 @@ public class LED extends SubsystemBase { m_LEDController = LEDController; setPattern(LEDConstants.DEFAULT_PATTERN); updateLED(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); + Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1e63336..322ccef 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -27,13 +27,14 @@ public class SwerveModule extends SubsystemBase { private static double kEncoderTicksPerRotation = 4096; private SwerveModuleState state; - + private double canCoderFeedbackCoefficient; /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.canCoder = canCoder; + canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient(); TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration(); @@ -82,7 +83,7 @@ public class SwerveModule extends SubsystemBase { // Find the new absolute position of the module based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation; // Convert the CANCoder from it's position reading back to ticks - double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); + double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient; double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle){ angleMotor.set(TalonFXControlMode.Position, desiredTicks); diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java new file mode 100644 index 0000000..1b6c6c6 --- /dev/null +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -0,0 +1,107 @@ +package frc4388.utility; + +import static org.fusesource.jansi.Ansi.ansi; + +import java.io.IOException; +import java.io.OutputStream; +import java.io.PrintStream; +import java.io.PrintWriter; +import java.io.StringWriter; +import java.time.ZoneId; +import java.time.ZonedDateTime; +import java.util.logging.ConsoleHandler; +import java.util.logging.Formatter; +import java.util.logging.Level; +import java.util.logging.LogManager; +import java.util.logging.LogRecord; +import java.util.logging.Logger; + +import org.fusesource.jansi.Ansi; +import org.fusesource.jansi.Ansi.Attribute; +import org.fusesource.jansi.Ansi.Color; +import org.fusesource.jansi.AnsiConsole; + +public class AnsiLogging extends ConsoleHandler { + public static void systemInstall() { + try { + // Configures java.util.logging.Logger to output additional colored information. + LogManager.getLogManager().updateConfiguration(key -> (o, n) -> { + switch (key) { + case ".level": return Level.ALL.getName(); + case "handlers": return AnsiColorConsoleHandler.class.getName(); + default: return n; + } + }); + // Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams. + AnsiConsole.systemInstall(); + // Replaces standard output stream with java.util.logging.Logger. + System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO)); + // Replaces standard error output stream with java.util.logging.Logger. + System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE)); + } catch (IOException exception) { + exception.printStackTrace(AnsiConsole.sysErr()); + } + } + + public static class AnsiColorConsoleHandler extends ConsoleHandler { + @Override + public void publish(LogRecord logRecord) { + AnsiConsole.err().print(getFormatter().format(logRecord)); + AnsiConsole.err().flush(); + } + @Override + public Formatter getFormatter() { + return formatter; + } + private static final Formatter formatter = new Formatter() { + @Override + public String format(LogRecord logRecord) { + ZonedDateTime zdt = ZonedDateTime.ofInstant(logRecord.getInstant(), ZoneId.systemDefault()); + String source; + if (logRecord.getSourceClassName() != null && !logRecord.getSourceClassName().startsWith(AnsiLogging.class.getName())) { + source = logRecord.getSourceClassName(); + if (logRecord.getSourceMethodName() != null) { + source += " " + logRecord.getSourceMethodName(); + } + } else + source = logRecord.getLoggerName(); + String message = formatMessage(logRecord); + String throwable = ""; + if (logRecord.getThrown() != null) { + StringWriter sw = new StringWriter(); + try (PrintWriter pw = new PrintWriter(sw)) { + pw.println(); + logRecord.getThrown().printStackTrace(pw); + } + throwable = sw.toString(); + } + Ansi ansi; + if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED); + else if (logRecord.getLevel() == Level.WARNING) ansi = ansi().fgBright(Color.YELLOW); + else if (logRecord.getLevel() == Level.INFO) ansi = ansi().fg(Color.GREEN); + else if (logRecord.getLevel() == Level.CONFIG) ansi = ansi().fgBright(Color.BLUE); + else if (logRecord.getLevel() == Level.FINE) ansi = ansi().fg(Color.CYAN); + else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA); + else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK); + else ansi = ansi().fg(Color.DEFAULT); + String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString(); + return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable); + } + }; + } + + private static PrintStream printStreamLogger(Logger logger, Level level) { + return new PrintStream(new OutputStream() { + private final StringBuilder stringBuilder = new StringBuilder(); + + @Override + public final void write(int i) throws IOException { + if (i == '\n') { + logger.log(level, stringBuilder::toString); + stringBuilder.setLength(0); + } else + stringBuilder.appendCodePoint(i); + } + }); + } +} \ No newline at end of file