From 3af8400bfe2827e7e2482d17fff47226ed4aa0f3 Mon Sep 17 00:00:00 2001 From: Aceius E Date: Mon, 26 Feb 2024 15:46:06 -0800 Subject: [PATCH 1/5] Re-add thing aleah accidently deleted --- .vscode/settings.json | 72 +++++++++++++++++++++---------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index c0fc28e..0c4ad71 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,37 +1,37 @@ { - "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", + // Style guide - most of this came from clicking buttons and then copying from my user settings.json + // General editor stuff + "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 +} \ No newline at end of file From 250fb5ee1658af61f0ee369978c83a0698cad295 Mon Sep 17 00:00:00 2001 From: Aceius E Date: Mon, 26 Feb 2024 15:52:58 -0800 Subject: [PATCH 2/5] Always fix everything --- .vscode/settings.json | 8 ++- src/main/java/frc/robot/commands/FaceTag.java | 55 ++++++++++--------- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 0c4ad71..ccc8f02 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -28,10 +28,12 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - // Style guide - most of this came from clicking buttons and then copying from my user settings.json - // General editor stuff + // 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.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/commands/FaceTag.java b/src/main/java/frc/robot/commands/FaceTag.java index e493410..24e5e3f 100644 --- a/src/main/java/frc/robot/commands/FaceTag.java +++ b/src/main/java/frc/robot/commands/FaceTag.java @@ -1,4 +1,5 @@ package frc.robot.commands; + import java.util.Map; import edu.wpi.first.math.controller.PIDController; @@ -8,30 +9,34 @@ import frc.robot.subsystems.Vision; public class FaceTag extends Command { - private final Vision vision; - private final SwerveDrivetrain drivetrain; - private final PIDController rotationPID; - - public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map targetTagPos){ - this.vision = vision; - this.drivetrain = drivetrain; - rotationPID = new PIDController( - RobotMovementConstants.ROTATION_PID_P, - RobotMovementConstants.ROTATION_PID_I, - RobotMovementConstants.ROTATION_PID_D); - rotationPID.enableContinuousInput(-Math.PI, Math.PI); - rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS); - - addRequirements(drivetrain); - } - - @Override - public void initialize(){ - drivetrain.toDefaultStates(); - } - - @Override - public void execute(){ - } + private final Vision vision; + private final SwerveDrivetrain drivetrain; + private final PIDController rotationPID; + + public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map targetTagPos) { + this.vision = vision; + this.drivetrain = drivetrain; + rotationPID = new PIDController( + RobotMovementConstants.ROTATION_PID_P, + RobotMovementConstants.ROTATION_PID_I, + RobotMovementConstants.ROTATION_PID_D); + rotationPID.enableContinuousInput(-Math.PI, Math.PI); + rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS); + + addRequirements(drivetrain); + } + + @Override + public void initialize() { + drivetrain.toDefaultStates(); + } + + public void a(String amog) { + return; + } + + @Override + public void execute() { + } } From a4d5f8143620eae9818651c8154dc07490eab8f5 Mon Sep 17 00:00:00 2001 From: Aceius E <144858100+AceiusRedshift@users.noreply.github.com> Date: Mon, 26 Feb 2024 16:45:15 -0800 Subject: [PATCH 3/5] Convert Arm class to interface (#3) * Arm as interface * Fix type error relating to arm interface --- src/main/java/frc/robot/Constants.java | 32 ++--- src/main/java/frc/robot/RobotContainer.java | 23 ++-- .../frc/robot/commands/ArmRemoteControl.java | 64 +++++----- .../java/frc/robot/commands/ArmRotateTo.java | 34 +++--- src/main/java/frc/robot/commands/Autos.java | 4 +- src/main/java/frc/robot/subsystems/Arm.java | 111 ----------------- .../robot/subsystems/arm/ArmInterface.java | 21 ++++ .../frc/robot/subsystems/arm/DummyArm.java | 30 +++++ .../frc/robot/subsystems/arm/RealArm.java | 113 ++++++++++++++++++ 9 files changed, 245 insertions(+), 187 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/Arm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmInterface.java create mode 100644 src/main/java/frc/robot/subsystems/arm/DummyArm.java create mode 100644 src/main/java/frc/robot/subsystems/arm/RealArm.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed106ab..395e686 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,43 +64,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); @@ -167,7 +167,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 126243b..47100e2 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.ArmRotateTo; import frc.robot.commands.ChassisRemoteControl; @@ -12,6 +11,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 frc.robot.inputs.OptionButtonInput; import frc.robot.inputs.OptionButtonInput.ActivationMode; @@ -70,12 +72,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); @@ -84,7 +91,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); @@ -137,7 +143,8 @@ public void setUpDriveController() { boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD); fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE); - //This bypasses arm remote control, arm remote control is incompatible with autonomous commands + // 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); 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/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 2aee1e2..57eaeef 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 @@ -50,7 +50,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()); + + } +} From 6649b3be86063cf213e1ad51c5411905e3d2fce9 Mon Sep 17 00:00:00 2001 From: Aleah H Date: Mon, 26 Feb 2024 17:30:09 -0800 Subject: [PATCH 4/5] Fix Xbox Joystick being inversed --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 47100e2..c97d2ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,8 +155,8 @@ public void setUpDriveController() { final CommandXboxController xbox = new CommandXboxController(genericHID.getPort()); inputs = new ChassisDriveInputs( - xbox::getLeftY, +1, - xbox::getLeftX, +1, + xbox::getLeftX, -1, + xbox::getLeftY, -1, xbox::getRightX, -1, Constants.DriverConstants.DEAD_ZONE); From 4ca5a7ce7c97bb5f158016767fa4f3c7ad414991 Mon Sep 17 00:00:00 2001 From: Linden <50274080+liulinden@users.noreply.github.com> Date: Mon, 26 Feb 2024 17:35:59 -0800 Subject: [PATCH 5/5] created autoposition file --- .../java/frc/robot/commands/AutoPosition.java | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutoPosition.java 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) { + } +}