diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 687a0a0..04df5d5 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(); + + // 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. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9fd15a3..acc402f 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; @@ -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; + } } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 1fbba42..baeffdc 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; @@ -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]; + } } 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; + } +}