Cache swerve module CANCoder feedback coefficients

- Add a proper logging system

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)

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 {
    * <p>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 
+// hi ryan

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

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

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);

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);
+      }
+    });
+  }
+}