From f780dc863bf74aa506988b2227c4cf54f602cb71 Mon Sep 17 00:00:00 2001 From: Kirill Truntaev Date: Sat, 25 Feb 2023 12:41:57 -0500 Subject: [PATCH 1/5] Implemented Data Logging --- src/main/java/frc/robot/Robot.java | 16 ++ src/main/java/frc/robot/RobotContainer.java | 35 ++++ .../robot/subsystems/DrivetrainSubsystem.java | 4 + .../java/frc/robot/util/DynamicField.java | 26 +++ src/main/java/frc/robot/util/Logger.java | 153 ++++++++++++++++++ src/main/java/frc/robot/util/StaticField.java | 30 ++++ 6 files changed, 264 insertions(+) create mode 100644 src/main/java/frc/robot/util/DynamicField.java create mode 100644 src/main/java/frc/robot/util/Logger.java create mode 100644 src/main/java/frc/robot/util/StaticField.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..e807864 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); + + // REMEMBER TO MOVE THIS OUT OF ROBOT PERIODIC + 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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4367f38..d6873d9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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; @@ -19,6 +22,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 @@ -34,6 +38,7 @@ public class RobotContainer { private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); private final BalanceCommand m_balancecommand; + private DriveCommand teleopDriveCmd; private DrivetrainSubsystem drivetrainSubsystem; @@ -45,6 +50,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; @@ -87,6 +94,25 @@ public RobotContainer() { this.m_balancecommand = new BalanceCommand(navxGyroSubsystem, m_blinkinSubsystem, drivetrainSubsystem); // 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_logger.createDynamicFieldDouble("Motor Speed", drivetrainSubsystem.getMotorSpeed(), drivetrainSubsystem::getMotorSpeed); + + 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(); } @@ -118,4 +144,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; + } } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index e4979fd..c4935c1 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -87,4 +87,8 @@ private static double clampPower(double power) { public void stop() { m_driveTrain.stopMotor(); } + + public double getMotorSpeed() { + return m_leftA.get(); + } } diff --git a/src/main/java/frc/robot/util/DynamicField.java b/src/main/java/frc/robot/util/DynamicField.java new file mode 100644 index 0000000..3c7272d --- /dev/null +++ b/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 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 s) { + super(n,v); + + supplier = s; + } + + public void update() { + super.setValue(supplier.get()); + } +} diff --git a/src/main/java/frc/robot/util/Logger.java b/src/main/java/frc/robot/util/Logger.java new file mode 100644 index 0000000..c96051b --- /dev/null +++ b/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 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 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 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 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 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 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 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; + } +} diff --git a/src/main/java/frc/robot/util/StaticField.java b/src/main/java/frc/robot/util/StaticField.java new file mode 100644 index 0000000..bc3a66d --- /dev/null +++ b/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; + } +} From b3ca77124fdcff01564e95d6ebc0e4ad0ab87c69 Mon Sep 17 00:00:00 2001 From: Kirill Truntaev Date: Sat, 11 Mar 2023 11:19:53 -0500 Subject: [PATCH 2/5] Minor Fixes --- src/main/java/frc/robot/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e807864..649adcc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -52,6 +52,7 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); // REMEMBER TO MOVE THIS OUT OF ROBOT PERIODIC + // Logs the Robot Data if(m_logTime >= m_logInterval) { m_logTime = 0; m_logger.updateFields(); From 674e254598066487d1429df54643d53a42516370 Mon Sep 17 00:00:00 2001 From: Kirill Truntaev Date: Sat, 11 Mar 2023 11:20:29 -0500 Subject: [PATCH 3/5] Minor Fixes --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6873d9..ce13b13 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,10 +99,10 @@ public RobotContainer() { // Sets the data to { - DateTimeFormatter dtf = DateTimeFormatter.ofPattern("yyyy/MM/dd HH:mm:ss"); - LocalDateTime now = LocalDateTime.now(); + //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("Time", dtf.format(now)); //DOESN'T WORK m_logger.createStaticField("Event", "CORI Preliminaries 1"); m_logger.recordFrequency(.250); From ca6ab229a7f7548b921bb0abed4e9f5e2abe075a Mon Sep 17 00:00:00 2001 From: Kirill Truntaev Date: Thu, 16 Mar 2023 09:44:01 -0400 Subject: [PATCH 4/5] Minor Fixes --- src/main/java/frc/robot/RobotContainer.java | 10 +++++++++- .../robot/subsystems/DrivetrainSubsystem.java | 18 ++++++++++++++++-- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce13b13..7003bd8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,7 +106,15 @@ public RobotContainer() { m_logger.createStaticField("Event", "CORI Preliminaries 1"); m_logger.recordFrequency(.250); - m_logger.createDynamicFieldDouble("Motor Speed", drivetrainSubsystem.getMotorSpeed(), drivetrainSubsystem::getMotorSpeed); + /* + * 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(); } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index c4935c1..7597a23 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/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; @@ -88,7 +90,19 @@ public void stop() { m_driveTrain.stopMotor(); } - public double getMotorSpeed() { - return m_leftA.get(); + /* + * 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]; } } From 86fb5d9a2d9d44359aeaf2f31078162f9774e354 Mon Sep 17 00:00:00 2001 From: Kirill Truntaev Date: Wed, 5 Apr 2023 21:44:06 -0400 Subject: [PATCH 5/5] Update Robot.java --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 649adcc..04df5d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -51,7 +51,6 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // REMEMBER TO MOVE THIS OUT OF ROBOT PERIODIC // Logs the Robot Data if(m_logTime >= m_logInterval) { m_logTime = 0;