From 70e9881522766670a86527c8349e755bb409c141 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Tue, 23 Jan 2024 17:23:23 -0800 Subject: [PATCH 1/5] added shooter subsystem --- .../lib2813/subsystems/MotorSubsystem.java | 2 +- .../java/com/team2813/subsystems/Shooter.java | 25 +++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) create mode 100644 Robot2024/src/main/java/com/team2813/subsystems/Shooter.java diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index df9e7af..413e880 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -15,7 +15,7 @@ * * @param the {@link MotorSubsystem.Position} type to use positions from. must be an enum */ -public abstract class MotorSubsystem & MotorSubsystem.Position> extends PIDSubsystem { +public abstract class MotorSubsystem extends PIDSubsystem { protected final Motor motor; protected final Encoder encoder; diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java new file mode 100644 index 0000000..7a03198 --- /dev/null +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -0,0 +1,25 @@ +package com.team2813.subsystems; + +import com.team2813.lib2813.control.motors.TalonFXWrapper; +import com.team2813.lib2813.subsystems.MotorSubsystem; +import static com.team2813.Constants.*; + +import com.ctre.phoenix.motorcontrol.TalonFXInvertType; + +public class Shooter extends MotorSubsystem { + public Shooter() { + // TODO: fix invert type + super(new MotorSubsystemConfiguration(new TalonFXWrapper(0, TalonFXInvertType.Clockwise))); + } + public static enum Angle implements MotorSubsystem.Position { + TEST(0.0); + private double pos; + Angle(double pos) { + this.pos = pos; + } + @Override + public double getPos() { + return pos; + } + } +} From 6216815f51211b3f08d9bfac0e6d0b676c71e973 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 24 Jan 2024 19:49:45 -0800 Subject: [PATCH 2/5] add motors to shooter subsystem --- .../lib2813/control/motors/TalonFXWrapper.java | 2 -- .../team2813/lib2813/subsystems/MotorSubsystem.java | 4 ++-- Robot2024/src/main/java/com/team2813/Constants.java | 3 ++- .../main/java/com/team2813/subsystems/Shooter.java | 11 ++++++++++- 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java index d0bc6f6..c2afd37 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java @@ -127,8 +127,6 @@ public void set(ControlMode controlMode, double demand, double feedForward) { case MOTION_MAGIC: demand = Units2813.motorRevsToTicks(demand, 2048); break; - case DUTY_CYCLE: - break; default: break; } diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 413e880..953e6b8 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -40,8 +40,8 @@ public static class MotorSubsystemConfiguration { * The default starting position if one is not defined */ public static final double DEFAULT_STARTING_POSITION = 0.0; - private Motor motor; - private Encoder encoder; + protected Motor motor; + protected Encoder encoder; private PIDController controller; private double acceptableError; private double startingPosition; diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index 1785760..3bde732 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -51,7 +51,8 @@ public static class DriverConstants { //Mechanism CAN IDs public static final int INTAKE = 14; public static final int KICKER = 15; - public static final int SHOOTER = 16; + public static final int SHOOTER_1 = 16; + public static final int SHOOTER_2 = -1; public static final int SHOOTER_PIVOT= 17; public static final int SHOOTER_ENCODER = 18; public static final int CLIMBER = 19; diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index 7a03198..1678fc8 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -1,15 +1,24 @@ package com.team2813.subsystems; +import com.team2813.lib2813.control.Motor; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.subsystems.MotorSubsystem; import static com.team2813.Constants.*; +import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.TalonFXInvertType; public class Shooter extends MotorSubsystem { + Motor shooterMotor; public Shooter() { // TODO: fix invert type - super(new MotorSubsystemConfiguration(new TalonFXWrapper(0, TalonFXInvertType.Clockwise))); + super(new MotorSubsystemConfiguration( + new TalonFXWrapper(SHOOTER_PIVOT, TalonFXInvertType.Clockwise), + null //TODO: write encoder class (embarassing wow) + )); + TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise); + m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster); + shooterMotor = m; } public static enum Angle implements MotorSubsystem.Position { TEST(0.0); From b9ac42c60827677144940d0b4ea32283e985e369 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 24 Jan 2024 20:27:10 -0800 Subject: [PATCH 3/5] oops --- .../com/team2813/lib2813/subsystems/MotorSubsystem.java | 9 ++++++++- .../src/main/java/com/team2813/subsystems/Shooter.java | 4 ++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 953e6b8..6cad962 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -13,7 +13,7 @@ /** * Defines PID controll over a motor, with values specified by an encoder * - * @param the {@link MotorSubsystem.Position} type to use positions from. must be an enum + * @param the {@link MotorSubsystem.Position} type to use positions from. */ public abstract class MotorSubsystem extends PIDSubsystem { @@ -115,6 +115,11 @@ public MotorSubsystemConfiguration startingPosition(Position startingPosition) { this.startingPosition = startingPosition.getPos(); return this; } + + public MotorSubsystemConfiguration acceptableError(double acceptableError) { + this.acceptableError = acceptableError; + return this; + } } /** @@ -165,10 +170,12 @@ public void set(ControlMode mode, double demand) { motor.set(mode, demand); } + @Override protected void useOutput(double output, double setpoint) { motor.set(ControlMode.DUTY_CYCLE, output); } + @Override protected double getMeasurement() { return encoder.position(); } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index 1678fc8..f790f9c 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -5,7 +5,6 @@ import com.team2813.lib2813.subsystems.MotorSubsystem; import static com.team2813.Constants.*; -import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.TalonFXInvertType; public class Shooter extends MotorSubsystem { @@ -19,10 +18,11 @@ public Shooter() { TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise); m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster); shooterMotor = m; + setSetpoint(Angle.TEST); } public static enum Angle implements MotorSubsystem.Position { TEST(0.0); - private double pos; + private final double pos; Angle(double pos) { this.pos = pos; } From 02a9ca662ec5dc480a7b3cc5b0df883f308b5a4a Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Sat, 27 Jan 2024 16:58:47 -0800 Subject: [PATCH 4/5] added encoder code --- .../control/encoders/CancoderWrapper.java | 37 +++++++++++++++++++ .../team2813/lib2813/util/ConfigUtils.java | 12 ++++++ .../src/main/java/com/team2813/Constants.java | 14 +++---- 3 files changed, 56 insertions(+), 7 deletions(-) create mode 100644 Robot2024/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java new file mode 100644 index 0000000..829df9a --- /dev/null +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java @@ -0,0 +1,37 @@ +package com.team2813.lib2813.control.encoders; +import com.team2813.lib2813.control.DeviceInformation; +import com.team2813.lib2813.control.Encoder; +import com.team2813.lib2813.util.ConfigUtils; +import com.ctre.phoenix6.hardware.CANcoder; + +public class CancoderWrapper implements Encoder { + private CANcoder cancoder; + private DeviceInformation info; + public CancoderWrapper(int id, String canbus) { + cancoder = new CANcoder(id, canbus); + info = new DeviceInformation(id, canbus); + } + public CancoderWrapper(int id) { + cancoder = new CANcoder(id); + info = new DeviceInformation(id); + } + @Override + public double position() { + return cancoder.getPosition().getValueAsDouble(); + } + + @Override + public void setPosition(double position) { + ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position)); + } + public CANcoder encoder() { + return cancoder; + } + @Override + public boolean equals(Object obj) { + if (!(obj instanceof CancoderWrapper)) + return false; + CancoderWrapper other = (CancoderWrapper) obj; + return info.equals(other.info); + } +} diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java index 1e54d69..07025bc 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java @@ -3,6 +3,7 @@ import java.util.function.Supplier; import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix6.StatusCode; import com.revrobotics.REVLibError; import edu.wpi.first.wpilibj.DriverStation; @@ -26,6 +27,17 @@ public static void revConfig(Supplier configMethod) { } } + public static void phoenix6Config(Supplier configMethod) { + StatusCode errorCode = configMethod.get(); + for (int i = 1; i <= ATTEMPTS && errorCode != StatusCode.OK; i++) { + DriverStation.reportError(String.format("%s: Config Attempt %d Failed", errorCode.toString(), i), false); + errorCode = configMethod.get(); + } + if (errorCode != StatusCode.OK) { + DriverStation.reportError(String.format("%s: Config Failed", errorCode.toString()), false); + } + } + public static void ctreConfig(Supplier configMethod) { ErrorCode errorCode = configMethod.get(); if (errorCode != ErrorCode.OK) { diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index 3bde732..3254e6a 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -52,11 +52,11 @@ public static class DriverConstants { public static final int INTAKE = 14; public static final int KICKER = 15; public static final int SHOOTER_1 = 16; - public static final int SHOOTER_2 = -1; - public static final int SHOOTER_PIVOT= 17; - public static final int SHOOTER_ENCODER = 18; - public static final int CLIMBER = 19; - public static final int MAGAZINE = 20; - public static final int INTAKE_ENCODER = 21; - public static final int INTAKE_PIVOT = 22; + public static final int SHOOTER_2 = 17; + public static final int SHOOTER_PIVOT= 18; + public static final int SHOOTER_ENCODER = 19; + public static final int CLIMBER = 20; + public static final int MAGAZINE = 21; + public static final int INTAKE_ENCODER = 22; + public static final int INTAKE_PIVOT = 23; } From c631574a10ff1356d3316345f660bb9ee5fe6f53 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Mon, 29 Jan 2024 14:35:37 -0800 Subject: [PATCH 5/5] added default shooter command --- .../java/com/team2813/RobotContainer.java | 7 ++++ .../commands/DefaultShooterCommand.java | 34 +++++++++++++++++++ .../java/com/team2813/subsystems/Shooter.java | 12 ++++++- 3 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java diff --git a/Robot2024/src/main/java/com/team2813/RobotContainer.java b/Robot2024/src/main/java/com/team2813/RobotContainer.java index a4179e0..2fae95c 100644 --- a/Robot2024/src/main/java/com/team2813/RobotContainer.java +++ b/Robot2024/src/main/java/com/team2813/RobotContainer.java @@ -14,14 +14,20 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import com.team2813.commands.DefaultDriveCommand; +import com.team2813.commands.DefaultShooterCommand; import com.team2813.subsystems.Drive; +import com.team2813.subsystems.Shooter; + import static com.team2813.Constants.*; import static com.team2813.Constants.DriverConstants.*; +import static com.team2813.Constants.OperatorConstants.*; public class RobotContainer { private final SendableChooser autoChooser; private final Drive drive = new Drive(); + private final Shooter shooter = new Shooter(); private final XboxController driverController = new XboxController(driverControllerPort); + private final XboxController operatorController = new XboxController(operatorControllerPort); public RobotContainer() { drive.setDefaultCommand(new DefaultDriveCommand( () -> -modifyAxis(driverController.getLeftY()) * Drive.MAX_VELOCITY, @@ -29,6 +35,7 @@ public RobotContainer() { () -> -modifyAxis(driverController.getRightX()) * Drive.MAX_ANGULAR_VELOCITY, drive )); + shooter.setDefaultCommand(new DefaultShooterCommand(shooter, operatorController::getRightY)); configureBindings(); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto", autoChooser); diff --git a/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java new file mode 100644 index 0000000..dd7c3e2 --- /dev/null +++ b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java @@ -0,0 +1,34 @@ +package com.team2813.commands; + +import java.util.function.DoubleSupplier; + +import com.team2813.lib2813.control.ControlMode; +import com.team2813.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DefaultShooterCommand extends Command { + private Shooter shooter; + private DoubleSupplier controll; + public DefaultShooterCommand(Shooter shooter, DoubleSupplier controll) { + this.shooter = shooter; + this.controll = controll; + } + /** + * The number to multiply results from {@link #controll}. Multiplies by {@value #MULTIPLIER} + */ + private static final double MULTIPLIER = 1; + private static final double DEADZONE = 0.01; + /** + * Sets the motor to the value from the {@link DoubleSupplier} {@link #controll}. + * Multiplies the result by {@value #MULTIPLIER}. The return from {@link #controll} MUST be less than or equal to 1. + */ + public void excecute() { + double val = controll.getAsDouble(); + if (Math.abs(val) > DEADZONE) { + shooter.set(ControlMode.DUTY_CYCLE, val * MULTIPLIER); + } else { + shooter.set(ControlMode.DUTY_CYCLE, 0); + } + } +} diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index f790f9c..46ff2e1 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -1,6 +1,8 @@ package com.team2813.subsystems; +import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.Motor; +import com.team2813.lib2813.control.encoders.CancoderWrapper; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.subsystems.MotorSubsystem; import static com.team2813.Constants.*; @@ -13,13 +15,21 @@ public Shooter() { // TODO: fix invert type super(new MotorSubsystemConfiguration( new TalonFXWrapper(SHOOTER_PIVOT, TalonFXInvertType.Clockwise), - null //TODO: write encoder class (embarassing wow) + new CancoderWrapper(SHOOTER_ENCODER) )); TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise); m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster); shooterMotor = m; setSetpoint(Angle.TEST); } + public void stop() { + shooterMotor.set(ControlMode.DUTY_CYCLE, 0); + } + + public void run(double demand) { + shooterMotor.set(ControlMode.DUTY_CYCLE, demand); + } + public static enum Angle implements MotorSubsystem.Position { TEST(0.0); private final double pos;