diff --git a/shuffleboard.json b/shuffleboard.json index 2142459..9c3e998 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -140,7 +140,7 @@ }, "6,2": { "size": [ - 3, + 2, 1 ], "content": { @@ -208,6 +208,60 @@ "Visuals/Orientation": "HORIZONTAL" } }, + "7,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/tPoseX", + "_title": "tPoseX", + "_glyph": 110, + "_showGlyph": true + } + }, + "6,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/tPoseY", + "_title": "tPoseY", + "_glyph": 111, + "_showGlyph": true + } + }, + "8,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/tPoseDegrees", + "_title": "tPoseDegrees", + "_glyph": 419, + "_showGlyph": true + } + }, + "8,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/tPoseActive", + "_title": "tPoseActive", + "_glyph": 288, + "_showGlyph": true, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, "1,0": { "size": [ 2, diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..f430671 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,12 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eaf2962..c3faae7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -76,7 +76,6 @@ public static class HangConstants { public static class DriverConstants { public static final int DRIVER_JOYSTICK_PORT = 0; - public static final int OPERATOR_JOYSTICK_PORT = 1; public static final double DEAD_ZONE = 0.25; @@ -130,8 +129,28 @@ public static class ArmConstants { } public static class IntakeShooterConstants { + static { + switch (currentBot) { + case WOOD_BOT: + HAS_INTAKE = false; + break; + + case COMP_BOT: + default: + HAS_INTAKE = true; + break; + } + } + + public static final boolean HAS_INTAKE; + public static final boolean INTAKE_REVERSE = false; public static final boolean FLYWHEEL_REVERSE = false; + + public static final int FLYWHEEL_MOTOR_LEFT_ID = 1; + public static final int FLYWHEEL_MOTOR_RIGHT_ID = 2; + public static final int INTAKE_MOTOR_LEFT_ID = 3; + public static final int INTAKE_MOTOR_RIGHT_ID = 4; } public static class RobotMovementConstants { @@ -282,14 +301,29 @@ public static class SwerveDrivetrainConstants { public static final double MODULE_LOCATION_X; } + public static class AutoConstants { + // preffered distance to tag, specifically for autopositioning the robot to in + // front of the tag + public static final double PREFERRED_TAG_DISTANCE = 2; + } + public static class VisionConstants { - public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d()); + public static final Transform3d CAMERA_POSE = new Transform3d(0, 0, 0, new Rotation3d()); public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera"; } public static class LightConstants { - public static final int LED_CONTROLLER_PWM_SLOT = 0; + public static final int LED_CONTROLLER_PWM_SLOT = 1; public static final int LED_QUANTITY = 60; + + public static final double LED_COLOR_RED = 0.61; + public static final double LED_COLOR_ORANGE = 0.65; + public static final double LED_COLOR_YELLOW = 0.69; + public static final double LED_COLOR_GREEN = 0.77; + public static final double LED_COLOR_BLUE = 0.87; + public static final double LED_COLOR_PURPLE = 0.91; + public static final double LED_COLOR_WHITE = 0.93; + public static final double LED_COLOR_RAINBOW = -0.99; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 91de078..782b3f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,11 +2,16 @@ import frc.robot.Constants.DriverConstants; import frc.robot.Constants.HangConstants; +import frc.robot.Constants.IntakeShooterConstants; import frc.robot.Constants.LightConstants; import frc.robot.Constants.SwerveDrivetrainConstants; import frc.robot.Constants.SwerveModuleConstants; import frc.robot.Constants.ArmConstants; +import frc.robot.Constants.AutoConstants; import frc.robot.commands.Autos; +import frc.robot.commands.CancelCommands; +import frc.robot.commands.ChassisRemoteControl; +import frc.robot.commands.FollowTag; import frc.robot.commands.AimAtTag; import frc.robot.commands.ArmRotateTo; import frc.robot.commands.ChassisRemoteControl; @@ -17,17 +22,19 @@ 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.subsystems.hang.DummyHang; import frc.robot.subsystems.hang.Hang; import frc.robot.subsystems.hang.RealHang; +import frc.robot.subsystems.intake.DummyShooter; +import frc.robot.subsystems.intake.IntakeShooter; +import frc.robot.subsystems.intake.RealShooter; import frc.robot.inputs.ChassisDriveInputs; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.HIDType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -86,7 +93,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, @@ -98,14 +105,22 @@ public class RobotContainer { HangConstants.LIMIT_SWITCH_ID) : new DummyHang(); - private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR, + private final IntakeShooter intakeShooter = Constants.IntakeShooterConstants.HAS_INTAKE ? new RealShooter( + IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID, + IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID, + IntakeShooterConstants.INTAKE_MOTOR_LEFT_ID, + IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID) : new DummyShooter(); + + private final SwerveDrivetrain drivetrain = new SwerveDrivetrain( + gyro, + swerveModuleFL, swerveModuleFR, swerveModuleBL, swerveModuleBR); private final SendableChooser autoChooser = new SendableChooser(); private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE); - private final LightStrip lightStrip = new LightStrip(new AddressableLED(LightConstants.LED_CONTROLLER_PWM_SLOT)); + private final LightStrip lightStrip = new LightStrip(LightConstants.LED_CONTROLLER_PWM_SLOT); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -114,7 +129,6 @@ public RobotContainer() { autoChooser.addOption("Rotate by 90", Autos.rotateTestAuto(drivetrain, 90, false)); autoChooser.addOption("Forward", Autos.driveAuto(drivetrain, +1)); autoChooser.addOption("Backward", Autos.driveAuto(drivetrain, -1)); - 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); @@ -122,6 +136,7 @@ public RobotContainer() { configureBindings(); setUpDriveController(); + setUpOperatorController(); PortForwarder.add(5800, "photonvision.local", 5800); } @@ -131,16 +146,18 @@ public void setUpDriveController() { final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT); final HIDType genericHIDType = genericHID.getType(); - SmartDashboard.putString("Drive Controller", genericHIDType.toString()); + final Command cancelCommand = new CancelCommands(drivetrain, arm); drivetrain.removeDefaultCommand(); - ChassisDriveInputs inputs; + if (genericHIDType == null) { + SmartDashboard.putString("Drive Ctrl", "No Connection"); + } else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { + SmartDashboard.putString("Drive Ctrl", "Joystick"); - if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { final CommandJoystick joystick = new CommandJoystick(genericHID.getPort()); - inputs = new ChassisDriveInputs( + ChassisDriveInputs inputs = new ChassisDriveInputs( joystick::getX, -1, joystick::getY, -1, joystick::getTwist, -1, @@ -157,10 +174,15 @@ public void setUpDriveController() { // joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain)); // joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, // drivetrain)); + + drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs)); + } else { + SmartDashboard.putString("Drive Ctrl", "GamePad"); + final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); - inputs = new ChassisDriveInputs( + ChassisDriveInputs inputs = new ChassisDriveInputs( xbox::getLeftX, -1, xbox::getLeftY, -1, xbox::getRightX, -1, @@ -170,32 +192,45 @@ public void setUpDriveController() { // xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates, // drivetrain)); - xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel)); + xbox.povDown().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel)); xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel)); - xbox.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative)); + xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative)); + + xbox.b().onTrue(cancelCommand); xbox.a().whileTrue(new AimAtTag(drivetrain, vision, 1, inputs)); - } + xbox.x().whileTrue(new FollowTag(drivetrain, vision, 1, AutoConstants.PREFERRED_TAG_DISTANCE)); - drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs)); + drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs)); + } } public void setUpOperatorController() { // Create joysticks - final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT); + final GenericHID genericHID = new GenericHID(DriverConstants.OPERATOR_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); + final Command armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES); + final Command armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES); + final Command armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); + + final Command intake = Commands.runOnce(intakeShooter::intake, intakeShooter); + final Command startFlyWheel = Commands.runOnce(intakeShooter::startFlyWheels, intakeShooter); + final Command amplifyLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_BLUE); + final Command coopLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED); + + final Command cancelCommand = new CancelCommands(drivetrain, arm, intakeShooter); + final SetHangSpeed hangStop = new SetHangSpeed(hang, 0); final SetHangSpeed hangForwarSpeed = new SetHangSpeed(hang, HangConstants.speed); final SetHangSpeed hangBackwardSpeed = new SetHangSpeed(hang, -HangConstants.speed); SmartDashboard.putString("Operator Controller", genericHIDType.toString()); - - if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { + if (genericHIDType == null) { + SmartDashboard.putString("Operator Ctrl", "No Connection"); + } else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) { + SmartDashboard.putString("Operator Ctrl", "Joystick"); final CommandJoystick joystick = new CommandJoystick(genericHID.getPort()); joystick.button(4).onTrue(armToIntake); @@ -205,7 +240,11 @@ public void setUpOperatorController() { joystick.button(7).or(joystick.button(8)).whileFalse(hangStop); joystick.button(7).onTrue(hangForwarSpeed); joystick.button(8).onTrue(hangBackwardSpeed); + + joystick.button(9).onTrue(amplifyLightSignal); + joystick.button(10).onTrue(coopLightSignal); } else { + SmartDashboard.putString("Operator Ctrl", "GamePad"); final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); xbox.povDown().onTrue(armToIntake); @@ -216,6 +255,17 @@ public void setUpOperatorController() { xbox.rightBumper().onTrue(hangForwarSpeed); xbox.leftBumper().onTrue(hangBackwardSpeed); + xbox.leftTrigger().onTrue(armToSpeaker); + xbox.leftBumper().onTrue(startFlyWheel); + + xbox.rightTrigger().onTrue(armToIntake); + xbox.rightBumper().onTrue(intake); + + xbox.a().onTrue(armToAmp); + + xbox.povLeft().onTrue(amplifyLightSignal); + xbox.povRight().onTrue(coopLightSignal); + xbox.b().onTrue(cancelCommand); } } @@ -231,4 +281,4 @@ private void configureBindings() { public Command getAutonomousCommand() { return autoChooser.getSelected(); } -} +}; \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java index e89a75a..0f5b179 100644 --- a/src/main/java/frc/robot/commands/AimAtTag.java +++ b/src/main/java/frc/robot/commands/AimAtTag.java @@ -18,7 +18,7 @@ public class AimAtTag extends Command { private final ChassisDriveInputs chassisDriveInputs; private final Vision vision; - private final Integer tagID; // Integer as opposed to int so it can be null for best tag + private final int tagID; private final PIDController rotatePID; @@ -28,11 +28,11 @@ public class AimAtTag extends Command { * * @param drivetrain the drivetrain of the robot * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to turn to, null - * for best tag + * @param tagID the numerical ID of the the tag to turn to, -1 for + * best tag * @param chassisDriveControl collection of inputs for driving */ - public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, ChassisDriveInputs chassisDriveInputs) { + public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, ChassisDriveInputs chassisDriveInputs) { this.drivetrain = drivetrain; this.vision = vision; @@ -69,11 +69,11 @@ public void initialize() { @Override public void execute() { - Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID); + Transform3d transform = vision.getTransformToTag(tagID); double tagYawRadians = 0; - if (distToTag != null) { - Rotation2d angleToTag = new Rotation2d(distToTag.getX(), distToTag.getY()); + if (transform != null) { + Rotation2d angleToTag = new Rotation2d(transform.getX(), transform.getY()); tagYawRadians = angleToTag.getRadians(); } @@ -87,7 +87,7 @@ public void execute() { ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, -rotateSpeed); - drivetrain.setDesiredState(desiredSpeeds, false, true); + drivetrain.setDesiredState(desiredSpeeds, chassisDriveInputs.isFieldRelative(), true); drivetrain.updateSmartDashboard(); } 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/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java index 2ddaac0..d474f58 100644 --- a/src/main/java/frc/robot/commands/AutoDriveTo.java +++ b/src/main/java/frc/robot/commands/AutoDriveTo.java @@ -56,18 +56,18 @@ public void initialize() { public void execute() { Pose2d position = this.drivetrain.getPosition(); - double x = position.getX() - initX; - double y = position.getY() - initY; + double targetX = position.getX() - initX; + double targetY = position.getY() - initY; - double xSpeed = xMovePID.calculate(x, goalX); - double ySpeed = yMovePID.calculate(y, goalY); + double xSpeed = xMovePID.calculate(targetX, goalX); + double ySpeed = yMovePID.calculate(targetY, goalY); double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED; - double maxSpeed = Math.max(Math.abs(x), Math.abs(y)); + double maxSpeed = Math.max(Math.abs(xSpeed), Math.abs(ySpeed)); if (maxSpeed > speedLimit) { - xSpeed /= speedLimit * maxSpeed; - ySpeed /= speedLimit * maxSpeed; + xSpeed = (xSpeed / maxSpeed) * speedLimit; + ySpeed = (ySpeed / maxSpeed) * speedLimit; } drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, 0)); diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java index 604a7c3..ae606c1 100644 --- a/src/main/java/frc/robot/commands/AutoRotateTo.java +++ b/src/main/java/frc/robot/commands/AutoRotateTo.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.RobotMovementConstants; public class AutoRotateTo extends Command { @@ -48,7 +47,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) { public void initialize() { currentAngleGoal = fieldRelative ? 0 : drivetrain.getHeading().getRadians(); currentAngleGoal += angleGoal; - SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal); + // SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal); } @Override @@ -56,7 +55,7 @@ public void execute() { final double currentAngle = drivetrain.getHeading().getRadians(); double turnsSeed = rotatePID.calculate(currentAngle, this.currentAngleGoal); - SmartDashboard.putNumber("Turn Speed Auto", turnsSeed); + // SmartDashboard.putNumber("Turn Speed Auto", turnsSeed); drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed)); 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; diff --git a/src/main/java/frc/robot/commands/CancelCommands.java b/src/main/java/frc/robot/commands/CancelCommands.java new file mode 100644 index 0000000..1dd3f3c --- /dev/null +++ b/src/main/java/frc/robot/commands/CancelCommands.java @@ -0,0 +1,26 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class CancelCommands extends Command { + public CancelCommands(Subsystem... subsystems) { + addRequirements(subsystems); + } + + private static void cancel(Subsystem subsystem) { + Command current = subsystem.getCurrentCommand(); + if (current != null) + current.cancel(); + } + + @Override + public void initialize() { + getRequirements().forEach(CancelCommands::cancel); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java index 80e4ef5..8ea882c 100644 --- a/src/main/java/frc/robot/commands/FollowTag.java +++ b/src/main/java/frc/robot/commands/FollowTag.java @@ -1,13 +1,15 @@ package frc.robot.commands; +import frc.robot.Constants.RobotMovementConstants; import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.Vision; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; /** Command to automatically drive a follow a tag a certain translation away */ @@ -15,11 +17,9 @@ public class FollowTag extends Command { private final SwerveDrivetrain drivetrain; private final Vision vision; - private final Integer tagID; // Integer as opposed to int so it can be null for best tag - private final Transform2d targetDistance; + private final int tagID; - private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode. - private double secondsSinceTagLastSeen; + private final PIDController xController, yController, rotationController; /** * Create a new FollowTag command. Tries to follow a tag while staying a certain @@ -27,83 +27,81 @@ public class FollowTag extends Command { * * @param drivetrain the drivetrain of the robot * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to follow, null - * for best tag - * @param targetDistanceToTag the target distance away from the tag to be + * @param tagID the numerical ID of the the tag to follow, -1 for best tag + * @param targetDistanceToTag the target distance away from the tag to be in meters * @param loseTagAfterSeconds how long to wait before giving up on rediscover * tag, set to null to never finish */ - public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID, - Double loseTagAfterSeconds) { + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Transform2d targetDistanceToTag) { this.drivetrain = drivetrain; this.vision = vision; this.tagID = tagID; - this.targetDistance = targetDistanceToTag; - this.loseTagAfterSeconds = loseTagAfterSeconds; + xController = new PIDController(RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D); xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS); + xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS); + xController.setSetpoint(targetDistanceToTag.getX()); + + yController = new PIDController(RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D); + yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS); + xController.setSetpoint(targetDistanceToTag.getY()); + + rotationController = new PIDController(RobotMovementConstants.ROTATION_PID_P, RobotMovementConstants.ROTATION_PID_I, RobotMovementConstants.ROTATION_PID_D); + rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS); + xController.setSetpoint(targetDistanceToTag.getRotation().getRadians()); addRequirements(drivetrain); } - /** - * Create a new FollowTag command. Tries to follow a tag while staying a certain - * distance away. - * - * @param drivetrain the drivetrain of the robot - * @param vision the vision subsystem of the robot - * @param tagID the numerical ID of the the tag to follow, null - * for whatever best is - * @param targetDistanceToTag the target distance away from the tag to be - * @param loseTagAfterSeconds how long to wait before giving up on rediscover - * tag, set to null to never finish - */ - public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Translation2d targetDistanceToTag, Integer tagID, - Double loseTagAfterSeconds) { - this(drivetrain, vision, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID, loseTagAfterSeconds); + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Translation2d targetDistanceToTag) { + this(drivetrain, vision, tagID, new Transform2d(targetDistanceToTag, new Rotation2d())); + } + + public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, double targetDistanceToTag) { + this(drivetrain, vision, tagID, new Transform2d(new Translation2d(targetDistanceToTag, 0), new Rotation2d())); } @Override public void initialize() { - secondsSinceTagLastSeen = 0; drivetrain.toDefaultStates(); } @Override public void execute() { + double forwardSpeed = 0; + double rotationSpeed = 0; + double leftSpeed = 0; - final Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID); - - if (distToTag == null) { - secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod; - drivetrain.stop(); - } else { + Transform3d transform = vision.getTransformToTag(tagID); - secondsSinceTagLastSeen = 0; + if (transform != null){ + double forward = transform.getX(); + double left = transform.getY(); + Rotation2d rotation = new Rotation2d(forward, left); - // https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html - // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ() - final Transform2d tagPosition = new Transform2d( - distToTag.getZ(), - distToTag.getX(), - Rotation2d.fromRadians(distToTag.getRotation().getZ())); - - final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse()); + forwardSpeed = -xController.calculate(forward); + leftSpeed = yController.calculate(left); + rotationSpeed = -rotationController.calculate(rotation.getRadians()); + } - drivetrain.setDesiredPosition(drivetrain.getPosition().plus(driveTransform)); + double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED; + double maxSpeed = Math.max(Math.abs(forwardSpeed), Math.abs(leftSpeed)); + if (maxSpeed > speedLimit) { + forwardSpeed = (forwardSpeed / maxSpeed) * speedLimit; + leftSpeed = (leftSpeed / maxSpeed) * speedLimit; } + drivetrain.setDesiredState(new ChassisSpeeds(forwardSpeed, leftSpeed, rotationSpeed)); drivetrain.updateSmartDashboard(); - } + } @Override public boolean isFinished() { - return (loseTagAfterSeconds != null) && (secondsSinceTagLastSeen < loseTagAfterSeconds); + return false; } @Override public void end(boolean interrupted) { - drivetrain.clearDesiredPosition(); drivetrain.stop(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/SetLightstripColor.java b/src/main/java/frc/robot/commands/SetLightstripColor.java index 8a3395c..a80d4f8 100644 --- a/src/main/java/frc/robot/commands/SetLightstripColor.java +++ b/src/main/java/frc/robot/commands/SetLightstripColor.java @@ -3,31 +3,26 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.LightStrip; - public class SetLightstripColor extends Command { private LightStrip lightStrip; - private int r; - private int g; - private int b; + private double pattern; /** * Set the color of the robot's lightstrip + * * @param lightStrip The lightstrip you want to use (there should only be ONE) - * @param r Red level - * @param g Green level - * @param b Blue level + * @param pattern Pattern */ - public SetLightstripColor(LightStrip lightStrip, int r, int g, int b) { + public SetLightstripColor(LightStrip lightStrip, double pattern) { this.lightStrip = lightStrip; - this.r = r; - this.g = g; - this.b = b; + this.pattern = pattern; } @Override public void initialize() { - lightStrip.setColor(r, g, b); + System.out.println("*** Command: Selecting pattern " + pattern); + lightStrip.setPattern(pattern); } @Override diff --git a/src/main/java/frc/robot/commands/deprecated/DriveToPose.java b/src/main/java/frc/robot/commands/deprecated/DriveToPose.java new file mode 100644 index 0000000..04876a9 --- /dev/null +++ b/src/main/java/frc/robot/commands/deprecated/DriveToPose.java @@ -0,0 +1,72 @@ +package frc.robot.commands.deprecated; + +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.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveDrivetrain; + +/** Command to automatically drive to a certain Pose on the field */ +public class DriveToPose extends Command { + private final SwerveDrivetrain drivetrain; + private final Pose2d targetPosition; + + /** + * Create a new DriveToPose command. Uses setDesiredPosition on drivetrain. + * + *

+ * This drives relative to the robot starting position, + * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started, + * where forward is whatever direction the robot started in + * + *

+ * The last place the drivetrain position was reset counts as the starting position + * + * @param drivetrain the drivetrain of the robot + * @param position Target pose the robot is trying to drive to + */ + public DriveToPose(SwerveDrivetrain drivetrain, Pose2d position) { + this.drivetrain = drivetrain; + this.targetPosition = position; + + addRequirements(drivetrain); + } + + /** + *

+ * This drives relative to the robot starting position, + * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started, + * where forward is whatever direction the robot started in + * + *

+ * The last place the drivetrain position was reset counts as the starting position + * + * @param drivetrain The drivetrain of the robot + * @param translation Target transform to drive + * @param rotation Target rotation to drive + */ + public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) { + this(drivetrain, new Pose2d(translation, rotation)); + } + + @Override + public void initialize() { + drivetrain.setDesiredPosition(targetPosition); + } + + @Override + public void execute() { + drivetrain.updateSmartDashboard(); + } + + @Override + public boolean isFinished() { + return drivetrain.isAtDesiredPosition(); + } + + @Override + public void end(boolean interrupted) { + drivetrain.clearDesiredPosition(); + drivetrain.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/deprecated/DriveTransform.java b/src/main/java/frc/robot/commands/deprecated/DriveTransform.java new file mode 100644 index 0000000..523b680 --- /dev/null +++ b/src/main/java/frc/robot/commands/deprecated/DriveTransform.java @@ -0,0 +1,68 @@ +package frc.robot.commands.deprecated; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveDrivetrain; + +/** Command to automatically drive a certain transform */ +public class DriveTransform extends Command { + private final SwerveDrivetrain drivetrain; + private final Transform2d transform; + + /** + * Create a new DriveTransform command. Tries to drive a certain transform using the DriveToPose command. + * + *

This drives relative to the robot starting position, + * so a transform of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started, + * where forward is whatever direction the robot is currently facing

+ * + * @param drivetrain the drivetrain of the robot + * @param transform target transform to drive to, will be added to current + * position to get target pose + */ + public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) { + this.drivetrain = drivetrain; + this.transform = transform; + + addRequirements(drivetrain); + } + + /** + * Create a new DriveToTransform command. Uses setDesiredPosition on drivetrain. + * + *

+ * This drives relative to the robot, so a transform of +2x and +1y will drive 2 + * meters forward and 1 meter left, + * where forward is whatever direction the robot is currently facing + * + * @param drivetrain The drivetrain of the robot + * @param translation Target transform to drive + * @param rotation Target rotation to drive + */ + public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) { + this(drivetrain, new Transform2d(translation, rotation)); + } + + @Override + public void initialize() { + drivetrain.setDesiredPosition(drivetrain.getPosition().plus(transform)); + } + + @Override + public void execute() { + drivetrain.updateSmartDashboard(); + } + + @Override + public boolean isFinished() { + return drivetrain.isAtDesiredPosition(); + } + + @Override + public void end(boolean interrupted) { + drivetrain.clearDesiredPosition(); + drivetrain.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java index bac5a85..6b0e0c4 100644 --- a/src/main/java/frc/robot/subsystems/LightStrip.java +++ b/src/main/java/frc/robot/subsystems/LightStrip.java @@ -1,34 +1,51 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.LightConstants; public class LightStrip extends SubsystemBase { - private AddressableLED ledStrip; - private AddressableLEDBuffer ledBuffer; + /** + * The LED controller PWM signal from 1000-2000us, which is identical to a Spark + * servo. So, we use the Spark class. + * -1 corresponds to 1000us + * 0 corresponds to 1500us + * +1 corresponds to 2000us + */ + private Spark ledStrip; + private double pattern; - public LightStrip(AddressableLED ledstrip) { + /** + * Creates a new LightStrip subsystem. + * + * @param ledstrip The 'spark' representing the Blinkin + */ + public LightStrip(Spark ledstrip) { ledStrip = ledstrip; - ledBuffer = new AddressableLEDBuffer(LightConstants.LED_QUANTITY); - - // Docs say this is an expensive operation so future maintainers should avoid - // modifying this excessibely - ledStrip.setLength(ledBuffer.getLength()); - ledStrip.setData(ledBuffer); + } - ledStrip.start(); + /** + * Creates a new LightStrip subsystem. + * + * @param pwmPort The port number for the blinkin + */ + public LightStrip(int pwmPort) { + ledStrip = new Spark(pwmPort); } - public void setColor(int r, int g, int b) { - for (int i = 0; i < ledBuffer.getLength(); i++) { - ledBuffer.setRGB(i, r, g, b); - } + /** + * Sets the blinkin to a pattern + * + * @param pattern Pattern ID to use. Consult section 5 of the blinkin manual. + * @link https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf + */ + public void setPattern(double pattern) { + this.pattern = pattern; } @Override public void periodic() { - ledStrip.setData(ledBuffer); + // System.out.println("*** Subsystem setting pattern to pattern number " + + // pattern); + ledStrip.set(pattern); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 8055869..523f2d6 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics.SwerveDriveWheelStates; import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -42,7 +43,7 @@ public class SwerveDrivetrain extends SubsystemBase { * * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html */ - private final SwerveDriveOdometry odometry; + private final SwerveDriveOdometry poseOdometry; /** Swerve module */ private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR; @@ -62,7 +63,7 @@ public class SwerveDrivetrain extends SubsystemBase { * * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose */ - private Pose2d pose; + private Pose2d pose = new Pose2d(); /** * Desired pose of robot. The desired pose is the X, Y and Rotation the robot wants to be in, relative to the last reset. @@ -84,7 +85,8 @@ public class SwerveDrivetrain extends SubsystemBase { * @param swerveModuleBL Back left swerve module * @param swerveModuleBR Back right swerve module */ - public SwerveDrivetrain(AHRS gyro, + public SwerveDrivetrain( + AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR, SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) { @@ -102,12 +104,13 @@ public SwerveDrivetrain(AHRS gyro, kinematics = new SwerveDriveKinematics( modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new)); - // create starting state for odometry - odometry = new SwerveDriveOdometry( - kinematics, - getHeading(), - modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)); - + // Create pose estimator, like odometry but cooler + poseOdometry = new SwerveDriveOdometry( + kinematics, + getHeading(), + getWheelPositions().positions, + pose); + // set up name and children for sendable registry setName(toString()); for (SwerveModule module : modules) { @@ -130,10 +133,16 @@ public SwerveDrivetrain(AHRS gyro, /** This is the periodic function of the swerve drivetrain, called periodically by the CommandScheduler, about every 20ms. */ @Override public void periodic() { - // use odometry to update the estimated pose - pose = odometry.update( - getHeading(), - getWheelPositions()); + pose = poseOdometry.update( + getHeading(), + getWheelPositions()); + + // if (visionSystem != null) { + // Optional estimatedPose = visionSystem.getEstimatedGlobalPose(); + // if (estimatedPose.isPresent()) { + // poseEstimator.addVisionMeasurement(estimatedPose.get().estimatedPose.toPose2d(), estimatedPose.get().timestampSeconds); + // } + // } if (desiredPose != null) { // Calculate our robot speeds with the PID controllers @@ -161,7 +170,7 @@ public Pose2d getPosition() { * Basically zeros out position. */ public void resetPosition() { - odometry.resetPosition( + poseOdometry.resetPosition( getHeading(), getWheelPositions(), pose); @@ -228,8 +237,7 @@ public void brakeMode() { */ public ChassisSpeeds getState() { // get all module states and convert them into chassis speeds - return kinematics.toChassisSpeeds( - modulesMap(SwerveModule::getState, SwerveModuleState[]::new)); + return kinematics.toChassisSpeeds(getWheelStates()); } /** @@ -319,9 +327,22 @@ public SwerveDriveWheelPositions getWheelPositions() { modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)); } + /** + * Get all the swerve module states + * + * @return a {@link SwerveDriveWheelPositions} object which contains an array of + * all swerve module positions + */ + public SwerveDriveWheelStates getWheelStates() { + return new SwerveDriveWheelStates( + modulesMap(SwerveModule::getState, SwerveModuleState[]::new)); + } + // --- Gyro methods --- /** + * Gyro Method + * *

* Return the heading of the robot as a rotation in a 2D coordinate frame * represented by a point on the unit circle (cosine and sine). @@ -346,6 +367,8 @@ public Rotation2d getHeading() { } /** + * Gyro Method + * *

* Return the heading of the robot in as a rotation in a 3D coordinate frame * represented by a quaternion. diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 58ffec8..74277e0 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -40,7 +40,7 @@ public Vision(String cameraName, Transform3d robotToCamera) { * center of the robot. Returns null if no tag found. * */ - public Transform3d getDistToTag() { + public Transform3d getTransformToTag() { PhotonTrackedTarget target = camera.getLatestResult().getBestTarget(); if (target == null) { return null; @@ -55,7 +55,9 @@ public Transform3d getDistToTag() { * @return the position of the tag (translation and rotation) based on the * center of the robot. Returns null if no tag found */ - public Transform3d getDistToTag(int tagID) { + public Transform3d getTransformToTag(int tagID) { + if (tagID == -1) return getTransformToTag(); + for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { if (target.getFiducialId() == tagID) return target.getBestCameraToTarget().plus(cameraToRobot); 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..c1573d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -0,0 +1,34 @@ +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 e49ac11..0000000 --- a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java +++ /dev/null @@ -1,21 +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 changeArmAngleDegreesBy(double desiredDegrees); - - 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 6989509..c8e1020 100644 --- a/src/main/java/frc/robot/subsystems/arm/DummyArm.java +++ b/src/main/java/frc/robot/subsystems/arm/DummyArm.java @@ -1,27 +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 changeArmAngleDegreesBy(double desiredDegrees) { - System.out.println(desiredDegrees); - } +public class DummyArm extends Arm { - public void setArmToAmpPosition() { - } + private Rotation2d armPosition = new Rotation2d(); - public void setArmToSpeakerPosition() { - } + @Override + public void setSetpoint(double degrees) { + SmartDashboard.putNumber("Arm SP Deg", degrees); + SmartDashboard.putNumber("Arm Deg", 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 a3f50c8..30dbd0b 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,37 +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 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 setSetpoint(double degrees) { + degrees = Math.max(degrees, ArmConstants.MINIMUM_ARM_DEGREES); + degrees = Math.min(degrees, ArmConstants.MAXIMUM_ARM_DEGREES); - public void setArmToAmpPosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES); - } + SmartDashboard.putNumber("Arm SP Deg", degrees); - public void setArmToSpeakerPosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); + armRaisePIDController.setSetpoint(Units.degreesToRotations(degrees)); } - public void setArmToIntakePosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES); - } - - public void setSetpoint(double degree) { - armSetpoint = Rotation2d.fromDegrees(degree); + public Rotation2d getArmPosition() { + return Rotation2d.fromRotations(armPosition.refresh().getValueAsDouble()); } public boolean isAtDesiredPosition() { @@ -97,17 +73,13 @@ 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 Deg", currentOnPosition.getDegrees()); } } diff --git a/src/main/java/frc/robot/subsystems/intake/DummyShooter.java b/src/main/java/frc/robot/subsystems/intake/DummyShooter.java new file mode 100644 index 0000000..5b627ca --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/DummyShooter.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.intake; + +public class DummyShooter extends IntakeShooter { + public void setFlyWheelSpeed(double speed) { + }; + + public void startFlyWheels() { + } + + public void stopFlywheels() { + } + + public void setIntakeSpeed(double speed) { + } + + public void intake() { + } + + public void intakeReverse() { + } + + public void stopIntake() { + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java b/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java new file mode 100644 index 0000000..e4dcd22 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeShooter.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class IntakeShooter extends SubsystemBase { + public void setFlyWheelSpeed(double speed) { + }; + + public void startFlyWheels() { + } + + public void stopFlywheels() { + } + + public void setIntakeSpeed(double speed) { + } + + public void intake() { + } + + public void intakeReverse() { + } + + public void stopIntake() { + } + +} diff --git a/src/main/java/frc/robot/subsystems/IntakeShooter.java b/src/main/java/frc/robot/subsystems/intake/RealShooter.java similarity index 73% rename from src/main/java/frc/robot/subsystems/IntakeShooter.java rename to src/main/java/frc/robot/subsystems/intake/RealShooter.java index 2bbe692..8b9bde7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeShooter.java +++ b/src/main/java/frc/robot/subsystems/intake/RealShooter.java @@ -1,9 +1,8 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.intake; 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; /** @@ -17,7 +16,7 @@ * * @author Aceius E. */ -public class IntakeShooter extends SubsystemBase { +public class RealShooter extends IntakeShooter { /* * 2 motors for flywheel * 2 for intake @@ -29,13 +28,13 @@ public class IntakeShooter extends SubsystemBase { private final CANSparkMax intake1; private final CANSparkMax intake2; // might not exist - public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) { + public RealShooter(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); @@ -43,29 +42,34 @@ public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake intake2.setInverted(IntakeShooterConstants.INTAKE_REVERSE); } + public void setFlyWheelSpeed(double speed) { + flywheel1.set(speed); + flywheel2.set(speed); + } + public void startFlyWheels() { - flywheel1.set(1); - flywheel2.set(1); + setFlyWheelSpeed(1); } - + public void stopFlywheels() { - flywheel1.stopMotor(); - flywheel2.stopMotor(); + setFlyWheelSpeed(0); + } + + public void setIntakeSpeed(double speed) { + intake1.set(speed); + intake2.set(speed); } - + public void intake() { - intake1.set(1); - intake1.set(1); + setIntakeSpeed(1); } public void intakeReverse() { - intake1.set(-1); - intake1.set(-1); + setIntakeSpeed(-1); } public void stopIntake() { - intake1.stopMotor(); - intake1.stopMotor(); + setIntakeSpeed(0); } }