diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb4d9aa8..e7d91785 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/helpers/MotorLogger.java b/src/main/java/frc/robot/helpers/MotorLogger.java new file mode 100644 index 00000000..02b20870 --- /dev/null +++ b/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()); + } +} diff --git a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerSubsystem.java index d710ba54..451d32f0 100644 --- a/src/main/java/frc/robot/subsystems/IndexerSubsystem.java +++ b/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,13 +68,13 @@ 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) @@ -63,14 +82,14 @@ public void off() { * 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. @@ -78,9 +97,21 @@ public void set(double speed) { */ 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 @@ -88,11 +119,15 @@ public void forwardInches(double inches) { */ 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); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index edb63c50..5b553a46 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -10,10 +10,15 @@ 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; @@ -21,6 +26,11 @@ public class IntakeSubsystem extends SubsystemBase { 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()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 499f40c3..1c5d3bf6 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/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); } }