From 27fd8cdfdbe8b4c133119439d9842a192e6d40d4 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu, 29 Feb 2024 15:46:39 -0800 Subject: [PATCH 1/5] Squashed commit of the following: commit cdcf43840388fe8989e12506315f64dc5e42ec2b Merge: 40ad7bb 326c5c5 Author: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu Feb 29 15:36:39 2024 -0800 Merge branch 'main' into onTheSubjectOfRelocatingRubberHoops commit 40ad7bb8ee2f9f910a3725ee027f5ddbd63b60bc Author: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu Feb 1 17:58:17 2024 -0800 Rename IntakeShooter.Java to IntakeShooter.java commit 8f683e0c09fec2d87e3568d3427a387e560de927 Author: Aceius E. Date: Thu Feb 1 17:40:27 2024 -0800 Fix compile error The compiler doesn't like if you capitalize .java commit 192edcd1b24c6fb6270126e08df62f4c7cf92969 Author: Aceius E. Date: Thu Feb 1 17:19:59 2024 -0800 Change to Talon controllers for flywheel commit d0b206795c7d090d38a7e6b633adaf4cbc449a34 Author: Aceius E. Date: Thu Feb 1 17:11:16 2024 -0800 Constructor allows speed change commit 0ebe58a38b99f21eab31c1e2ddb15081529f9d4d Author: Aceius E. Date: Thu Feb 1 17:09:42 2024 -0800 Test shooter command commit 2f051b8322d410b8d654f684c5276cd833f56783 Author: Aceius E. Date: Thu Feb 1 16:46:49 2024 -0800 Skeleton of the skeleton for the intake shooter --- .../frc/robot/commands/tests/Shooter.java | 36 ++++++++++++ .../frc/robot/subsystems/IntakeShooter.java | 56 +++++++++++++++++++ 2 files changed, 92 insertions(+) create mode 100644 src/main/java/frc/robot/commands/tests/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeShooter.java diff --git a/src/main/java/frc/robot/commands/tests/Shooter.java b/src/main/java/frc/robot/commands/tests/Shooter.java new file mode 100644 index 0000000..bcc23e9 --- /dev/null +++ b/src/main/java/frc/robot/commands/tests/Shooter.java @@ -0,0 +1,36 @@ +package frc.robot.commands.tests; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IntakeShooter; + +public class Shooter extends Command { + private final IntakeShooter intakeShooter; + private final int speed; + private final long endSpinning; + + public Shooter(IntakeShooter intakeShooter, int speed, long spinTimeMillis) { + this.intakeShooter = intakeShooter; + addRequirements(this.intakeShooter); + + this.speed = speed; + + long beganSpinning = System.currentTimeMillis(); + this.endSpinning = (beganSpinning + spinTimeMillis); + } + + @Override + public void initialize() { + this.intakeShooter.setFlywheelSpeed(speed); + } + + @Override + public boolean isFinished() { + // Wait 1 second before finishing + return endSpinning >= System.currentTimeMillis(); + } + + @Override + public void end(boolean interrupted) { + this.intakeShooter.stopFlywheels(); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/IntakeShooter.java new file mode 100644 index 0000000..7b488f4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeShooter.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems; +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * The intake & shooter system (mounted to the end of the arm) + * + * Other proposed names for this class were: + * The Relocator + * Non-Linear Accelarator + * I. C. E. + * Throngler + * + * @author Aceius E. + */ +public class IntakeShooter extends SubsystemBase { + /* + * 2 motors for flywheel + * 2 for intake + * neo550s, 2 sim motors, + */ + + private final Talon flywheel1; // AndyMark CIM, doublecheck this + private final Talon flywheel2; + private final CANSparkMax intake1; + private final CANSparkMax intake2; // might not exist + + public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { + this.flywheel1 = new Talon(flywheel1id); + this.flywheel2 = new Talon(flywheel2id); + this.intake1 = new CANSparkMax(intake1id, CANSparkLowLevel.MotorType.kBrushless); + this.intake2 = new CANSparkMax(intake2id, CANSparkLowLevel.MotorType.kBrushless); + } + + public void setFlywheelSpeed(double speed) { + flywheel1.set(speed); + flywheel2.set(speed); + } + + public void stopFlywheels() { + flywheel1.stopMotor(); + flywheel2.stopMotor(); + } + + /** + * This method is called periodically by the CommandScheduler, about every 20ms. + * It should be used for updating subsystem-specific state that you don't want to offload to a Command. + * Try to avoid "doing to much" in this method (for example no driver control here). + */ + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 6ec5e4702809d6addcd9edc5b213ca5fd1b14fa6 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu, 29 Feb 2024 16:15:36 -0800 Subject: [PATCH 2/5] add Control for IntakeShooter --- src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 42 ++++++--- .../frc/robot/commands/tests/Shooter.java | 36 -------- .../frc/robot/subsystems/IntakeShooter.java | 89 +++++++++++-------- 5 files changed, 87 insertions(+), 88 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/tests/Shooter.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8b1e68c..86c2299 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,8 +90,7 @@ public static class ArmConstants { public static final double ARM_INTAKE_DEGREES = -40; public static final int LEFT_MOTOR_ID = 5; - // public static final int LEFT_ENCODER_ID = 0; - public static final int RIGHT_MOTOR_ID = 19; + public static final int RIGHT_MOTOR_ID = 19; public static final int RIGHT_ENCODER_ID = 6; public static final boolean ARE_MOTORS_REVERSED = false; @@ -101,7 +100,11 @@ public static class ArmConstants { public static final double ELEVATION_PID_P = 15; public static final double ELEVATION_PID_I = 0; public static final double ELEVATION_PID_D = 0; + } + public static class IntakeShooterConstants { + public static final boolean INTAKE_REVERSE = false; + public static final boolean FLYWHEEL_REVERSE = false; } public static class RobotMovementConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c98c1d3..a78ddc7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -87,6 +87,7 @@ public void teleopInit() { } robotContainer.setUpDriveController(); + robotContainer.setUpOperatorController(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61aa40b..d172a81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,10 +94,6 @@ public class RobotContainer { private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE); - private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES); - private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES); - private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); - private final LightStrip lightStrip = new LightStrip(new AddressableLED(LightConstants.LED_CONTROLLER_PWM_SLOT)); /** @@ -110,6 +106,8 @@ public RobotContainer() { autoChooser.addOption("Make LEDs blue", new SetLightstripColor(lightStrip, 0, 0, 200)); SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber); + configureBindings(); setUpDriveController(); @@ -122,10 +120,7 @@ public void setUpDriveController() { final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT); final HIDType genericHIDType = genericHID.getType(); - final CommandJoystick operatorJoystick = new CommandJoystick(DriverConstants.OPERATOR_JOYSTICK_PORT); - SmartDashboard.putString("Drive Controller", genericHIDType.toString()); - SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber); drivetrain.removeDefaultCommand(); @@ -148,12 +143,6 @@ public void setUpDriveController() { joystick.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative)); - // This bypasses arm remote control, arm remote control is incompatible with - // autonomous commands - operatorJoystick.button(4).onTrue(armToIntake); - operatorJoystick.button(5).onTrue(armToAmp); - operatorJoystick.button(6).onTrue(armToSpeaker); - // joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain)); // joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, // drivetrain)); @@ -180,6 +169,33 @@ public void setUpDriveController() { drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs)); } + public void setUpOperatorController() { + // Create joysticks + final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT); + final HIDType genericHIDType = genericHID.getType(); + + final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES); + final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES); + final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); + + SmartDashboard.putString("Operator Controller", genericHIDType.toString()); + + if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { + final CommandJoystick joystick = new CommandJoystick(genericHID.getPort()); + + joystick.button(4).onTrue(armToIntake); + joystick.button(5).onTrue(armToAmp); + joystick.button(6).onTrue(armToSpeaker); + } else { + final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); + + xbox.rightTrigger().onTrue(armToIntake); + xbox.leftTrigger(5).onTrue(armToSpeaker); + xbox.povDown().onTrue(armToAmp); + + } + } + /** Use this method to define your trigger->command mappings. */ private void configureBindings() { } diff --git a/src/main/java/frc/robot/commands/tests/Shooter.java b/src/main/java/frc/robot/commands/tests/Shooter.java deleted file mode 100644 index bcc23e9..0000000 --- a/src/main/java/frc/robot/commands/tests/Shooter.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.commands.tests; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.IntakeShooter; - -public class Shooter extends Command { - private final IntakeShooter intakeShooter; - private final int speed; - private final long endSpinning; - - public Shooter(IntakeShooter intakeShooter, int speed, long spinTimeMillis) { - this.intakeShooter = intakeShooter; - addRequirements(this.intakeShooter); - - this.speed = speed; - - long beganSpinning = System.currentTimeMillis(); - this.endSpinning = (beganSpinning + spinTimeMillis); - } - - @Override - public void initialize() { - this.intakeShooter.setFlywheelSpeed(speed); - } - - @Override - public boolean isFinished() { - // Wait 1 second before finishing - return endSpinning >= System.currentTimeMillis(); - } - - @Override - public void end(boolean interrupted) { - this.intakeShooter.stopFlywheels(); - } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/IntakeShooter.java index 7b488f4..2bbe692 100644 --- a/src/main/java/frc/robot/subsystems/IntakeShooter.java +++ b/src/main/java/frc/robot/subsystems/IntakeShooter.java @@ -1,8 +1,10 @@ package frc.robot.subsystems; + import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakeShooterConstants; /** * The intake & shooter system (mounted to the end of the arm) @@ -16,41 +18,54 @@ * @author Aceius E. */ public class IntakeShooter extends SubsystemBase { - /* - * 2 motors for flywheel - * 2 for intake - * neo550s, 2 sim motors, - */ - - private final Talon flywheel1; // AndyMark CIM, doublecheck this - private final Talon flywheel2; - private final CANSparkMax intake1; - private final CANSparkMax intake2; // might not exist - - public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { - this.flywheel1 = new Talon(flywheel1id); - this.flywheel2 = new Talon(flywheel2id); - this.intake1 = new CANSparkMax(intake1id, CANSparkLowLevel.MotorType.kBrushless); - this.intake2 = new CANSparkMax(intake2id, CANSparkLowLevel.MotorType.kBrushless); - } - - public void setFlywheelSpeed(double speed) { - flywheel1.set(speed); - flywheel2.set(speed); - } - - public void stopFlywheels() { - flywheel1.stopMotor(); - flywheel2.stopMotor(); - } - - /** - * This method is called periodically by the CommandScheduler, about every 20ms. - * It should be used for updating subsystem-specific state that you don't want to offload to a Command. - * Try to avoid "doing to much" in this method (for example no driver control here). - */ - @Override - public void periodic() { - // This method will be called once per scheduler run - } + /* + * 2 motors for flywheel + * 2 for intake + * neo550s, 2 sim motors, + */ + + private final Talon flywheel1; // AndyMark CIM, doublecheck this + private final Talon flywheel2; + private final CANSparkMax intake1; + private final CANSparkMax intake2; // might not exist + + public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { + this.flywheel1 = new Talon(flywheel1id); + this.flywheel2 = new Talon(flywheel2id); + + flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE); + flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE); + + this.intake1 = new CANSparkMax(intake1id, CANSparkLowLevel.MotorType.kBrushless); + this.intake2 = new CANSparkMax(intake2id, CANSparkLowLevel.MotorType.kBrushless); + + intake1.setInverted(IntakeShooterConstants.INTAKE_REVERSE); + intake2.setInverted(IntakeShooterConstants.INTAKE_REVERSE); + } + + public void startFlyWheels() { + flywheel1.set(1); + flywheel2.set(1); + } + + public void stopFlywheels() { + flywheel1.stopMotor(); + flywheel2.stopMotor(); + } + + public void intake() { + intake1.set(1); + intake1.set(1); + } + + public void intakeReverse() { + intake1.set(-1); + intake1.set(-1); + } + + public void stopIntake() { + intake1.stopMotor(); + intake1.stopMotor(); + } + } From 79f6d072cb321da1d7dd63f2c64033e15d423bf3 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu, 29 Feb 2024 16:32:07 -0800 Subject: [PATCH 3/5] remove unused method from arm --- .../frc/robot/subsystems/arm/ArmInterface.java | 2 -- .../java/frc/robot/subsystems/arm/DummyArm.java | 4 ---- .../java/frc/robot/subsystems/arm/RealArm.java | 15 --------------- 3 files changed, 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java index e49ac11..8126216 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java @@ -7,8 +7,6 @@ * RealArm (formerly known as Arm) class. */ public interface ArmInterface extends Subsystem { - public void changeArmAngleDegreesBy(double desiredDegrees); - public void setArmToAmpPosition(); public void setArmToSpeakerPosition(); diff --git a/src/main/java/frc/robot/subsystems/arm/DummyArm.java b/src/main/java/frc/robot/subsystems/arm/DummyArm.java index 6989509..a0623bb 100644 --- a/src/main/java/frc/robot/subsystems/arm/DummyArm.java +++ b/src/main/java/frc/robot/subsystems/arm/DummyArm.java @@ -7,10 +7,6 @@ * robot does not have an arm in order to prevent errors. */ public class DummyArm extends SubsystemBase implements ArmInterface { - public void changeArmAngleDegreesBy(double desiredDegrees) { - System.out.println(desiredDegrees); - } - public void setArmToAmpPosition() { } diff --git a/src/main/java/frc/robot/subsystems/arm/RealArm.java b/src/main/java/frc/robot/subsystems/arm/RealArm.java index a3f50c8..a0d907a 100644 --- a/src/main/java/frc/robot/subsystems/arm/RealArm.java +++ b/src/main/java/frc/robot/subsystems/arm/RealArm.java @@ -55,21 +55,6 @@ public RealArm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean ar } - // public void setArmAngleDegrees(double desiredDegree) { - // //maximum should ALWAYS be a greater value then minimum - // if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || - // ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) { - // armRotation2d = Rotation2d.fromDegrees(desiredDegree); - // } - // } - - public void changeArmAngleDegreesBy(double desiredDegrees) { - if (armSetpoint.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES - || ArmConstants.MINIMUM_ARM_DEGREES > armSetpoint.getDegrees()) { - armSetpoint = Rotation2d.fromDegrees(armSetpoint.getDegrees() + desiredDegrees); - } - } - public void setArmToAmpPosition() { armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES); } From ea273196b675905e63c925895d2bf28726af427b Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu, 29 Feb 2024 16:36:44 -0800 Subject: [PATCH 4/5] Squashed commit of the following: commit cd61ef593ed60911513568107f2ac026b4edfd3c Author: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Wed Feb 28 10:31:36 2024 -0800 add missing files commit 853a614b66e1f036b9962b256b2800ba932806d7 Author: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Wed Feb 28 10:31:12 2024 -0800 Allow arm to be moved manully by operator --- .../java/frc/robot/commands/ArmRotateBy.java | 33 +++++++++++++ .../java/frc/robot/commands/ArmRotateTo.java | 14 ++++-- .../java/frc/robot/subsystems/arm/Arm.java | 35 ++++++++++++++ .../robot/subsystems/arm/ArmInterface.java | 19 -------- .../frc/robot/subsystems/arm/DummyArm.java | 22 +++++---- .../frc/robot/subsystems/arm/RealArm.java | 46 +++++++------------ 6 files changed, 107 insertions(+), 62 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ArmRotateBy.java create mode 100644 src/main/java/frc/robot/subsystems/arm/Arm.java delete mode 100644 src/main/java/frc/robot/subsystems/arm/ArmInterface.java diff --git a/src/main/java/frc/robot/commands/ArmRotateBy.java b/src/main/java/frc/robot/commands/ArmRotateBy.java new file mode 100644 index 0000000..80fe1d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmRotateBy.java @@ -0,0 +1,33 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.Arm; + +public class ArmRotateBy extends Command { + + private final Rotation2d dSetpoint; + private final Arm arm; + + public ArmRotateBy(Arm arm, Rotation2d dRotation) { + this.dSetpoint = dRotation; + this.arm = arm; + + addRequirements(arm); + } + + public ArmRotateBy(Arm arm, double dDegrees) { + this(arm, Rotation2d.fromDegrees(dDegrees)); + } + + @Override + public void initialize() { + arm.setSetpoint(arm.getArmPosition().plus(dSetpoint)); + } + + @Override + public boolean isFinished() { + // return arm.isAtDesiredPosition(); + return true; + } +} diff --git a/src/main/java/frc/robot/commands/ArmRotateTo.java b/src/main/java/frc/robot/commands/ArmRotateTo.java index 6927824..86c8fa6 100644 --- a/src/main/java/frc/robot/commands/ArmRotateTo.java +++ b/src/main/java/frc/robot/commands/ArmRotateTo.java @@ -1,19 +1,23 @@ package frc.robot.commands; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.arm.ArmInterface; +import frc.robot.subsystems.arm.Arm; public class ArmRotateTo extends Command { - private final double setpoint; - private final ArmInterface arm; + private final Rotation2d setpoint; + private final Arm arm; - public ArmRotateTo(ArmInterface arm, double degree) { - this.setpoint = degree; + public ArmRotateTo(Arm arm, Rotation2d rotation) { + this.setpoint = rotation; this.arm = arm; addRequirements(arm); + } + public ArmRotateTo(Arm arm, double degrees) { + this(arm, Rotation2d.fromDegrees(degrees)); } @Override diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java new file mode 100644 index 0000000..e092966 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ArmConstants; + +/** + * This interface represnets the arm. This interface is implemented by the + * RealArm (formerly known as Arm) class. + */ +public abstract class Arm extends SubsystemBase { + + public abstract void setSetpoint(double degrees); + + public void setSetpoint(Rotation2d rotation) { + setSetpoint(rotation.getDegrees()); + } + + public abstract boolean isAtDesiredPosition(); + + public abstract Rotation2d getArmPosition(); + + + public void setArmToAmpPosition() { + setSetpoint(ArmConstants.ARM_AMP_SHOOTING_DEGREES); + } + + public void setArmToSpeakerPosition() { + setSetpoint(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); + } + + public void setArmToIntakePosition() { + setSetpoint(ArmConstants.ARM_INTAKE_DEGREES); + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java deleted file mode 100644 index 8126216..0000000 --- a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.subsystems.arm; - -import edu.wpi.first.wpilibj2.command.Subsystem; - -/** - * This interface represnets the arm. This interface is implemented by the - * RealArm (formerly known as Arm) class. - */ -public interface ArmInterface extends Subsystem { - public void setArmToAmpPosition(); - - public void setArmToSpeakerPosition(); - - public void setArmToIntakePosition(); - - public void setSetpoint(double degree); - - public boolean isAtDesiredPosition(); -} diff --git a/src/main/java/frc/robot/subsystems/arm/DummyArm.java b/src/main/java/frc/robot/subsystems/arm/DummyArm.java index a0623bb..2296581 100644 --- a/src/main/java/frc/robot/subsystems/arm/DummyArm.java +++ b/src/main/java/frc/robot/subsystems/arm/DummyArm.java @@ -1,23 +1,27 @@ package frc.robot.subsystems.arm; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * This is a dummy stand in class that represents the arm, in the event that the * robot does not have an arm in order to prevent errors. */ -public class DummyArm extends SubsystemBase implements ArmInterface { - public void setArmToAmpPosition() { - } +public class DummyArm extends Arm { - public void setArmToSpeakerPosition() { - } + private Rotation2d armPosition = new Rotation2d(); + + @Override + public void setSetpoint(double degrees) { + SmartDashboard.putNumber("Arm Setpoint Degrees", degrees); + SmartDashboard.putNumber("Arm Degrees", degrees); - public void setArmToIntakePosition() { + armPosition = Rotation2d.fromDegrees(degrees); } - public void setSetpoint(double degree) { - System.out.println(degree); + @Override + public Rotation2d getArmPosition() { + return armPosition; } public boolean isAtDesiredPosition() { diff --git a/src/main/java/frc/robot/subsystems/arm/RealArm.java b/src/main/java/frc/robot/subsystems/arm/RealArm.java index a0d907a..8d57c4c 100644 --- a/src/main/java/frc/robot/subsystems/arm/RealArm.java +++ b/src/main/java/frc/robot/subsystems/arm/RealArm.java @@ -2,8 +2,8 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ArmConstants; import com.revrobotics.CANSparkMax; @@ -13,39 +13,32 @@ import com.ctre.phoenix6.hardware.CANcoder; /** This is the subsystem that represents the arm. */ -public class RealArm extends SubsystemBase implements ArmInterface { +public class RealArm extends Arm { private final CANSparkMax leftArmMotor; - // private final CANcoder leftArmEncoder; - - // leftArmEncoder may be useless, as the right arm equivelent is used for the - // position for both.

- private final CANSparkMax rightArmMotor; + private final CANcoder rightArmEncoder; private final StatusSignal armPosition; private final PIDController armRaisePIDController; - private Rotation2d armSetpoint; - /** Constructor. Creates a new Arm Subsystem. */ public RealArm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean areMotorsReversed) { leftArmMotor = new CANSparkMax(leftMotorId, MotorType.kBrushless); - // leftArmEncoder = new CANcoder(leftEncoderId); - rightArmMotor = new CANSparkMax(rightMotorId, MotorType.kBrushless); + rightArmEncoder = new CANcoder(rightEncoderId); armRaisePIDController = new PIDController( ArmConstants.ELEVATION_PID_P, ArmConstants.ELEVATION_PID_I, ArmConstants.ELEVATION_PID_D); + armPosition = rightArmEncoder.getAbsolutePosition(); - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES); leftArmMotor.setIdleMode(IdleMode.kCoast); rightArmMotor.setIdleMode(IdleMode.kCoast); @@ -53,22 +46,20 @@ public RealArm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean ar leftArmMotor.setInverted(areMotorsReversed); rightArmMotor.setInverted(!areMotorsReversed); + setSetpoint(ArmConstants.ARM_INTAKE_DEGREES); } - public void setArmToAmpPosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES); - } + public void setSetpoint(double degrees) { + degrees = Math.max(degrees, ArmConstants.MINIMUM_ARM_DEGREES); + degrees = Math.min(degrees, ArmConstants.MAXIMUM_ARM_DEGREES); - public void setArmToSpeakerPosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); - } + SmartDashboard.putNumber("Arm Setpoint Degrees", degrees); - public void setArmToIntakePosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES); + armRaisePIDController.setSetpoint(Units.degreesToRotations(degrees)); } - public void setSetpoint(double degree) { - armSetpoint = Rotation2d.fromDegrees(degree); + public Rotation2d getArmPosition() { + return Rotation2d.fromRotations(armPosition.refresh().getValueAsDouble()); } public boolean isAtDesiredPosition() { @@ -82,17 +73,14 @@ public boolean isAtDesiredPosition() { public void periodic() { // This method will be called once per scheduler run - double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(), - armSetpoint.getRotations()); + Rotation2d currentOnPosition = getArmPosition(); - SmartDashboard.putNumber("Arm Speed", armSpeed); + double armSpeed = armRaisePIDController.calculate(currentOnPosition.getRotations()); leftArmMotor.set(armSpeed); rightArmMotor.set(armSpeed); - SmartDashboard.putNumber("Arm Degrees", - Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees()); - SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees()); - + SmartDashboard.putNumber("Arm Speed", armSpeed); + SmartDashboard.putNumber("Arm Degrees", currentOnPosition.getDegrees()); } } From e1694f74bed809833cdfab066afd87d0316debc3 Mon Sep 17 00:00:00 2001 From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com> Date: Thu, 29 Feb 2024 16:37:42 -0800 Subject: [PATCH 5/5] Switch to Arm base class over interface --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/Autos.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d172a81..a72a206 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,7 @@ import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.SwerveModule; import frc.robot.subsystems.Vision; -import frc.robot.subsystems.arm.ArmInterface; +import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.DummyArm; import frc.robot.subsystems.arm.RealArm; import frc.robot.inputs.ChassisDriveInputs; @@ -81,7 +81,7 @@ public class RobotContainer { * if ArmConstants.HAS_ARM is false, a dummy class implementing the arm's API is * created instead to prevent errors. */ - private final ArmInterface arm = Constants.ArmConstants.HAS_ARM ? new RealArm( + private final Arm arm = Constants.ArmConstants.HAS_ARM ? new RealArm( ArmConstants.LEFT_MOTOR_ID, ArmConstants.RIGHT_MOTOR_ID, ArmConstants.RIGHT_ENCODER_ID, diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 31a93a2..cf0b373 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -1,7 +1,7 @@ package frc.robot.commands; import frc.robot.subsystems.SwerveDrivetrain; -import frc.robot.subsystems.arm.ArmInterface; +import frc.robot.subsystems.arm.Arm; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -25,7 +25,7 @@ public static Command rotateTestAuto(SwerveDrivetrain drivetrain, double degrees } /** Linden did this */ - public static Command startingAuto(ArmInterface arm, SwerveDrivetrain drivetrain, boolean invertY) { + public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) { // assumes start position in corner double invert = 1;