From 67f150b7eb86d497c200fdafb2b68e2cbb7517f7 Mon Sep 17 00:00:00 2001 From: SturrockPeter <147894410+SturrockPeter@users.noreply.github.com> Date: Sun, 16 Jun 2024 12:28:20 +0800 Subject: [PATCH 1/3] Implement WPILog file logging (#41) (#44) Resolves https://github.com/CurtinFRC/2024-Offseason/issues/15 Closes https://github.com/CurtinFRC/2024-Offseason/pull/39 Co-authored-by: Jade Co-authored-by: koz --- src/main/java/frc/robot/Robot.java | 5 ++++ src/main/java/frc/robot/subsystems/Arm.java | 30 +++++++++++++++---- .../java/frc/robot/subsystems/Climber.java | 14 +++++++-- .../java/frc/robot/subsystems/Intake.java | 17 +++++++++-- .../java/frc/robot/subsystems/Shooter.java | 16 +++++++--- 5 files changed, 68 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e286192..3fd4ea5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -95,6 +97,9 @@ private void configureBindings() { @SuppressWarnings("removal") public Robot() { + DataLogManager.start(); + DriverStation.startDataLog(DataLogManager.getLog()); + m_driver = new CommandXboxController(Constants.driverport); m_codriver = new CommandXboxController(Constants.codriverport); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 33a0cb4..697351f 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -7,6 +7,10 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -28,6 +32,15 @@ public enum Setpoint { private CANSparkMax m_primaryMotor; private DutyCycleEncoder m_encoder; private ArmFeedforward m_feedforward; + private DataLog m_log = DataLogManager.getLog(); + private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/arm/pid/output"); + private DoubleLogEntry log_pid_setpoint = new DoubleLogEntry(m_log, "/arm/pid/setpoint"); + private DoubleLogEntry log_ff_position_setpoint = + new DoubleLogEntry(m_log, "/arm/ff/position_setpoint"); + private DoubleLogEntry log_ff_velocity_setpoint = + new DoubleLogEntry(m_log, "/arm/ff/velocity_setpoint"); + private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); + private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); /** * Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. @@ -47,11 +60,16 @@ public Arm(CANSparkMax primaryMotor) { /** Achieves and maintains speed for the primary motor. */ private Command achievePosition(double position) { return Commands.run( - () -> - m_primaryMotor.setVoltage( - -1 - * (m_feedforward.calculate(position, (5676 / 250)) - + m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * 3.14, position)))); + () -> { + var pid_output = m_pid.calculate(m_encoder.getAbsolutePosition() * 2 * 3.14, position); + log_pid_output.append(pid_output); + log_pid_setpoint.append(m_pid.getSetpoint()); + var ff_output = m_feedforward.calculate(position, (5676 / 250)); + log_ff_output.append(ff_output); + log_ff_position_setpoint.append(position); + log_ff_velocity_setpoint.append((5676 / 250)); + m_primaryMotor.setVoltage(-1 * ff_output + pid_output); + }); } public Command stop() { @@ -101,10 +119,12 @@ public Command manualControl(DoubleSupplier speed) { * Makes the arm go to a setpoint from the {@link Setpoint} enum * * @param setpoint The setpoint to go to, a {@link Setpoint} + * * @return A {@link Command} to go to the setpoint */ public Command goToSetpoint(Setpoint setpoint) { double position = 0; + log_setpoint.append(setpoint.name()); switch (setpoint) { case kAmp: diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index b307af8..6f5cff6 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -8,6 +8,9 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,6 +21,8 @@ public class Climber extends SubsystemBase { private PIDController m_pid; private CANSparkMax m_motor; private RelativeEncoder m_encoder; + private DataLog m_log = DataLogManager.getLog(); + private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/climber/pid/output"); /** * Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}. @@ -38,8 +43,11 @@ public Climber(CANSparkMax motor) { */ public Command climb() { return Commands.run( - () -> - m_motor.setVoltage( - m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition() * -1), -3.14159))); + () -> { + var output = + m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition() * -1), -3.14159); + log_pid_output.append(output); + m_motor.setVoltage(output); + }); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d8ba054..6af2d0d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -5,19 +5,28 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { private CANSparkMax m_motor; + private DataLog log = DataLogManager.getLog(); + private DoubleLogEntry log_output = new DoubleLogEntry(log, "/intake/output"); public Intake(CANSparkMax motor) { m_motor = motor; } public Command intake() { - return Commands.run(() -> m_motor.setVoltage(4)) + return Commands.run( + () -> { + log_output.append(4); + m_motor.setVoltage(4); + }) .withTimeout(2) .andThen(runOnce(() -> m_motor.setVoltage(0))); } @@ -27,7 +36,11 @@ public Command stop() { } public Command outake() { - return Commands.run(() -> m_motor.setVoltage(-4)) + return Commands.run( + () -> { + log_output.append(-4); + m_motor.setVoltage(-4); + }) .withTimeout(4) .andThen(runOnce(() -> m_motor.setVoltage(0))); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index dddae74..78f570c 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -8,6 +8,9 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,6 +22,8 @@ public class Shooter extends SubsystemBase { private PIDController m_pid; private CANSparkMax m_motor; private RelativeEncoder m_encoder; + private DataLog m_log = DataLogManager.getLog(); + private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output"); /** * Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. @@ -36,10 +41,13 @@ private Command achieveSpeeds(double speed) { m_pid.reset(); m_pid.setSetpoint(speed); return Commands.run( - () -> - m_motor.setVoltage( - m_pid.calculate( - -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity())))); + () -> { + var output = + m_pid.calculate( + -1 * Units.rotationsPerMinuteToRadiansPerSecond(m_encoder.getVelocity())); + log_pid_output.append(output); + m_motor.setVoltage(output); + }); } /** From d776edb39c60cbafd9ea0a5ab4db86bcb30b7f48 Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 16 Jun 2024 14:41:23 +0800 Subject: [PATCH 2/3] added LED patterns to arm states --- src/main/java/frc/robot/subsystems/Arm.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 697351f..44d3994 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.PWM; /** Our Arm Subsystem */ public class Arm extends SubsystemBase { @@ -42,6 +43,8 @@ public enum Setpoint { private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); + PWM lEDPwm = new PWM(0); + /** * Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. * @@ -129,18 +132,22 @@ public Command goToSetpoint(Setpoint setpoint) { switch (setpoint) { case kAmp: position = 5.34; + lEDPwm.setSpeed(0.67); break; - case kIntake: + case kSpeaker: position = 3.7; + lEDPwm.setSpeed(0.73); break; - case kSpeaker: + case kIntake: position = 3.7; + lEDPwm.setSpeed(0.57); break; case kStowed: position = 3.7; + lEDPwm.setSpeed(0.99); break; } From f6c5cc04de8ac249034afc886e2c386f17c45744 Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 23 Jun 2024 14:00:23 +0800 Subject: [PATCH 3/3] changed LED states to be in intake and shooter subsystems --- src/main/java/frc/robot/subsystems/Arm.java | 11 ++--------- src/main/java/frc/robot/subsystems/Intake.java | 5 +++++ src/main/java/frc/robot/subsystems/Shooter.java | 7 ++++++- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 44d3994..697351f 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj.PWM; /** Our Arm Subsystem */ public class Arm extends SubsystemBase { @@ -43,8 +42,6 @@ public enum Setpoint { private DoubleLogEntry log_ff_output = new DoubleLogEntry(m_log, "/arm/ff/output"); private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); - PWM lEDPwm = new PWM(0); - /** * Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. * @@ -132,22 +129,18 @@ public Command goToSetpoint(Setpoint setpoint) { switch (setpoint) { case kAmp: position = 5.34; - lEDPwm.setSpeed(0.67); break; - case kSpeaker: + case kIntake: position = 3.7; - lEDPwm.setSpeed(0.73); break; - case kIntake: + case kSpeaker: position = 3.7; - lEDPwm.setSpeed(0.57); break; case kStowed: position = 3.7; - lEDPwm.setSpeed(0.99); break; } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6af2d0d..5bd382d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -11,17 +11,21 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.PWM; public class Intake extends SubsystemBase { private CANSparkMax m_motor; private DataLog log = DataLogManager.getLog(); private DoubleLogEntry log_output = new DoubleLogEntry(log, "/intake/output"); + PWM lEDPwm = new PWM(0); + public Intake(CANSparkMax motor) { m_motor = motor; } public Command intake() { + lEDPwm.setSpeed(0.59); // dark red // return Commands.run( () -> { log_output.append(4); @@ -36,6 +40,7 @@ public Command stop() { } public Command outake() { + lEDPwm.setSpeed(0.63); // red orange, 0.65 for normal orange // return Commands.run( () -> { log_output.append(-4); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 78f570c..bcef316 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; +import edu.wpi.first.wpilibj.PWM; /** Our Crescendo shooter Subsystem */ public class Shooter extends SubsystemBase { @@ -24,7 +25,8 @@ public class Shooter extends SubsystemBase { private RelativeEncoder m_encoder; private DataLog m_log = DataLogManager.getLog(); private DoubleLogEntry log_pid_output = new DoubleLogEntry(m_log, "/shooter/pid/output"); - + + PWM lEDPwm = new PWM(0); /** * Creates a new {@link Shooter} {@link edu.wpi.first.wpilibj2.command.Subsystem}. * @@ -57,10 +59,12 @@ private Command achieveSpeeds(double speed) { * @return a {@link Command} to get to the desired speed. */ public Command spinup(double speed) { + lEDPwm.setSpeed(0.69); // yellow // return achieveSpeeds(speed).until(m_pid::atSetpoint); } public Command stop() { + lEDPwm.setSpeed(0.73); // lime // return runOnce(() -> m_motor.set(0)); } @@ -70,6 +74,7 @@ public Command stop() { * @return A {@link Command} to hold the speed at the setpoint. */ public Command maintain() { + lEDPwm.setSpeed(0.57); // hot pink // return achieveSpeeds(m_pid.getSetpoint()); }