66-logging-util Initial Implementation

src/main/java/frc/robot/Robot.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.util.Logger;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -18,6 +19,10 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  private Logger m_logger;
+
+  private final double m_logInterval = 6000;
+  private double m_logTime = 0;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -28,6 +33,7 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    m_logger = m_robotContainer.getLogger();
   }
 
   /**
@@ -44,6 +50,16 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // Logs the Robot Data
+    if(m_logTime >= m_logInterval) {
+      m_logTime = 0;
+      m_logger.updateFields();
+      m_logger.log();
+    } else {
+      m_logTime += 20;
+    }
+
   }
 
   /** This function is called once each time the robot enters Disabled mode. */

src/main/java/frc/robot/RobotContainer.java

@@ -4,6 +4,9 @@
 
 package frc.robot;
 
+import java.time.LocalDateTime;
+import java.time.format.DateTimeFormatter;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
@@ -20,6 +23,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.NavXGyroSubsystem;
+import frc.robot.util.Logger;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -35,7 +39,9 @@ public class RobotContainer {
 
   private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
   private final BalanceCommand m_balancecommand;
+
   private final DriveStraightCommand m_drivestraightcommand;
+
   private DriveCommand teleopDriveCmd;
 
   private DrivetrainSubsystem drivetrainSubsystem;
@@ -47,6 +53,8 @@ public class RobotContainer {
 
   private XboxController xboxController;
 
+  private Logger m_logger;
+
   public double getRightY() {
     if (!DriverStation.isJoystickConnected(ControllerConstants.kXboxControllerPort)) {
       return Math.abs(rightJoystick.getY()) > ControllerConstants.kDeadZoneRadius ? -rightJoystick.getY() : 0;
@@ -91,6 +99,33 @@ public RobotContainer() {
     this.m_drivestraightcommand = new DriveStraightCommand(navxGyroSubsystem, drivetrainSubsystem, m_blinkinSubsystem, this::getLeftY,this::getRightY, this::getThrottle);
     // this.colorSensorSubsystem.setDefaultCommand(colorSensorCommand);   <--- Causes an error right now
 
+    // Creates the logger
+    m_logger = new Logger("/home/lvuser/logs/", "ezlog");
+
+    // Sets the data to
+    {
+      //DateTimeFormatter dtf = DateTimeFormatter.ofPattern("yyyy/MM/dd HH:mm:ss");
+      //LocalDateTime now = LocalDateTime.now();
+
+      //m_logger.createStaticField("Time", dtf.format(now));  //DOESN'T WORK
+      m_logger.createStaticField("Event", "CORI Preliminaries 1");
+      m_logger.recordFrequency(.250);
+
+      /*
+      *      m_leftB(0)     m_rightB(1)
+      * 
+      *      m_leftA(2)     m_rightA(3)
+      */
+      m_logger.createDynamicFieldDouble("Left B Motor Speed", drivetrainSubsystem.getMotorSpeed(0), () -> drivetrainSubsystem.getMotorSpeed(0));
+      m_logger.createDynamicFieldDouble("Right B Motor Speed", drivetrainSubsystem.getMotorSpeed(1), () -> drivetrainSubsystem.getMotorSpeed(1));
+      m_logger.createDynamicFieldDouble("Left A Motor Speed", drivetrainSubsystem.getMotorSpeed(2), () -> drivetrainSubsystem.getMotorSpeed(2));
+      m_logger.createDynamicFieldDouble("Right A Motor Speed", drivetrainSubsystem.getMotorSpeed(3), () -> drivetrainSubsystem.getMotorSpeed(3));
+
+      m_logger.setup();
+    }
+    // NOTE: The logs can be accessed through File Explorer by typing into the bar:
+    // ftp://roborio-1014-frc.local/home/lvuser/logs/
+
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -131,4 +166,13 @@ public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
     return m_autoCommand;
   }
+
+  /**
+   * Use this to pass the logger to the main {@link Robot} class to schedule updates.
+   *
+   * @return the logger
+   */
+  public Logger getLogger() {
+    return m_logger;
+  }
 }

src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java

@@ -12,6 +12,8 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.MovementConstants;
 
+import java.io.IOException;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel;
 import com.revrobotics.CANSparkMax.IdleMode;
@@ -87,4 +89,20 @@ private static double clampPower(double power) {
     public void stop() {
         m_driveTrain.stopMotor();
     }
+
+    /*
+     *      m_leftB(0)     m_rightB(1)
+     * 
+     *      m_leftA(2)     m_rightA(3)
+     */
+    public double[] getMotorSpeeds() {
+        return new double[]{m_leftB.get(), m_rightB.get(), m_leftA.get(), m_rightA.get()};
+    }
+
+    public double getMotorSpeed(int index) {
+        if(index < 0 || index > getMotorSpeeds().length-1)
+            return -999;
+
+        return getMotorSpeeds()[index];
+    }
 }

src/main/java/frc/robot/util/DynamicField.java

@@ -0,0 +1,26 @@
+package frc.robot.util;
+
+import java.util.function.Supplier;
+
+public class DynamicField extends StaticField {
+
+    private Supplier<Object> supplier;
+
+    /**
+     * A supplier "broker". Stores the name of the measurement, latest value, and the supplier of the values.
+     * Uses Object as a flexible data type.
+     *
+     * @param n     field name
+     * @param v     initial value
+     * @param s     value supplier/source
+     */
+    public DynamicField(String n, Object v, Supplier<Object> s) {
+        super(n,v);
+
+        supplier = s;
+    }
+
+    public void update() {
+        super.setValue(supplier.get());
+    }
+}

src/main/java/frc/robot/util/Logger.java

@@ -0,0 +1,153 @@
+package frc.robot.util;
+
+import java.io.*;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.StringJoiner;
+import java.util.function.Supplier;
+
+/**
+ * Responsible for recording the sensor data
+ */
+public class Logger {
+
+    private String path;
+    private File file;
+
+    List<DynamicField> dynamicFields;
+
+    // TODO: Look into the datalog tool to be able to automatically pull the log file off the robot
+    // TODO: Use the RobotSimulation to test the logging utility
+    // TODO: Track Autonomous and Teleop Status
+    public Logger(String p, String name) {
+        path = p;
+        try {
+            file = new File(path + name + ".csv");
+            if (file.createNewFile()) {
+                System.out.println("File Created: " + file.getName());
+            } else {
+                System.out.println("File already exists.");
+            }
+
+            // Wipes the log if it already contains text
+            FileWriter myWriter = new FileWriter(file,false);
+            myWriter.close();
+
+        } catch (IOException e) {
+            System.out.println("Unable to create a file. ");
+            e.printStackTrace();
+        }
+
+        dynamicFields = new ArrayList<>();
+    }
+
+    public void log() {
+        try {
+            // TODO: Do not close the writer after each write
+            FileWriter myWriter = new FileWriter(file,true);
+
+            myWriter.write("\n");
+
+            List<String> values = new ArrayList<>();
+
+            for(DynamicField dF : dynamicFields) {
+                values.add(dF.getValue().toString());
+            }
+
+            myWriter.write(String.join(",", values));
+            myWriter.close();
+        } catch (IOException e) {
+            System.out.println("Could not write to file.");
+            e.printStackTrace();
+        }
+    }
+
+    /**
+     * Sets up the Dynamic Fields 
+     */ 
+    public void setup() {
+        try {
+            FileWriter myWriter = new FileWriter(file,true);
+
+            //myWriter.write("Created using FREZ Log");
+
+            myWriter.write("---\n");
+
+            List<String> variableNames = new ArrayList<>();
+
+            for(DynamicField dF : dynamicFields) {
+                variableNames.add(dF.getName());
+            }
+
+            myWriter.write(String.join(",", variableNames));
+
+            myWriter.close();
+        } catch (IOException e) {
+            System.out.println("Could not write to file.");
+            e.printStackTrace();
+        }
+    }
+
+    public void createDynamicFieldDouble(String n, Double v, Supplier<Double> s){
+        // This causes a runtime error which can prevent logging a value of a wrong type
+        createDynamicField(n, v, () -> s.get());
+    }
+
+    public void createDynamicFieldString(String n, Object v, Supplier<String> s){
+        // This causes a runtime error which can prevent logging a value of a wrong type
+        createDynamicField(n, v, () -> s.get());
+    }
+
+    public void createDynamicFieldBoolean(String n, Boolean v, Supplier<Boolean> s){
+        // This causes a runtime error which can prevent logging a value of a wrong type
+        createDynamicField(n, booleanToInt(v), () -> booleanToInt(s.get()));
+    }
+
+    private void createDynamicField(String n, Object v, Supplier<Object> s) {
+        DynamicField newField = new DynamicField(n, v, s);
+        dynamicFields.add(newField);
+    }
+
+    public void createStaticField(String n, Object v) {
+        StaticField newField = new StaticField(n, v);
+        writeStaticField(n, v);
+    }
+
+    /**
+     * Records the Frequency of the Logs (does not determine how often they are actually created)
+     *
+     * @param timeUnit - decimal of a second that represents how often the logs are updated
+     */
+    public void recordFrequency(double timeUnit) {
+        // t? is the property tag to signify time unit
+        createStaticField("t?Time Unit", timeUnit);
+    }
+
+    public void writeStaticField(String n, Object v) {
+        try {
+            FileWriter myWriter = new FileWriter(file,true);
+
+            myWriter.write(n+","+v+"\n");
+
+            myWriter.close();
+        } catch (IOException e) {
+            System.out.println("Could not write to file.");
+            e.printStackTrace();
+        }
+    }
+
+    public void updateFields() {
+        for(DynamicField field : dynamicFields) {
+            field.update();
+        }
+
+        System.out.println("Updated Dynamic Fields.");
+    }
+
+    public int booleanToInt(boolean bool) {
+        if(bool)
+            return 1;
+        return 0;
+    }
+}

src/main/java/frc/robot/util/StaticField.java

@@ -0,0 +1,30 @@
+package frc.robot.util;
+
+import java.util.Objects;
+import java.util.function.Supplier;
+
+/**
+ * Struct for storing the sensor data field
+ */
+
+public class StaticField{
+    private String name;
+    private Object value;
+
+    public StaticField(String n, Object v) {
+        name = n;
+        value = v;
+    }
+
+    public Object getValue() {
+        return value;
+    }
+
+    public void setValue(Object v) {
+        value = v;
+    }
+
+    public String getName() {
+        return name;
+    }
+}