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 01/10] 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 02/10] 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 03/10] 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()); } From aa2a63af353723bdf36063eb80c8323a79567e1d Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 16 Jun 2024 14:41:23 +0800 Subject: [PATCH 04/10] 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 806d9dd..a429315 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -19,6 +19,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 { @@ -44,6 +45,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}. * @@ -133,18 +136,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 c68d3e9b661661705d23441c085c4fcab78f74f1 Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 23 Jun 2024 14:00:23 +0800 Subject: [PATCH 05/10] 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 a429315..806d9dd 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -19,7 +19,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 { @@ -45,8 +44,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}. * @@ -136,22 +133,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 0c4cb07..34a8259 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,17 +13,21 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +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() { m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless); + lEDPwm = new PWM(0); } public Command intake() { + lEDPwm.setSpeed(0.59); // dark red // return Commands.run( () -> { log_output.append(4); @@ -38,6 +42,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 960f02d..c863994 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -17,6 +17,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 { @@ -25,7 +26,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}. * @@ -58,10 +60,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)); } @@ -71,6 +75,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()); } From 4f05a4a19241967e7b3685aab9ddb1b56ef74d6f Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Thu, 4 Jul 2024 15:31:17 +0800 Subject: [PATCH 06/10] began changes to arm subsystem --- src/main/java/frc/robot/Constants.java | 4 + src/main/java/frc/robot/subsystems/Arm.java | 294 ++++++++++-------- .../java/frc/robot/subsystems/Intake.java | 2 - 3 files changed, 173 insertions(+), 127 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9196a6f..7cd9d4c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,6 +25,10 @@ public final class Constants { public static final double armG = 0; public static final double armV = 0; public static final double armA = 0; + public static final int armMaxVelRadPerSec = 0; + public static final int armMaxAccelRadPerSec = 0; + public static final int armDistPerPulse = 0; + public static final int armOffsetRad = 0; public static final int armLeadPort = 21; public static final int armFollowerPort = 26; public static final int armEncoderPort = 3; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 806d9dd..91b0138 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -8,6 +8,9 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; @@ -15,139 +18,180 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import java.util.function.DoubleSupplier; -/** Our Arm Subsystem */ -public class Arm extends SubsystemBase { - public enum Setpoint { - kAmp, - kIntake, - kSpeaker, - kStowed - } - - private PIDController m_pid; - private CANSparkMax m_primaryMotor; - private CANSparkMax m_followerMotor; - 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}. - * - * @param primaryMotor The primary motor that controls the arm. - */ - public Arm() { - m_primaryMotor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); - m_followerMotor = new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless); - m_followerMotor.follow(m_primaryMotor); - - m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); - m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD); - m_pid.setTolerance(0.2); - m_feedforward = - new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA); - } - - /** Achieves and maintains speed for the primary motor. */ - private Command achievePosition(double position) { - return Commands.run( - () -> { - 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() { - return runOnce(() -> m_primaryMotor.set(0)); +public class Arm extends ProfiledPIDSubsystem { + + private final CANSparkMax m_motor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); + private final DutyCycleEncoder m_encoder = + new DutyCycleEncoder(Constants.armEncoderPort); + private final ArmFeedforward m_feedforward = + new ArmFeedforward( + Constants.armS, Constants.armG, + Constants.armV, Constants.armA); + +public Arm() { + super( + new ProfiledPIDController( + Constants.armP, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.armMaxVelRadPerSec, + Constants.armMaxAccelRadPerSec)), + 0); + + // Start arm at rest in neutral position + setGoal(Constants.armOffsetRad); } - /** - * Moves the arm to the specified position then stops. - * - * @param position The desired position. - * @return a {@link Command} to get to the desired position. - */ - private Command moveToPosition(double position) { - return achievePosition(position) - .until(() -> m_pid.atSetpoint() && m_encoder.getAbsolutePosition() * 2 * 3.14 == position); + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_motor.setVoltage(output + feedforward); } - /** - * Holds the arm at the current position setpoint. - * - * @return A {@link Command} to hold the position at the setpoint. - */ - public Command maintain() { - return achievePosition(m_pid.getSetpoint()); - } - - /** - * Checks if the Arm is at its setpoint and the loop is stable. - * - * @return A {@link Trigger} from the result. - */ - public Trigger atSetpoint() { - return new Trigger(() -> m_pid.atSetpoint()); - } - - /** - * Allows for manual control of the arm using a joystick. - * - * @param speed The speed from the joystick input. - * @return A {@link Command} to control the arm manually. - */ - public Command manualControl(DoubleSupplier speed) { - return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this); - } - - /* - * 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: - position = 5.34; - break; - - case kIntake: - position = 3.7; - break; - - case kSpeaker: - position = 3.7; - break; - - case kStowed: - position = 3.7; - break; - } - - return moveToPosition(position); + @Override + public double getMeasurement() { + return m_encoder.getDistance() + Constants.armOffsetRad; } } + +/** Our Arm Subsystem */ +// public class Arm extends SubsystemBase { +// public enum Setpoint { +// kAmp, +// kIntake, +// kSpeaker, +// kStowed +// } + +// private PIDController m_pid; +// private CANSparkMax m_primaryMotor; +// private CANSparkMax m_followerMotor; +// 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}. +// * +// * @param primaryMotor The primary motor that controls the arm. +// */ +// public Arm() { +// m_primaryMotor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); +// m_followerMotor = new CANSparkMax(Constants.armFollowerPort, MotorType.kBrushless); +// m_followerMotor.follow(m_primaryMotor); + +// m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); +// m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD); +// m_pid.setTolerance(0.2); +// m_feedforward = +// new ArmFeedforward(Constants.armS, Constants.armG, Constants.armV, Constants.armA); +// } + +// /** Achieves and maintains speed for the primary motor. */ +// private Command achievePosition(double position) { +// return Commands.run( +// () -> { +// 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() { +// return runOnce(() -> m_primaryMotor.set(0)); +// } + +// /** +// * Moves the arm to the specified position then stops. +// * +// * @param position The desired position. +// * @return a {@link Command} to get to the desired position. +// */ +// private Command moveToPosition(double position) { +// return achievePosition(position) +// .until(() -> m_pid.atSetpoint() && m_encoder.getAbsolutePosition() * 2 * 3.14 == position); +// } + +// /** +// * Holds the arm at the current position setpoint. +// * +// * @return A {@link Command} to hold the position at the setpoint. +// */ +// public Command maintain() { +// return achievePosition(m_pid.getSetpoint()); +// } + +// /** +// * Checks if the Arm is at its setpoint and the loop is stable. +// * +// * @return A {@link Trigger} from the result. +// */ +// public Trigger atSetpoint() { +// return new Trigger(() -> m_pid.atSetpoint()); +// } + +// /** +// * Allows for manual control of the arm using a joystick. +// * +// * @param speed The speed from the joystick input. +// * @return A {@link Command} to control the arm manually. +// */ +// public Command manualControl(DoubleSupplier speed) { +// return Commands.run(() -> m_primaryMotor.set(speed.getAsDouble()), this); +// } + +// /* +// * 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: +// position = 5.34; +// break; + +// case kIntake: +// position = 3.7; +// break; + +// case kSpeaker: +// position = 3.7; +// break; + +// case kStowed: +// position = 3.7; +// break; +// } + +// return moveToPosition(position); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 7389a37..2a53ecf 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -24,8 +24,6 @@ public class Intake extends SubsystemBase { private DoubleLogEntry log_output = new DoubleLogEntry(log, "/intake/output"); PWM lEDPwm = new PWM(0); - PWM lEDPwm = new PWM(0); - public Intake() { m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless); lEDPwm = new PWM(0); From 2f320e924f5162dd7cf9a3b489fe58885cb13e15 Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 7 Jul 2024 12:31:51 +0800 Subject: [PATCH 07/10] added motion profiling to arm PID, buildsbut likeley contains redundant or wrong code --- src/main/java/frc/robot/subsystems/Arm.java | 52 +++++++++++++++------ 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index be3b9eb..cfdbbe9 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -24,7 +24,7 @@ import frc.robot.Constants; import java.util.function.DoubleSupplier; -public class Arm extends ProfiledPIDSubsystem { +public class Arm extends SubsystemBase { public enum Setpoint { kAmp, @@ -32,47 +32,71 @@ public enum Setpoint { kSpeaker, kStowed } - + + private PIDController m_pid; private final CANSparkMax m_motor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); - private final DutyCycleEncoder m_encoder = - new DutyCycleEncoder(Constants.armEncoderPort); + 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"); + private final DutyCycleEncoder m_encoder = + new DutyCycleEncoder(Constants.armEncoderPort); + + private Command achievePosition(double position) { + return Commands.run( + () -> { + 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_motor.setVoltage(-1 * ff_output + pid_output); + }); +} + private final ArmFeedforward m_feedforward = new ArmFeedforward( Constants.armS, Constants.armG, Constants.armV, Constants.armA); + public Arm() { - super( + new ProfiledPIDController( Constants.armP, Constants.armI, Constants.armD, new TrapezoidProfile.Constraints( Constants.armMaxVelRadPerSec, - Constants.armMaxAccelRadPerSec)), - 0); + Constants.armMaxAccelRadPerSec)); // Start arm at rest in neutral position - setGoal(Constants.armOffsetRad); + achievePosition(Constants.armOffsetRad); } - @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { // Calculate the feedforward from the sepoint double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); // Add the feedforward to the PID output to get the motor output m_motor.setVoltage(output + feedforward); } - - @Override + public double getMeasurement() { return m_encoder.getDistance() + Constants.armOffsetRad; } - public void goToSetpoint(Setpoint setpoint) { + public Command goToSetpoint(Setpoint setpoint) { double position = 0; - // log_setpoint.append(setpoint.name()); + log_setpoint.append(setpoint.name()); switch (setpoint) { case kAmp: @@ -92,7 +116,7 @@ public void goToSetpoint(Setpoint setpoint) { break; } - setGoal(position); + return achievePosition(position); } } From 66e24cb4b1b65d0fba717625aa62bac9f6b3cd87 Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sat, 13 Jul 2024 14:36:30 +0800 Subject: [PATCH 08/10] hopefully complete motion profiling, not checked by mentors, motion profiling/pid/feedforeward values not added --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/subsystems/Arm.java | 123 ++++++++++++++------ 2 files changed, 93 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7cd9d4c..cc6468d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,10 +21,10 @@ public final class Constants { public static final double armP = 21.0; public static final double armI = 0; public static final double armD = 0.015; - public static final double armS = 0; - public static final double armG = 0; - public static final double armV = 0; - public static final double armA = 0; + public static final double armS = 0; //stiction gain + public static final double armG = 0; //gravity gain + public static final double armV = 0; //velocity gain + public static final double armA = 0; //acceleration gain public static final int armMaxVelRadPerSec = 0; public static final int armMaxAccelRadPerSec = 0; public static final int armDistPerPulse = 0; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index cfdbbe9..0daf5f3 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -2,7 +2,7 @@ // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project -package frc.robot.subsystems; +package frc.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; @@ -26,6 +26,8 @@ public class Arm extends SubsystemBase { + + public enum Setpoint { kAmp, kIntake, @@ -33,9 +35,13 @@ public enum Setpoint { kStowed } + private TrapezoidProfile m_profile; + private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); private PIDController m_pid; private final CANSparkMax m_motor = new CANSparkMax(Constants.armLeadPort, MotorType.kBrushless); private DataLog m_log = DataLogManager.getLog(); + private DoubleLogEntry log_trapezoid_goal = new DoubleLogEntry(m_log, "/arm/trapezoid/goal"); 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 = @@ -47,54 +53,103 @@ public enum Setpoint { private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); - private Command achievePosition(double position) { - return Commands.run( - () -> { - 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_motor.setVoltage(-1 * ff_output + pid_output); - }); -} - - private final ArmFeedforward +private ArmFeedforward m_feedforward = new ArmFeedforward( Constants.armS, Constants.armG, Constants.armV, Constants.armA); + + private void useOutput(double output, Double position, Double velocity) { + + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(position, velocity); + + log_trapezoid_goal.append(m_goal.position); + log_trapezoid_goal.append(m_goal.velocity); + log_pid_output.append(output); + log_pid_setpoint.append(m_pid.getSetpoint()); + log_ff_output.append(feedforward); + log_ff_position_setpoint.append(position); + log_ff_velocity_setpoint.append(velocity); + // Add the feedforward to the PID output to get the motor output + m_motor.setVoltage(output + feedforward); + } + + /** + * Sets the goal for the ProfiledPIDController. + * + * @param goal The desired goal state. + */ + private void setGoal(TrapezoidProfile.State goal) { + m_goal = goal; + } + + /** + * Sets the goal for the ProfiledPIDController. + * + * @param goal The desired goal position. + */ + private void setGoal(double goal) { + m_goal = new TrapezoidProfile.State(goal, 0); + } + /** + * Gets the goal for the ProfiledPIDController. + * + * @return The goal. + */ + private TrapezoidProfile.State getGoal() { + return m_goal; + } + + private double getPeriod() { + return m_pid.getPeriod(); + } + + /** + * Returns the next output of the PID controller. + * + * @param measurement The current measurement of the process variable. + * @param goal The new goal of the controller. + * @return The controller's next output. + */ + public double calculate(double measurement) { + + m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal); + return m_pid.calculate(measurement, m_setpoint.position); + + } + + public double calculate(double measurement, TrapezoidProfile.State goal) { + setGoal(goal); + return calculate(measurement); + } + public double calculate(double measurement, double goal) { + setGoal(goal); + return calculate(measurement); + } + + public Arm() { - new ProfiledPIDController( - Constants.armP, - Constants.armI, - Constants.armD, - new TrapezoidProfile.Constraints( - Constants.armMaxVelRadPerSec, - Constants.armMaxAccelRadPerSec)); + m_pid = new PIDController( + Constants.armP, + Constants.armI, + Constants.armD); + new TrapezoidProfile(new TrapezoidProfile.Constraints( + Constants.armMaxVelRadPerSec, + Constants.armMaxAccelRadPerSec)); // Start arm at rest in neutral position - achievePosition(Constants.armOffsetRad); + setGoal(Constants.armOffsetRad); } - - public void useOutput(double output, TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - m_motor.setVoltage(output + feedforward); - } - public double getMeasurement() { return m_encoder.getDistance() + Constants.armOffsetRad; } - public Command goToSetpoint(Setpoint setpoint) { + public void goToSetpoint(Setpoint setpoint) { double position = 0; log_setpoint.append(setpoint.name()); @@ -116,7 +171,7 @@ public Command goToSetpoint(Setpoint setpoint) { break; } - return achievePosition(position); + useOutput(calculate(m_encoder.getAbsolutePosition(), position), m_setpoint.position, m_setpoint.velocity); } } From bf76d9d2df351732b0fc21da846e14bba7bb896d Mon Sep 17 00:00:00 2001 From: SturrockPeter Date: Sun, 14 Jul 2024 13:49:22 +0800 Subject: [PATCH 09/10] organised arm code a little bit --- src/main/java/frc/robot/subsystems/Arm.java | 33 ++++++++++----------- 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 0daf5f3..9bddf0e 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -25,15 +25,6 @@ import java.util.function.DoubleSupplier; public class Arm extends SubsystemBase { - - - - public enum Setpoint { - kAmp, - kIntake, - kSpeaker, - kStowed - } private TrapezoidProfile m_profile; private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); @@ -52,13 +43,19 @@ public enum Setpoint { private StringLogEntry log_setpoint = new StringLogEntry(m_log, "/arm/setpoint"); private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); - -private ArmFeedforward - m_feedforward = - new ArmFeedforward( - Constants.armS, Constants.armG, - Constants.armV, Constants.armA); + private ArmFeedforward + m_feedforward = + new ArmFeedforward( + Constants.armS, Constants.armG, + Constants.armV, Constants.armA); + public enum Setpoint { + kAmp, + kIntake, + kSpeaker, + kStowed + } + private void useOutput(double output, Double position, Double velocity) { // Calculate the feedforward from the sepoint @@ -114,18 +111,18 @@ private double getPeriod() { * @param goal The new goal of the controller. * @return The controller's next output. */ - public double calculate(double measurement) { + private double calculate(double measurement) { m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal); return m_pid.calculate(measurement, m_setpoint.position); } - public double calculate(double measurement, TrapezoidProfile.State goal) { + private double calculate(double measurement, TrapezoidProfile.State goal) { setGoal(goal); return calculate(measurement); } - public double calculate(double measurement, double goal) { + private double calculate(double measurement, double goal) { setGoal(goal); return calculate(measurement); } From 1414718ca39cadd4d8310b0f6d9f0bb53647d9c3 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 28 Jul 2024 06:27:49 +0000 Subject: [PATCH 10/10] Formatting fixes --- src/main/java/frc/robot/Constants.java | 12 ++-- src/main/java/frc/robot/subsystems/Arm.java | 55 +++++++------------ .../java/frc/robot/subsystems/Intake.java | 3 - .../java/frc/robot/subsystems/Shooter.java | 7 --- 4 files changed, 27 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8c41f59..02b2d2a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,12 +25,12 @@ public final class Constants { public static final double armP = 21.0; public static final double armI = 0; public static final double armD = 0.015; - public static final double armS = 0; //stiction gain - public static final double armG = 0; //gravity gain - public static final double armV = 0; //velocity gain - public static final double armA = 0; //acceleration gain - public static final int armMaxVelRadPerSec = 0; - public static final int armMaxAccelRadPerSec = 0; + public static final double armS = 0; // stiction gain + public static final double armG = 0; // gravity gain + public static final double armV = 0; // velocity gain + public static final double armA = 0; // acceleration gain + public static final int armMaxVelRadPerSec = 0; + public static final int armMaxAccelRadPerSec = 0; public static final int armDistPerPulse = 0; public static final int armOffsetRad = 0; public static final int armLeadPort = 21; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index edad4aa..2fba4fe 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -2,30 +2,23 @@ // Open Source Software, you can modify it according to the terms // of the MIT License at the root of this project -package frc.robot.subsystems; +package frc.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Encoder; 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; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import java.util.function.DoubleSupplier; public class Arm extends SubsystemBase { - + private TrapezoidProfile m_profile; private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); @@ -41,14 +34,12 @@ public class Arm extends SubsystemBase { 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"); - private final DutyCycleEncoder m_encoder = - new DutyCycleEncoder(Constants.armEncoderPort); - private ArmFeedforward - m_feedforward = - new ArmFeedforward( - Constants.armS, Constants.armG, - Constants.armV, Constants.armA); - + private final DutyCycleEncoder m_encoder = new DutyCycleEncoder(Constants.armEncoderPort); + private ArmFeedforward m_feedforward = + new ArmFeedforward( + Constants.armS, Constants.armG, + Constants.armV, Constants.armA); + public enum Setpoint { kAmp, kIntake, @@ -57,7 +48,6 @@ public enum Setpoint { } private void useOutput(double output, Double position, Double velocity) { - // Calculate the feedforward from the sepoint double feedforward = m_feedforward.calculate(position, velocity); @@ -72,8 +62,8 @@ private void useOutput(double output, Double position, Double velocity) { // Add the feedforward to the PID output to get the motor output m_motor.setVoltage(output + feedforward); } - - /** + + /** * Sets the goal for the ProfiledPIDController. * * @param goal The desired goal state. @@ -112,32 +102,28 @@ private double getPeriod() { * @return The controller's next output. */ private double calculate(double measurement) { - + m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal); return m_pid.calculate(measurement, m_setpoint.position); - } private double calculate(double measurement, TrapezoidProfile.State goal) { setGoal(goal); return calculate(measurement); } + private double calculate(double measurement, double goal) { setGoal(goal); return calculate(measurement); } - -public Arm() { - - m_pid = new PIDController( - Constants.armP, - Constants.armI, - Constants.armD); - new TrapezoidProfile(new TrapezoidProfile.Constraints( - Constants.armMaxVelRadPerSec, - Constants.armMaxAccelRadPerSec)); - + public Arm() { + + m_pid = new PIDController(Constants.armP, Constants.armI, Constants.armD); + new TrapezoidProfile( + new TrapezoidProfile.Constraints( + Constants.armMaxVelRadPerSec, Constants.armMaxAccelRadPerSec)); + // Start arm at rest in neutral position setGoal(Constants.armOffsetRad); } @@ -168,4 +154,5 @@ public void goToSetpoint(Setpoint setpoint) { break; } } -}; +} +; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 85ad546..07c4353 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import edu.wpi.first.wpilibj.PWM; public class Intake extends SubsystemBase { private final CANSparkMax m_motor = new CANSparkMax(Constants.intakePort, MotorType.kBrushless); @@ -23,7 +22,6 @@ public class Intake extends SubsystemBase { public Intake() {} public Command intake() { - return Commands.run( () -> { log_output.append(4); @@ -38,7 +36,6 @@ public Command stop() { } public Command outake() { - 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 9166743..3d21858 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -12,15 +12,11 @@ import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; import edu.wpi.first.wpilibj.DataLogManager; -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; 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 { @@ -59,12 +55,10 @@ private Command achieveSpeeds(double speed) { * @return a {@link Command} to get to the desired speed. */ public Command spinup(double speed) { - return achieveSpeeds(speed).until(m_pid::atSetpoint); } public Command stop() { - return runOnce(() -> m_motor.set(0)); } @@ -74,7 +68,6 @@ public Command stop() { * @return A {@link Command} to hold the speed at the setpoint. */ public Command maintain() { - return achieveSpeeds(m_pid.getSetpoint()); } }