Logging

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

@@ -66,6 +66,7 @@
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -148,7 +149,10 @@ public RobotContainer() {
     Preferences.initBoolean("Use Limelight", true);
     Preferences.initBoolean("Use 2 Limelights", true);
     Preferences.initDouble("wait # of seconds", 0);
-
+
+    DataLogManager.start(); //Start logging
+    DriverStation.startDataLog(DataLogManager.getLog()); //Joystick Data logging
+
     // DataLogManager.start();
     // URCL.start();
     // SignalLogger.setPath("/media/sda1/ctre-logs/");
@@ -182,7 +186,7 @@ public RobotContainer() {
     driveTrainInst();
 
 
-    if(intakeExists) {intakeInst();}
+    if(intakeExists) {intakeInst(); /* Must happen before indexInit */}
     if(shooterExists) {shooterInst();}
     if(angleShooterExists) {angleShooterInst();}
     if(climberExists) {climberInst();}
@@ -235,7 +239,7 @@ private void climberInst() {
     climber = new Climber();
   }
   private void indexInit() {
-    indexer = new IndexerSubsystem();
+    indexer = new IndexerSubsystem(intakeExists ? intake::isNoteSeen : () -> false);
   }
   private void indexCommandInst() {
     defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorPreRevSup, intakeReverse);
@@ -536,4 +540,4 @@ public void disabledInit() {
       new InstantCommand(angleShooterSubsystem::stop, angleShooterSubsystem);
     }
   }
-}
+}

src/main/java/frc/robot/helpers/MotorLogger.java

@@ -0,0 +1,34 @@
+package frc.robot.helpers;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+
+/** 
+ * logs moter data to a logger
+ */
+public class MotorLogger {
+    DoubleLogEntry percentOutput;
+    DoubleLogEntry current;
+    DoubleLogEntry velocity;
+
+    public MotorLogger(DataLog log, String path) {
+        percentOutput = new DoubleLogEntry(log, path + "/output");
+        current = new DoubleLogEntry(log, path + "/current");
+        velocity = new DoubleLogEntry(log, path + "/velocity");
+    }
+
+    public void log(CANSparkMax motor) {
+        current.append(motor.getOutputCurrent());
+        percentOutput.append(motor.getAppliedOutput());
+        velocity.append(motor.getEncoder().getVelocity()/60);
+    }
+
+    public void log(TalonFX motor) {
+        current.append(motor.getStatorCurrent().getValueAsDouble());
+        percentOutput.append(motor.get());
+        velocity.append(motor.getVelocity().getValueAsDouble());
+    }
+}

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

@@ -1,10 +1,13 @@
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
+
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
 import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
 import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
@@ -13,22 +16,38 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.revrobotics.SparkAnalogSensor;
 
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.helpers.MotorLogger;
 import frc.robot.settings.Constants.IndexerConstants;
 
 public class IndexerSubsystem extends SubsystemBase {
-    TalonFX indexerMotor;
-    SparkAnalogSensor distanceSensor;
+    TalonFX m_IndexerMotor;
     CurrentLimitsConfigs currentLimitsConfigs;
+    TalonFXConfigurator configurator;
+    BooleanSupplier isNoteIn;
+    double noteStart;
+    boolean wasNoteIn = false;
+    MotorLogger motorLogger;
+    DoubleLogEntry notePositionLog;
 
-    public IndexerSubsystem() {
-
-        indexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR);
+    public IndexerSubsystem(BooleanSupplier isNoteIn) {
+        m_IndexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR);
+        m_IndexerMotor.setInverted(false);
+        this.isNoteIn = isNoteIn;
 
         currentLimitsConfigs = new CurrentLimitsConfigs();
         currentLimitsConfigs.SupplyCurrentLimit = IndexerConstants.CURRENT_LIMIT;
         currentLimitsConfigs.SupplyCurrentLimitEnable = true;
+
+        configurator = m_IndexerMotor.getConfigurator();
+        configurator.apply(currentLimitsConfigs);
+
+        motorLogger = new MotorLogger(DataLogManager.getLog(), "/indexer/motor");
+        notePositionLog = new DoubleLogEntry(DataLogManager.getLog(),"/indexer/notePosistion");
+
         TalonFXConfiguration talonFXConfig = new TalonFXConfiguration().withCurrentLimits(currentLimitsConfigs)
                 .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive))
                 .withSlot0(new Slot0Configs().
@@ -40,7 +59,7 @@ public IndexerSubsystem() {
                         .withMotionMagicCruiseVelocity(IndexerConstants.INDEXER_CRUISE_VELOCITY)
                         .withMotionMagicAcceleration(IndexerConstants.INDEXER_ACCELERATION)
                         .withMotionMagicJerk(IndexerConstants.INDEXER_JERK));
-        indexerMotor.getConfigurator().apply(talonFXConfig);
+        m_IndexerMotor.getConfigurator().apply(talonFXConfig);
 
     }
     /**
@@ -49,50 +68,66 @@ public IndexerSubsystem() {
      * uses percentage of full power
      */
     public void on() {
-        indexerMotor.set(IndexerConstants.INDEXER_INTAKE_SPEED);
+        m_IndexerMotor.set(IndexerConstants.INDEXER_INTAKE_SPEED);
     }
     /**
     sets the indxer motor's percent-of-full-power to 0
      */
     public void off() {
-        indexerMotor.set(0);
+        m_IndexerMotor.set(0);
     }
     /**
      * sets the indexer motor to -INDEXER_INTAKE_SPEED (from constants)
      * <p>
      * uses percentage of full power
      */
     public void reverse() {
-        indexerMotor.set(-IndexerConstants.INDEXER_INTAKE_SPEED);
+        m_IndexerMotor.set(-IndexerConstants.INDEXER_INTAKE_SPEED);
     }
     /**
      * sets the percentage-of-full-power on the indexer
      * @param speed the desired speed, from -1 to 1
      */
     public void set(double speed) {
-        indexerMotor.set(speed);
+        m_IndexerMotor.set(speed);
     }
     /**
      * uses the indexer motor's onboard Motion Magic control to move the indexer forward. To move backwards, use negative inches.
      * @param inches the inches to move forward.
      */
     public void forwardInches(double inches) {
         double rotationsRequested = inches/IndexerConstants.MOTOR_ROTATIONS_TO_INCHES;
-        double position = indexerMotor.getPosition().getValueAsDouble()+rotationsRequested;
+        double position = m_IndexerMotor.getPosition().getValueAsDouble()+rotationsRequested;
         MotionMagicVoltage distanceRequest = new MotionMagicVoltage(position);
-        indexerMotor.setControl(distanceRequest);
+        m_IndexerMotor.setControl(distanceRequest);
+    }
+
+    public void trackNote() {
+        boolean noteIn = isNoteIn.getAsBoolean();
+        if (!wasNoteIn && noteIn) {
+            noteStart = m_IndexerMotor.getPosition().getValueAsDouble();
+        }
+        wasNoteIn = noteIn;
+    }
+
+    public double getNotePosition() {
+        return m_IndexerMotor.getPosition().getValueAsDouble() - noteStart;
     }
     /**
      * uses the indexer motor's onboard Motion Magic control to set the motor to a desired RPS
      * @param RPS the desired speed, in rotations per second
      */
     public void magicRPS(double RPS) {
         MotionMagicVelocityVoltage speedRequest = new MotionMagicVelocityVoltage(RPS);
-        indexerMotor.setControl(speedRequest);
+        m_IndexerMotor.setControl(speedRequest);
     }
 
     @Override
     public void periodic() {
-        SmartDashboard.putNumber("indexer current", indexerMotor.getSupplyCurrent().getValueAsDouble());
+        trackNote();
+        motorLogger.log(m_IndexerMotor);
+        double pos = getNotePosition();
+        notePositionLog.append(pos);
+        SmartDashboard.putNumber("note pos", pos);
     }
 }

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

@@ -10,17 +10,27 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.util.datalog.BooleanLogEntry;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
- import frc.robot.settings.Constants.IntakeConstants;
+import frc.robot.helpers.MotorLogger;
+import frc.robot.settings.Constants.IntakeConstants;
 public class IntakeSubsystem extends SubsystemBase {
   /** Creates a new Intake. */
   CANSparkMax intake1;
   CANSparkMax intake2;
   SparkAnalogSensor m_DistanceSensor;
   boolean isNoteHeld;
 
+  MotorLogger motorLogger1;
+  MotorLogger motorLogger2;
+  DoubleLogEntry logDistance;
+  BooleanLogEntry logNoteIn;
+
   double intakeRunSpeed;
   public IntakeSubsystem() {
     intake1 = new CANSparkMax(IntakeConstants.INTAKE_1_MOTOR, MotorType.kBrushless);
@@ -41,6 +51,12 @@ public IntakeSubsystem() {
     intake2.setSmartCurrentLimit(25, 40, 1000);
     intake1.burnFlash();
     intake2.burnFlash();
+
+    DataLog log = DataLogManager.getLog();
+    motorLogger1 = new MotorLogger(log, "/intake/motor1");
+    motorLogger2 = new MotorLogger(log, "/intake/motor2");
+    logDistance = new DoubleLogEntry(log, "/intake/noteDistance");
+    logNoteIn = new BooleanLogEntry(log, "/intake/noteIn");
   }
   /**
    * sets the intakes speed
@@ -79,8 +95,12 @@ public void setNoteHeld(boolean held) {
   }
   @Override
   public void periodic() {
-  SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage());
-  SmartDashboard.putBoolean("is note in", isNoteSeen());
-  SmartDashboard.putBoolean("is note held", isNoteHeld());
+    SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage());  
+    motorLogger1.log(intake1);
+    motorLogger2.log(intake2);
+    logDistance.append(m_DistanceSensor.getVoltage());
+    logNoteIn.append(isNoteSeen());
+    SmartDashboard.putBoolean("is note in", isNoteSeen());
+    SmartDashboard.putBoolean("is note held", isNoteHeld());
   }
 }

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

@@ -17,8 +17,11 @@
 import com.revrobotics.RelativeEncoder;
  import frc.robot.commands.AngleShooter;
 import frc.robot.commands.RotateRobot;
+import frc.robot.helpers.MotorLogger;
 import frc.robot.settings.Constants;
 import  frc.robot.settings.Constants.ShooterConstants;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase; 
@@ -49,6 +52,9 @@ public class ShooterSubsystem extends SubsystemBase {
 	AngleShooter angleShooter;
 	int accumulativeTurns;
   int runsValid;
+
+  MotorLogger motorLoggerR;
+  MotorLogger motorLoggerL;
 
   /** Creates a new Shooter. */
   public ShooterSubsystem(double runSpeed) {
@@ -81,6 +87,10 @@ public ShooterSubsystem(double runSpeed) {
 
     configuratorR.apply(PIDRightconfigs);
     configuratorL.apply(PIDLeftconfigs);
+
+    DataLog log = DataLogManager.getLog();
+    motorLoggerL = new MotorLogger(log, "/shooter/motorL");
+    motorLoggerR = new MotorLogger(log, "/shooter/motorR");
   }
   /**
    * configures the current limits on the shooter motors. Can be ran as many times as you want
@@ -185,6 +195,8 @@ public void periodic() {
     } else {
       runsValid = 0;
     }
+    motorLoggerL.log(shooterL);
+    motorLoggerR.log(shooterR);
   }
 }