From 1516c0e599a22dd749647436c65f4f995761babe Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Wed, 27 Mar 2024 16:59:05 -0400 Subject: [PATCH 1/6] Add ArmConstants and GyroIO classes, remove unused imports --- src/main/java/frc/robot/RobotContainer.java | 9 ++ .../robot/experimental/ArmConstants.java.txt | 10 ++ .../java/frc/robot/experimental/GyroIO.java | 39 +++++++ .../frc/robot/experimental/GyroIOPigeon2.java | 81 +++++++++++++ .../robot/experimental/IntakeIndexTimer.java | 1 - .../experimental/ShooterConstants.java.txt | 110 ++++++++++++++++++ src/main/java/frc/robot/subsystems/Gyro.java | 93 +++++++++++++++ 7 files changed, 342 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/experimental/ArmConstants.java.txt create mode 100644 src/main/java/frc/robot/experimental/GyroIO.java create mode 100644 src/main/java/frc/robot/experimental/GyroIOPigeon2.java create mode 100644 src/main/java/frc/robot/experimental/ShooterConstants.java.txt create mode 100644 src/main/java/frc/robot/subsystems/Gyro.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9e4ecc8..a7eef8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -34,6 +35,7 @@ import frc.robot.commands.ShootFromStage; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.Gyro; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Limelight; @@ -64,6 +66,8 @@ public class RobotContainer { WinchSubsystem2 winch = new WinchSubsystem2(); ArmSubsystem arm = new ArmSubsystem(); NoteSensorSubsystem notesensor = new NoteSensorSubsystem(); + Gyro pidgey = new Gyro(); + // #endregion Subsystems // #region commands @@ -224,6 +228,11 @@ public void DebugMethodSingle() { // arm.showArmTelemetry("Driver Diagnostics"); // Shuffleboard.getTab("Arm").add("Arm Output", arm); + + SmartDashboard.putNumber("Yaw", pidgey.getYaw()); + SmartDashboard.putNumber("Angle", pidgey.getAngle()); + SmartDashboard.putNumber("Rotation2d", pidgey.Rotation2d()); + // #endregion Testing } diff --git a/src/main/java/frc/robot/experimental/ArmConstants.java.txt b/src/main/java/frc/robot/experimental/ArmConstants.java.txt new file mode 100644 index 0000000..e7e668e --- /dev/null +++ b/src/main/java/frc/robot/experimental/ArmConstants.java.txt @@ -0,0 +1,10 @@ +/* +Use this approach in offseason. +See TunerConstants.java for an example of how to setup +*/ + +package frc.robot.subsystems; + +public class ArmConstants { + +} diff --git a/src/main/java/frc/robot/experimental/GyroIO.java b/src/main/java/frc/robot/experimental/GyroIO.java new file mode 100644 index 0000000..05ae339 --- /dev/null +++ b/src/main/java/frc/robot/experimental/GyroIO.java @@ -0,0 +1,39 @@ +/* This is from FRC 2910, 2023 season with some + * updates for this year's version of the CTRE Phoenix library. + */ +package frc.robot.experimental; + +import com.ctre.phoenix6.BaseStatusSignal; +// import org.littletonrobotics.junction.AutoLog; + +/** + * Connects the software to the hardware and directly receives data from the gyroscope. + */ +public interface GyroIO { + /** + * Reads information from sources (hardware or simulation) and updates the inputs object. + * + * @param inputs Contains the defaults for the input values listed above. + */ + // default void updateInputs(GyroIOInputs inputs) {} + + default BaseStatusSignal[] getSignals() { + return new BaseStatusSignal[0]; + } + + + /** + * Holds data that can be read from the corresponding gyroscope IO implementation. + */ + + /* + @AutoLog + class GyroIOInputs { + public boolean connected = false; + public double yaw = 0; + public double pitch = 0; + public double roll = 0; + public double angularVelocity = 0; + } + */ +} diff --git a/src/main/java/frc/robot/experimental/GyroIOPigeon2.java b/src/main/java/frc/robot/experimental/GyroIOPigeon2.java new file mode 100644 index 0000000..bf20002 --- /dev/null +++ b/src/main/java/frc/robot/experimental/GyroIOPigeon2.java @@ -0,0 +1,81 @@ +/* +* This is from FRC 2910, 2023 season with some +* updates for this year's version of the CTRE Phoenix library. +*/ + +package frc.robot.experimental; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.GyroTrimConfigs; +import com.ctre.phoenix6.configs.MountPoseConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + + +@SuppressWarnings("unused") + +/** + * Implementation of the gyroscope class. Uses the Pigeon2 gyroscope to receive data about the robots yaw, pitch, and roll values. + */ +public class GyroIOPigeon2 implements GyroIO { + + /** + * Pigeon2 gyroscope object. Receives direct data from the gyroscope about the yaw, pitch, and roll. + */ + private final Pigeon2 gyro; + + /** + * Pigeon2 gyroIO implementation. + * + * @param canId Is the unique identifier to the gyroscope used on the robot. + * @param canBus The name of the CAN bus the device is connected to. + */ + private StatusSignal yawSignal; + + private StatusSignal angularVelocitySignal; + + private BaseStatusSignal[] signals; + + public GyroIOPigeon2(int mountPose, double error, String canBus) { + + gyro = new Pigeon2(mountPose, canBus); + + Pigeon2Configuration config = new Pigeon2Configuration(); + MountPoseConfigs mountPoseConfigs = new MountPoseConfigs(); + mountPoseConfigs.MountPoseYaw = mountPose; + GyroTrimConfigs gyroTrimConfigs = new GyroTrimConfigs(); + gyroTrimConfigs.GyroScalarZ = error; + config.MountPose = mountPoseConfigs; + config.GyroTrim = gyroTrimConfigs; + gyro.getConfigurator().apply(config); + + yawSignal = gyro.getYaw(); + // angularVelocitySignal = gyro.getAngularVelocityZ(); + angularVelocitySignal = gyro.getAngularVelocityZDevice(); // Not sure if this is the correct method to use or ZWorld + + signals = new BaseStatusSignal[2]; + signals[0] = yawSignal; + signals[1] = angularVelocitySignal; + } + + + /* + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = true; + + inputs.yaw = Units.degreesToRadians( + BaseStatusSignalValue.getLatencyCompensatedValue(yawSignal, angularVelocitySignal)); + inputs.pitch = Units.degreesToRadians(gyro.getPitch().getValue()); + inputs.roll = Units.degreesToRadians(gyro.getRoll().getValue()); + inputs.angularVelocity = Units.degreesToRadians(angularVelocitySignal.getValue()); + } + */ + @Override + public BaseStatusSignal[] getSignals() { + return signals; + } +} diff --git a/src/main/java/frc/robot/experimental/IntakeIndexTimer.java b/src/main/java/frc/robot/experimental/IntakeIndexTimer.java index 315249a..d050f58 100644 --- a/src/main/java/frc/robot/experimental/IntakeIndexTimer.java +++ b/src/main/java/frc/robot/experimental/IntakeIndexTimer.java @@ -1,7 +1,6 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants; import frc.robot.commands.SetIndex; import frc.robot.commands.SetIntake; diff --git a/src/main/java/frc/robot/experimental/ShooterConstants.java.txt b/src/main/java/frc/robot/experimental/ShooterConstants.java.txt new file mode 100644 index 0000000..3f92a3c --- /dev/null +++ b/src/main/java/frc/robot/experimental/ShooterConstants.java.txt @@ -0,0 +1,110 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import frc.robot.Constants; + +public class ShooterConstants { + + // Declare variables for the motors to be controlled + private TalonFX m_primaryMotor = new TalonFX(Constants.CANIDs.RIGHT_FLYWHEEL_ID, "rio"); // Right + private TalonFX m_secondaryMotor = new TalonFX(Constants.CANIDs.LEFT_FLYWHEEL_ID, "rio"); // Left + + // TODO Tune this value + public final double FLYWHEEL_VELOCITY = 100; // rotations per second (rps) + public final double FLYWHEEL_VELOCITY_SHORT_RANGE = 40; // Not currently in use anywhere + public final double FLYWHEEL_VELOCITY_LONG_RANGE = 60; // Not currently in use anywhere, Flywheel speed for long range shots only + public final double FLYWHEEL_IDLE_VELOCITY = FLYWHEEL_VELOCITY * 0.30; // 30% of max speed + public final double FLYWHEEL_MARGIN_ERROR = FLYWHEEL_VELOCITY * 0.10; // 5% of max speed + public final double FLYWHEEL_MIN = FLYWHEEL_VELOCITY * .95; + public final double FLYWHEEL_MAX = FLYWHEEL_VELOCITY * 1.05; + + + // Class member variables + private VelocityVoltage m_velocityVoltage = new VelocityVoltage(0); + + private double currentFlywheelVel = m_primaryMotor.getVelocity().getValue(); + m_primaryMotor.setControl(m_velocityVoltage); + m_secondaryMotor.setControl(m_velocityVoltage); + + m_primaryMotor.getConfigurator().apply(new TalonFXConfiguration()); + m_secondaryMotor.getConfigurator().apply(new TalonFXConfiguration()); + + /* Various Configs */ + /* + | Ks | output to overcome static friction (output) + | Kv | output per unit of velocity (output/rps) + | Ka | output per unit of acceleration; unused, as there is no target acceleration (output/(rps/s)) + | Kp | output per unit of error (output/rps) + | Ki | output per unit of integrated error in velocity (output/rotation) + | kd | output per unit of error derivative in velocity (output/(rps/s)) + */ + + /* Example provided by CTRE */ + /* + slot0Configs.kS = 0.05; // Add 0.05 V output to overcome static friction + slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kP = 0.11; // An error of 1 rps results in 0.11 V output + slot0Configs.kI = 0; // no output for integrated error + slot0Configs.kD = 0; // no output for error derivative + */ + + var flywheelConfigs0 = new Slot0Configs(); + flywheelConfigs0 + // https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/VelocityClosedLoop/src/main/java/frc/robot/Robot.java + .withKP(0.10) + .withKI(0.00) + .withKS(0.00) // Should low for a light flywheel? Maybe the pulley strength would impact it though? + .withKV(0.00); + + + var flywheelVelocityConfig = new VoltageConfigs(); + flywheelVelocityConfig + .withPeakForwardVoltage(12) // FRC 2910 running 12 + .withPeakReverseVoltage(12); // Originally -8, with negative the "helper" text goes away + + var flywheelCurrentConfigs = new CurrentLimitsConfigs(); + flywheelCurrentConfigs + .withStatorCurrentLimit(60) + .withStatorCurrentLimitEnable(true); + + var flywheelMotorOutput = new MotorOutputConfigs(); + flywheelMotorOutput + .withNeutralMode(NeutralModeValue.Coast); + + /* Apply Configs */ + m_primaryMotor.getConfigurator().apply(flywheelConfigs0); + m_secondaryMotor.getConfigurator().apply(flywheelConfigs0); + + m_primaryMotor.getConfigurator().apply(flywheelVelocityConfig); + m_secondaryMotor.getConfigurator().apply(flywheelVelocityConfig); + + m_primaryMotor.getConfigurator().apply(flywheelCurrentConfigs); + m_secondaryMotor.getConfigurator().apply(flywheelCurrentConfigs); + + m_primaryMotor.getConfigurator().apply(flywheelMotorOutput); + m_secondaryMotor.getConfigurator().apply(flywheelMotorOutput); + + m_primaryMotor.setInverted(true); + m_secondaryMotor.setInverted(true); + + // Alternate way to get the current flywheel velocity? +} diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java new file mode 100644 index 0000000..cb6d11f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -0,0 +1,93 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MountPoseConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +public class Gyro { + + private final Pigeon2 pidgey; + + public Gyro() { + pidgey = new Pigeon2(14); // ID from Change the ID as needed + + // StatusSignal yawSignal; + + /* User can change the configs if they want, or leave it empty for factory-default */ + // Mimicing ArmSubsystem.java + pidgey.getConfigurator().apply(new Pigeon2Configuration()); + + var pidgeyConfigs = new Pigeon2Configuration(); + /* Speed up signals to an appropriate rate */ + // pidgey.getYaw().setUpdateFrequency(100); + // pidgey.getGravityVectorZ().setUpdateFrequency(100); + // pidgey.getAccelerationX().setUpdateFrequency(100); + var pidgeyMountConfigs = new MountPoseConfigs(); + // pidgeyMountConfigs.MountPose = MountPoseConfigs.MountedPose.TwoDegreesPitch; + + pidgey.getConfigurator().apply(pidgeyConfigs); + pidgey.getConfigurator().apply(pidgeyMountConfigs); + } + + /* Not sure what difference is between these three + * Values will be posted to the Dashboard + */ + public double getYaw() { + return pidgey.getYaw().getValue(); + } + + public double getAngle() { + return pidgey.getAngle(); + } + + public double Rotation2d() { + return pidgey.getRotation2d().getDegrees(); + } + + /* + * The Pigeon2 class has a default reset method `reset()` + * So can it be called directly without this method? + * Maybe, but not 100% sure + */ + + public void resetGyro() { + pidgey.reset(); + } + + + + } // end of class Gyro + + + /* + * Mount Calibration + * It’s recommended to perform a mount calibration when placement of the Pigeon 2.0 has been finalized. + * This can be done via the Calibration page in Tuner X. + */ + + /* Documentation + https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/Pigeon2.html + + Iterative Example + https://github.com/CrossTheRoadElec/Phoenix6-Examples/blob/main/java/Pigeon2/src/main/java/frc/robot/Robot.java + + * Method Summary + Modifier & Type | Method | Description + ----------------|--------------- |------------------- + void | close() | + double | getAngle() | Returns the heading of the robot in degrees. + double | getRate() | Returns the rate of rotation of the Pigeon 2. + Rotation2d | getRotation2d() | Returns the heading of the robot as a Rotation2d. + Rotation3d | getRotation3d() | Returns the orientation of the robot as a Rotation3d created from the quaternion signals. + void | initSendable​(SendableBuilder builder) | + void | reset() | Resets the Pigeon 2 to a heading of zero. + */ + + /* + * public double getAngle() + Returns the heading of the robot in degrees. + The angle increases as the Pigeon 2 turns clockwise when looked at from the top. This follows the NED axis convention. + The angle is continuous; that is, it will continue from 360 to 361 degrees. This allows for algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from 360 to 0 on the second time around. + Returns: The current heading of the robot in degrees + */ \ No newline at end of file From 078efe1d65b4a94a86d76623a2766a347b4773d3 Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Thu, 28 Mar 2024 14:15:37 -0400 Subject: [PATCH 2/6] Reorganizing --- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/commands/IntakeIndex.java | 2 +- src/main/java/frc/robot/commands/RevAndShootCommand.java | 2 +- src/main/java/frc/robot/commands/RevAndShootEndsCommand.java | 2 +- src/main/java/frc/robot/commands/RunShooter.java | 2 +- src/main/java/frc/robot/commands/ShootClose.java | 2 +- src/main/java/frc/robot/commands/ShootFromStage.java | 2 +- src/main/java/frc/robot/experimental/GyroIOPigeon2.java | 2 +- src/main/java/frc/robot/experimental/IntakeIndexTimer.java | 2 +- src/main/java/frc/robot/experimental/ShootWhenReady.java | 2 +- src/main/java/frc/robot/experimental/ShootWhenReady2.java | 2 +- .../java/frc/robot/experimental/ShootWhenReadyAuton.java | 2 +- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 2 +- .../frc/robot/{ => subsystems}/CommandSwerveDrivetrain.java | 5 +++-- src/main/java/frc/robot/subsystems/IndexSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/LedSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/ShooterSubsystem2.java | 2 +- .../java/frc/robot/subsystems/ShooterSubsystemVelocity.java | 2 +- src/main/java/frc/robot/subsystems/WinchSubsystem.java | 2 +- src/main/java/frc/robot/subsystems/WinchSubsystem2.java | 2 +- src/main/java/frc/robot/{ => util}/Constants.java | 2 +- .../java/frc/robot/{generated => util}/TunerConstants.java | 4 ++-- 24 files changed, 29 insertions(+), 26 deletions(-) rename src/main/java/frc/robot/{ => subsystems}/CommandSwerveDrivetrain.java (97%) rename src/main/java/frc/robot/{ => util}/Constants.java (99%) rename src/main/java/frc/robot/{generated => util}/TunerConstants.java (98%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a7eef8f..673591d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,14 +33,16 @@ import frc.robot.experimental.ShootWhenReadyAuton; import frc.robot.experimental.ShootWhenReady; import frc.robot.commands.ShootFromStage; -import frc.robot.generated.TunerConstants; import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Gyro; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Limelight; import frc.robot.subsystems.ShooterSubsystemVelocity; import frc.robot.subsystems.WinchSubsystem2; +import frc.robot.util.Constants; +import frc.robot.util.TunerConstants; import frc.robot.subsystems.ShooterSubsystem2; import frc.robot.subsystems.NoteSensorSubsystem; diff --git a/src/main/java/frc/robot/commands/IntakeIndex.java b/src/main/java/frc/robot/commands/IntakeIndex.java index f910a07..733fd13 100644 --- a/src/main/java/frc/robot/commands/IntakeIndex.java +++ b/src/main/java/frc/robot/commands/IntakeIndex.java @@ -2,9 +2,9 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.util.Constants; public class IntakeIndex extends SequentialCommandGroup { IndexSubsystem index; diff --git a/src/main/java/frc/robot/commands/RevAndShootCommand.java b/src/main/java/frc/robot/commands/RevAndShootCommand.java index 4d60c55..83a8c90 100644 --- a/src/main/java/frc/robot/commands/RevAndShootCommand.java +++ b/src/main/java/frc/robot/commands/RevAndShootCommand.java @@ -7,10 +7,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.Constants; import frc.robot.commands.IncrementIndex; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystemVelocity; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java b/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java index c75f27e..642fdc7 100644 --- a/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java +++ b/src/main/java/frc/robot/commands/RevAndShootEndsCommand.java @@ -7,10 +7,10 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.Constants; // import frc.robot.experimental.IncrementIndex; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystemVelocity; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/commands/RunShooter.java b/src/main/java/frc/robot/commands/RunShooter.java index e891773..b650868 100644 --- a/src/main/java/frc/robot/commands/RunShooter.java +++ b/src/main/java/frc/robot/commands/RunShooter.java @@ -1,8 +1,8 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.ShooterSubsystemVelocity; +import frc.robot.util.Constants; public class RunShooter extends Command { private ShooterSubsystemVelocity shooter; diff --git a/src/main/java/frc/robot/commands/ShootClose.java b/src/main/java/frc/robot/commands/ShootClose.java index b3cfe4d..f79ee65 100644 --- a/src/main/java/frc/robot/commands/ShootClose.java +++ b/src/main/java/frc/robot/commands/ShootClose.java @@ -2,11 +2,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystemVelocity; +import frc.robot.util.Constants; public class ShootClose extends SequentialCommandGroup { public ShootClose(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem intake, ShooterSubsystemVelocity shooter) { diff --git a/src/main/java/frc/robot/commands/ShootFromStage.java b/src/main/java/frc/robot/commands/ShootFromStage.java index 0ec0f48..f6d96f9 100644 --- a/src/main/java/frc/robot/commands/ShootFromStage.java +++ b/src/main/java/frc/robot/commands/ShootFromStage.java @@ -1,13 +1,13 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants; import frc.robot.experimental.ShootWhenReady; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.NoteSensorSubsystem; import frc.robot.subsystems.ShooterSubsystem2; +import frc.robot.util.Constants; public class ShootFromStage extends SequentialCommandGroup { public ShootFromStage(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem intake, ShooterSubsystem2 shooter2, NoteSensorSubsystem notesensor) { diff --git a/src/main/java/frc/robot/experimental/GyroIOPigeon2.java b/src/main/java/frc/robot/experimental/GyroIOPigeon2.java index bf20002..5f80bab 100644 --- a/src/main/java/frc/robot/experimental/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/experimental/GyroIOPigeon2.java @@ -12,7 +12,7 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.util.Units; -import frc.robot.Constants; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/experimental/IntakeIndexTimer.java b/src/main/java/frc/robot/experimental/IntakeIndexTimer.java index d050f58..8c65956 100644 --- a/src/main/java/frc/robot/experimental/IntakeIndexTimer.java +++ b/src/main/java/frc/robot/experimental/IntakeIndexTimer.java @@ -1,11 +1,11 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.Constants; import frc.robot.commands.SetIndex; import frc.robot.commands.SetIntake; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.util.Constants; public class IntakeIndexTimer extends ParallelCommandGroup { IndexSubsystem index; diff --git a/src/main/java/frc/robot/experimental/ShootWhenReady.java b/src/main/java/frc/robot/experimental/ShootWhenReady.java index 160b301..1467ff2 100644 --- a/src/main/java/frc/robot/experimental/ShootWhenReady.java +++ b/src/main/java/frc/robot/experimental/ShootWhenReady.java @@ -1,9 +1,9 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystem2; +import frc.robot.util.Constants; import frc.robot.subsystems.NoteSensorSubsystem; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/experimental/ShootWhenReady2.java b/src/main/java/frc/robot/experimental/ShootWhenReady2.java index 50ff9b8..07bc2c6 100644 --- a/src/main/java/frc/robot/experimental/ShootWhenReady2.java +++ b/src/main/java/frc/robot/experimental/ShootWhenReady2.java @@ -1,9 +1,9 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.ShooterSubsystem2; +import frc.robot.util.Constants; import frc.robot.subsystems.NoteSensorSubsystem; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/experimental/ShootWhenReadyAuton.java b/src/main/java/frc/robot/experimental/ShootWhenReadyAuton.java index f6ce749..c94e6c9 100644 --- a/src/main/java/frc/robot/experimental/ShootWhenReadyAuton.java +++ b/src/main/java/frc/robot/experimental/ShootWhenReadyAuton.java @@ -1,12 +1,12 @@ package frc.robot.experimental; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.IndexSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem2; import frc.robot.subsystems.ShooterSubsystemVelocity; +import frc.robot.util.Constants; import frc.robot.subsystems.NoteSensorSubsystem; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 7121c30..fbc39a0 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -13,7 +13,7 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; public class ArmSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java similarity index 97% rename from src/main/java/frc/robot/CommandSwerveDrivetrain.java rename to src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 7b5d256..6980659 100644 --- a/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,4 +1,4 @@ -package frc.robot; +package frc.robot.subsystems; import java.util.function.Supplier; @@ -21,7 +21,8 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.generated.TunerConstants; +import frc.robot.RobotContainer; +import frc.robot.util.TunerConstants; /** * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem diff --git a/src/main/java/frc/robot/subsystems/IndexSubsystem.java b/src/main/java/frc/robot/subsystems/IndexSubsystem.java index bbaa4f6..d138156 100644 --- a/src/main/java/frc/robot/subsystems/IndexSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexSubsystem.java @@ -4,7 +4,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; /*For indexing cargo. That is, taking cargo from a stored position and proceeding it to the shooter.*/ public class IndexSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index c7b9dbe..5227b8f 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -6,7 +6,7 @@ // import com.playingwithfusion.TimeOfFlight.RangingMode; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; /** * Using a ToF sensor and a single motor diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java index 033f3bb..4c9e808 100644 --- a/src/main/java/frc/robot/subsystems/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -24,7 +24,7 @@ CANdle requires the Phoenix (v5) vendordep import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java index 6a3c28a..8f6af9a 100644 --- a/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java @@ -15,7 +15,7 @@ */ import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; public class NoteSensorSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java index 266de42..9d3b92b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem2.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java b/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java index 9567d8f..5e78101 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystemVelocity.java @@ -11,7 +11,7 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; @SuppressWarnings("unused") diff --git a/src/main/java/frc/robot/subsystems/WinchSubsystem.java b/src/main/java/frc/robot/subsystems/WinchSubsystem.java index 8fac624..58f7c3c 100644 --- a/src/main/java/frc/robot/subsystems/WinchSubsystem.java +++ b/src/main/java/frc/robot/subsystems/WinchSubsystem.java @@ -6,7 +6,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; /** * Made of a motor, connected to a pulley. Contains manual drive for now. diff --git a/src/main/java/frc/robot/subsystems/WinchSubsystem2.java b/src/main/java/frc/robot/subsystems/WinchSubsystem2.java index bdd8bda..18e81ec 100644 --- a/src/main/java/frc/robot/subsystems/WinchSubsystem2.java +++ b/src/main/java/frc/robot/subsystems/WinchSubsystem2.java @@ -3,7 +3,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.util.Constants; public class WinchSubsystem2 extends SubsystemBase { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/util/Constants.java similarity index 99% rename from src/main/java/frc/robot/Constants.java rename to src/main/java/frc/robot/util/Constants.java index 4ae2df3..bd2c34a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot; +package frc.robot.util; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/util/TunerConstants.java similarity index 98% rename from src/main/java/frc/robot/generated/TunerConstants.java rename to src/main/java/frc/robot/util/TunerConstants.java index 2ba7180..084ca7a 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/util/TunerConstants.java @@ -1,4 +1,4 @@ -package frc.robot.generated; +package frc.robot.util; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; @@ -8,7 +8,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; import edu.wpi.first.math.util.Units; -import frc.robot.CommandSwerveDrivetrain; +import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html From d3812b00407c7badf7cead88f21d0e2cdd87bed6 Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Sat, 30 Mar 2024 09:21:02 -0400 Subject: [PATCH 3/6] Update Constants.java --- src/main/java/frc/robot/util/Constants.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index bd2c34a..5ef8457 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -119,8 +119,9 @@ public static final class ConstantsPlus { } public static class IndexConstants { - public static final double INDEX_POWER = 1.0; - public static final double INDEX_POWER_REV = -0.75; + // Tested in Tuner: Negative value is red, and brings in the note + public static final double INDEX_POWER = -1.0; + public static final double INDEX_POWER_REV = 0.75; } From a74a2985fde02df6a275bb2ecdcd357e1b254655 Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Sat, 30 Mar 2024 09:29:05 -0400 Subject: [PATCH 4/6] same subsystem trying to be used in a parallel group for auton --- src/main/deploy/pathplanner/autos/Test.auto | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto index c8fa89a..6df8b2a 100644 --- a/src/main/deploy/pathplanner/autos/Test.auto +++ b/src/main/deploy/pathplanner/autos/Test.auto @@ -15,12 +15,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - }, { "type": "path", "data": { @@ -34,12 +28,6 @@ "type": "parallel", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "SetFlywheelToIdle" - } - }, { "type": "path", "data": { From 1b9adf410d1da46911a6d67c002b3e8556d37de6 Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Sat, 30 Mar 2024 19:41:35 -0400 Subject: [PATCH 5/6] Add Gyro subsystem and update Robot and RobotContainer classes --- src/main/java/frc/robot/Robot.java | 7 +++++++ src/main/java/frc/robot/RobotContainer.java | 15 +++++++++------ src/main/java/frc/robot/subsystems/Gyro.java | 20 +++++++++++++++++--- 3 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 56fe110..5360d9e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Gyro; // TODO Added by Scoy @SuppressWarnings("unused") @@ -20,6 +21,8 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + private Gyro gyro = new Gyro(); // Added by Scoy + @Override public void robotInit() { m_robotContainer = new RobotContainer(); @@ -54,6 +57,10 @@ public void disabledExit() { @Override public void autonomousInit() { + + // gyro.setAutonStartingPose180(180); // TODO Added by Scoy, value already set + gyro.setAutonStartingPose(180); // TODO Added by Scoy + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 673591d..adaad36 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.IntakeCommandGroup; import frc.robot.commands.IntakeIndex; @@ -109,8 +110,7 @@ public class RobotContainer { private final ShootWhenReady shootWhenReady = new ShootWhenReady(shooter2, index, notesensor); // Autonomous version of the Shoot When Ready command that addeds notesensor checks for ending the command private final ShootWhenReadyAuton shootWhenReadyAuton = new ShootWhenReadyAuton(shooter2, index, notesensor); - private final ShootFromStage shootFromStage = new ShootFromStage(arm, index, intake, shooter2, notesensor); - + private final ShootFromStage shootFromStage = new ShootFromStage(arm, index, intake, shooter2, notesensor); // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest); @@ -125,13 +125,16 @@ public RobotContainer() { /* Pathplanner Named Commands */ NamedCommands.registerCommand("ShootClose", new ShootClose(arm, index, intake, shooter)); - NamedCommands.registerCommand("ShootClose2", shootWhenReadyAuton); NamedCommands.registerCommand("ShootFromStage", shootFromStage); NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake)); - - NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton); - NamedCommands.registerCommand("ShootWhenReady", shootWhenReadyAuton); // Autonomous + NamedCommands.registerCommand("AutonShootWhenReady", shootWhenReadyAuton); NamedCommands.registerCommand("SetFlywheelToIdle", setShooterIdle); + + // FIXME Use this approach if wanting a custom set starting pose command + // NamedCommands.registerCommand("SetStartingPose180", new InstantCommand()) -> Gyro.setAutonStartingPose(180)); + // NamedCommands.registerCommand("SetAutonStartingPose180", new InstantCommand(Gyro::setAutonStartingPose, Gyro, 180)); + + // Constants.ArmConstants.ARM_MID_POSE)); /* diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index cb6d11f..1c76706 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -5,12 +5,20 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; + public class Gyro { private final Pigeon2 pidgey; + private final double startingPose; + public Gyro() { pidgey = new Pigeon2(14); // ID from Change the ID as needed + + this.startingPose = 180; // StatusSignal yawSignal; @@ -51,10 +59,16 @@ public double Rotation2d() { * Maybe, but not 100% sure */ - public void resetGyro() { - pidgey.reset(); + + public void setAutonStartingPose(double startingPose) { + pidgey.setYaw(startingPose); } - + + + public void setAutonStartingPose180(double startingPose) { + pidgey.setYaw(180); + } + } // end of class Gyro From 990569ebbb745c30cb3976972e56ea390d633ed6 Mon Sep 17 00:00:00 2001 From: Jestin VanScoyoc <31118852+JediScoy@users.noreply.github.com> Date: Sat, 30 Mar 2024 21:08:00 -0400 Subject: [PATCH 6/6] Changing the Changing 180 in auton init in Robot.java had no change on auton, but it did impact teleop --- src/main/deploy/pathplanner/paths/2meters.path | 6 +++--- src/main/java/frc/robot/Robot.java | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/2meters.path b/src/main/deploy/pathplanner/paths/2meters.path index 7c0cccc..2ded22d 100644 --- a/src/main/deploy/pathplanner/paths/2meters.path +++ b/src/main/deploy/pathplanner/paths/2meters.path @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5360d9e..9621347 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,9 +58,8 @@ public void disabledExit() { @Override public void autonomousInit() { - // gyro.setAutonStartingPose180(180); // TODO Added by Scoy, value already set - gyro.setAutonStartingPose(180); // TODO Added by Scoy - + // gyro.setAutonStartingPose180(180); + // gyro.setAutonStartingPose(-180); // TODO Seems to impact Teleop and auton m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) {