diff --git a/.vscode/settings.json b/.vscode/settings.json index c0fc28e..ccc8f02 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,37 +1,39 @@ { - "java.configuration.updateBuildConfiguration": "automatic", - "java.server.launchMode": "Standard", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - ], - "java.test.defaultConfig": "WPIlibUnitTests", - "cSpell.words": [ - "AHRS", - "Deadzone", - "Odometry", - "setpoint", - "velociticities" - ] -} - + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + // Auto Formatter + "editor.formatOnSave": true, // So you cant forget + "editor.tabSize": 4, // 4 char width indentation + "editor.detectIndentation": false, // Prevent above from being overridden + "editor.insertSpaces": false, // use tab character instead of spaces + "editor.codeActionsOnSave": { + "source.fixAll": "always" + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 27cccca..8126c23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,43 +66,43 @@ public static class DriverConstants { public static final double[] maxSpeedOptionsRotation = { 0.25, 0.75, 1 }; } - public static class ArmConstants { + public static class ArmConstants { static { switch (currentBot) { case WOOD_BOT: - HAR_ARM = false; + HAS_ARM = false; break; case COMP_BOT: default: - HAR_ARM = true; + HAS_ARM = true; break; } } - public static final boolean HAR_ARM; + public static final boolean HAS_ARM; - public static final double MAXIMUM_ARM_DEGREES = 1; - public static final double MINIMUM_ARM_DEGREES = 0; + public static final double MAXIMUM_ARM_DEGREES = 1; + public static final double MINIMUM_ARM_DEGREES = 0; - public static final double ARM_AMP_SHOOTING_DEGREES = -20; - public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45; - public static final double ARM_INTAKE_DEGREES = -40; + public static final double ARM_AMP_SHOOTING_DEGREES = -20; + public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45; + 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_ENCODER_ID = 6; + 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_ENCODER_ID = 6; public static final boolean ARE_MOTORS_REVERSED = false; - public static final double DEGREES_PER_SECOND = 2.0; + public static final double DEGREES_PER_SECOND = 2.0; 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 RobotMovementConstants { public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001); @@ -169,7 +169,7 @@ public static class SwerveModuleConstants { VELOCITY_MOTOR_ID_BL = 8; ANGULAR_MOTOR_ID_BL = 9; ANGULAR_MOTOR_ENCODER_ID_BL = 2; - ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 +0.5; + ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 + 0.5; // Back right VELOCITY_MOTOR_ID_BR = 10; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4c507e5..fb6232f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,6 @@ import frc.robot.Constants.SwerveDrivetrainConstants; import frc.robot.Constants.SwerveModuleConstants; import frc.robot.Constants.ArmConstants; -import frc.robot.subsystems.Arm; import frc.robot.commands.Autos; import frc.robot.commands.AimAtTag; import frc.robot.commands.ArmRotateTo; @@ -13,6 +12,9 @@ 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.DummyArm; +import frc.robot.subsystems.arm.RealArm; import frc.robot.inputs.ChassisDriveInputs; import com.kauailabs.navx.frc.AHRS; @@ -69,12 +71,17 @@ public class RobotContainer { -SwerveDrivetrainConstants.MODULE_LOCATION_Y)); private final AHRS gyro = new AHRS(); - // private final Arm arm = new Arm( - // ArmConstants.LEFT_MOTOR_ID, - // ArmConstants.RIGHT_MOTOR_ID, - // ArmConstants.RIGHT_ENCODER_ID, - // ArmConstants.ARE_MOTORS_REVERSED); + /** + * This is the robot arm. In some situations, the robot may not have an arm, so + * 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( + ArmConstants.LEFT_MOTOR_ID, + ArmConstants.RIGHT_MOTOR_ID, + ArmConstants.RIGHT_ENCODER_ID, + ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm(); private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR, swerveModuleBL, swerveModuleBR); @@ -83,10 +90,9 @@ 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 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); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -134,10 +140,14 @@ public void setUpDriveController() { joystick.button(1).onTrue(Commands.run(inputs::speedUp)); joystick.button(3).onTrue(Commands.run(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); + // 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)); } else { final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); diff --git a/src/main/java/frc/robot/commands/ArmRemoteControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java index bb30a02..5384f00 100644 --- a/src/main/java/frc/robot/commands/ArmRemoteControl.java +++ b/src/main/java/frc/robot/commands/ArmRemoteControl.java @@ -1,40 +1,36 @@ package frc.robot.commands; import frc.robot.Constants.ArmConstants; -import frc.robot.subsystems.Arm; import frc.robot.inputs.OptionButtonInput; +import frc.robot.subsystems.arm.ArmInterface; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; - public class ArmRemoteControl extends Command { - private final Arm arm; + private final ArmInterface arm; private final OptionButtonInput raiseArmButton; private final OptionButtonInput lowerArmButton; - private final OptionButtonInput speakerPositionButton; - private final OptionButtonInput ampPositionButton; - private final OptionButtonInput intakePositionButton; - - - public ArmRemoteControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton, - OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, OptionButtonInput intakePositionButton) { - this.arm = arm; + private final OptionButtonInput speakerPositionButton; + private final OptionButtonInput ampPositionButton; + private final OptionButtonInput intakePositionButton; - this.raiseArmButton = raiseArmButton; - this.lowerArmButton = lowerArmButton; + public ArmRemoteControl(ArmInterface arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton, + OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, + OptionButtonInput intakePositionButton) { + this.arm = arm; - this.speakerPositionButton = speakerPositionButton; - this.ampPositionButton = ampPositionButton; - this.intakePositionButton = intakePositionButton; + this.raiseArmButton = raiseArmButton; + this.lowerArmButton = lowerArmButton; + this.speakerPositionButton = speakerPositionButton; + this.ampPositionButton = ampPositionButton; + this.intakePositionButton = intakePositionButton; addRequirements(arm); } - - @Override public void initialize() { } @@ -46,24 +42,26 @@ public void initialize() { @Override public void execute() { - // Arm Motor - arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND); - arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND); - - // Arm Povs - if (speakerPositionButton.getState()) { - arm.setArmToSpeakerPosition(); - } - if (ampPositionButton.getState()) { - arm.setArmToAmpPosition(); - } - if (intakePositionButton.getState()) { - arm.setArmToIntakePosition(); - } + // Arm Motor + arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod + * ArmConstants.DEGREES_PER_SECOND); + arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod + * ArmConstants.DEGREES_PER_SECOND); + + // Arm Povs + if (speakerPositionButton.getState()) { + arm.setArmToSpeakerPosition(); + } + if (ampPositionButton.getState()) { + arm.setArmToAmpPosition(); + } + if (intakePositionButton.getState()) { + arm.setArmToIntakePosition(); + } } - @Override + @Override public boolean isFinished() { return false; } diff --git a/src/main/java/frc/robot/commands/ArmRotateTo.java b/src/main/java/frc/robot/commands/ArmRotateTo.java index 0f7c524..6927824 100644 --- a/src/main/java/frc/robot/commands/ArmRotateTo.java +++ b/src/main/java/frc/robot/commands/ArmRotateTo.java @@ -1,28 +1,28 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Arm; +import frc.robot.subsystems.arm.ArmInterface; -public class ArmRotateTo extends Command{ +public class ArmRotateTo extends Command { - private final double setpoint; - private final Arm arm; + private final double setpoint; + private final ArmInterface arm; - public ArmRotateTo(Arm arm, double degree){ - this.setpoint = degree; - this.arm = arm; + public ArmRotateTo(ArmInterface arm, double degree) { + this.setpoint = degree; + this.arm = arm; - addRequirements(arm); + addRequirements(arm); - } + } - @Override - public void initialize() { - arm.setSetpoint(setpoint); - } + @Override + public void initialize() { + arm.setSetpoint(setpoint); + } - @Override - public boolean isFinished() { - return arm.isAtDesiredPosition(); - } + @Override + public boolean isFinished() { + return arm.isAtDesiredPosition(); + } } diff --git a/src/main/java/frc/robot/commands/AutoPosition.java b/src/main/java/frc/robot/commands/AutoPosition.java new file mode 100644 index 0000000..46ced32 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoPosition.java @@ -0,0 +1,89 @@ +package frc.robot.commands; + +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +// How to make Command: https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html (ignore image instructions, code is out of date, just look at written general instructions) +// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html +// Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html + +/** An example command that uses an example subsystem. */ +public class AutoPosition extends Command { + private final ExampleSubsystem subsystem; + + /** + * The constructor creates a new command and is automatically called one time + * when the command is created (with 'new' keyword). + * It should set up the initial state and properties of the object to ensure + * it's ready for use. + * This can take in any arguments you need. It normally uses 1 subsystem (but an + * take multiple when necessary), + * as wells as arguments for what to do, such as a joystick in the drive command + * or a desired position in an auto command. + * Example uses include saving parameters passed to the command, creating and + * configuring objects for the class like PID controllers, and adding subsystem + * requirements + */ + public AutoPosition(ExampleSubsystem subsystem) { + // use "this" to access member variable subsystem rather than local subsystem + this.subsystem = subsystem; + + // Use addRequirements() here to declare subsystem dependencies. + // This makes sure no other commands try to do stuff with your subsystem while + // you are using it. + addRequirements(this.subsystem); + } + + /** + * initialize() is used to prepare a command for execution and is called once + * when the command is scheduled. + * It should reset the command's state since command objects can be used + * multiple times. + * Example uses include setting motor to constant speed, setting a solenoid to a + * certain state, and resetting variables + */ + @Override + public void initialize() { + } + + /** + * execute() is called repeatedly while a command is scheduled, about every + * 20ms. + * It should handle continuous tasks specific to the command, like updating + * motor outputs based on joystick inputs or utilizing control loop results. + * Example uses include adjusting motor speeds for real-time control, processing + * sensor data within a scheduled command, and using the output of a control + * loop. + */ + @Override + public void execute() { + } + + /** + * isFinished() finished is called repeatedly while a command is scheduled, + * right after execute. + * It should return true when you want the command to finish. end(false) is + * called directly after isFinished() returns true. + * Example uses include checking if control loop is at set point, and always + * returning false to end after just 1 call to execute. + */ + @Override + public boolean isFinished() { + return true; + } + + /** + * end(boolean interrupted) is called once when a command ends, regardless of + * whether it finishes normally or is interrupted. + * It should wrap up the command since other commands might use the same + * subsystems. + * Once end runs the command will no longer be in the command scheduler loop. + * It takes in a boolean interrupted which is set to true when the command is + * ended without isFinished() returning true. + * Example uses include setting motor speeds back to zero, and setting a + * solenoid back to a "default" state. + */ + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 446e6b1..8dbe1ca 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -2,12 +2,12 @@ import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.Vision; +import frc.robot.subsystems.arm.ArmInterface; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants.SwerveDrivetrainConstants; -import frc.robot.subsystems.Arm; /** * This class just contains a bunch of auto-modes. Do not call this class @@ -58,7 +58,7 @@ public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera, } /** Linden did this */ - public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) { + public static Command startingAuto(ArmInterface arm, SwerveDrivetrain drivetrain, boolean invertY) { // assumes start position in corner double invert = 1; diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java deleted file mode 100644 index ecb55a3..0000000 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ /dev/null @@ -1,111 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ArmConstants; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.hardware.CANcoder; - -public class Arm extends SubsystemBase { - - 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 Arm(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); - - leftArmMotor.setInverted(areMotorsReversed); - rightArmMotor.setInverted(!areMotorsReversed); - - } - - // 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); - } - - public void setArmToSpeakerPosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); - } - - public void setArmToIntakePosition() { - armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES); - } - - public void setSetpoint(double degree){ - armSetpoint = Rotation2d.fromDegrees(degree); - } - - public boolean isAtDesiredPosition(){ - return armRaisePIDController.atSetpoint(); - } - - /** - * This method is called periodically by the CommandScheduler, about every 20ms. - */ - @Override - public void periodic() { - // This method will be called once per scheduler run - - - double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armSetpoint.getRotations()); - - SmartDashboard.putNumber("Arm Speed", armSpeed); - - leftArmMotor.set(armSpeed); - rightArmMotor.set(armSpeed); - - SmartDashboard.putNumber("Arm Degrees", Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees()); - SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees()); - - - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java new file mode 100644 index 0000000..e49ac11 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java @@ -0,0 +1,21 @@ +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 new file mode 100644 index 0000000..6989509 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/DummyArm.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * 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 void setArmToAmpPosition() { + } + + public void setArmToSpeakerPosition() { + } + + public void setArmToIntakePosition() { + } + + public void setSetpoint(double degree) { + System.out.println(degree); + } + + public boolean isAtDesiredPosition() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/arm/RealArm.java b/src/main/java/frc/robot/subsystems/arm/RealArm.java new file mode 100644 index 0000000..a3f50c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/arm/RealArm.java @@ -0,0 +1,113 @@ +package frc.robot.subsystems.arm; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ArmConstants; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; + +/** This is the subsystem that represents the arm. */ +public class RealArm extends SubsystemBase implements ArmInterface { + + 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); + + leftArmMotor.setInverted(areMotorsReversed); + rightArmMotor.setInverted(!areMotorsReversed); + + } + + // 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); + } + + public void setArmToSpeakerPosition() { + armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES); + } + + public void setArmToIntakePosition() { + armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES); + } + + public void setSetpoint(double degree) { + armSetpoint = Rotation2d.fromDegrees(degree); + } + + public boolean isAtDesiredPosition() { + return armRaisePIDController.atSetpoint(); + } + + /** + * This method is called periodically by the CommandScheduler, about every 20ms. + */ + @Override + public void periodic() { + // This method will be called once per scheduler run + + double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(), + armSetpoint.getRotations()); + + SmartDashboard.putNumber("Arm Speed", armSpeed); + + leftArmMotor.set(armSpeed); + rightArmMotor.set(armSpeed); + + SmartDashboard.putNumber("Arm Degrees", + Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees()); + SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees()); + + } +}