From 44660a3ba5bae3d60dea92232e639a0e24bd60cb Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 31 Jan 2024 12:20:03 -0800 Subject: [PATCH 01/25] reimplement single module swerve subsystem --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 481cdc2b..29be766f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,7 +136,7 @@ private void configureBindings() { final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - // swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); } , swerveSubsystem)); @@ -168,16 +168,14 @@ private void configureBindings() { } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; - // System.out.println("1"); - - // swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - // swerveSubsystem.setDrivePowers(controller.getLeftX(), -controller.getLeftY());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - // } - // , swerveSubsystem)); + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + } + , swerveSubsystem)); - // AButton.onTrue(new InstantCommand(() -> { - // swerveSubsystem.toggletoRun(); - // })); + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + swerveSubsystem.toggletoRun(); + })); } From a354be73e2dcaed3ca0013f30b3c9479c34c46a2 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 31 Jan 2024 14:03:57 -0800 Subject: [PATCH 02/25] renaming and reformating intake, adding ids --- .vscode/settings.json | 3 +- src/main/java/frc/robot/Constants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 67 +++++++++---------- .../pivot/IntakePivotExtendedCommand.java | 2 +- .../pivot/IntakePivotVerticalCommand.java | 2 +- .../roller/IntakeRollerFeedCommand.java | 6 +- .../roller/IntakeRollerIntakeCommand.java | 4 +- .../roller/IntakeRollerOutakeCommand.java | 6 +- .../intake/IntakePivotSubsystem.java | 10 +-- .../intake/IntakeRollersSubsystem.java | 32 ++++----- 10 files changed, 70 insertions(+), 72 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 4ed293b7..565b0808 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,6 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3cf12d98..99fccf14 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,11 +66,11 @@ public static class SwerveConstants { public static final Translation2d BR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST); } - public static class RollerandPivotConstants { - public static final int lastmotorID = 0; - public static final int topmotorID = 2; - public static final int bottommotorID = 3; - public static final int motor1ID = 1; + public static class IntakeConstants { + public static final int INTEGRATION_MOTOR_ID = 19; + public static final int FRONT_MOTOR_ID = 17; + public static final int BACK_MOTOR_ID = 18; + public static final int PIVOT_MOTOR_ID = 16; public static final int sensorID = 0; public static final int extendedlimitswitchID = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29be766f..a9fa18dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,13 +37,14 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final IntakeRollersSubsystem intakeSubsystem = new IntakeRollersSubsystem(); private final BaseDriveController driveController = new DualJoystickDriveController(); private final BaseSwerveSubsystem baseSwerveSubsystem; private final IntakePivotSubsystem intakePivotSubsystem; - private final ShooterFlywheelSubsystem shooterSubsystem; - private final ShooterFeederSubsystem feederSubsystem; + private final IntakeRollersSubsystem intakeRollerSubsystem = new IntakeRollersSubsystem(); + + private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; + private final ShooterFeederSubsystem shooterFeederSubsystem; private final ShooterPivotSubsystem shooterPivotSubsystem; private final ClimbSubsystem climbSubsystem; @@ -56,6 +57,7 @@ public class RobotContainer { private final JoystickButton rightBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value); private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value); + //private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); ChoreoTrajectory traj; @@ -68,10 +70,10 @@ public RobotContainer() { // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); baseSwerveSubsystem = new SwerveSubsystem(); intakePivotSubsystem = new IntakePivotSubsystem(); - feederSubsystem = new ShooterFeederSubsystem(); + shooterFeederSubsystem = new ShooterFeederSubsystem(); shooterPivotSubsystem = new ShooterPivotSubsystem(false); - shooterSubsystem = new ShooterFlywheelSubsystem(); + shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); climbSubsystem = new ClimbSubsystem(); @@ -84,47 +86,21 @@ public RobotContainer() { private void configureBindings() { - /** - * Right trigger - run pivot forward at speed pulled - * Left trigger - run pivot backward at speed pulled - * A button - run flywheels while button held - * B button - run feed wheels while button held - */ - - // aButton.onTrue(new LoadNoteCommand(feederSubsystem)); - - // bButton.onTrue(new ShootNoteCommand(feederSubsystem)); - - // leftBumper.onTrue(new AutoAimCommand(pivotSubsystem)); - - // rightBumper.onTrue(new VerticalCommand(pivotSubsystem)); - - // xButton.onTrue(new ReadyShooterCommand(shooterSubsystem)); - - // yButton.onTrue(new StopShooterCommands(shooterSubsystem)); - - intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - intakePivotSubsystem.setPivotSpeed(-mechController.getLeftTriggerAxis()); - })); - - shooterSubsystem.setDefaultCommand(new InstantCommand(() -> { - intakePivotSubsystem.setPivotSpeed(mechController.getRightTriggerAxis()); - })); bButton.onTrue(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(.7); + shooterFeederSubsystem.setFeederMotorSpeed(.7); })); bButton.onFalse(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(0); + shooterFeederSubsystem.setFeederMotorSpeed(0); })); xButton.onTrue(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(-.7); + shooterFeederSubsystem.setFeederMotorSpeed(-.7); })); xButton.onFalse(new InstantCommand(() -> { - feederSubsystem.setFeederMotorSpeed(0); + shooterFeederSubsystem.setFeederMotorSpeed(0); })); shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { @@ -132,6 +108,27 @@ private void configureBindings() { // pivotSubsystem.printCurrentAngle(); }, shooterPivotSubsystem)); + aButton.onTrue(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); + })); + + aButton.onFalse(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(0); + })); + + intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { + if(mechController.getPOV() == 90){ + intakeRollerSubsystem.setAllRollSpeed(.3, .3); + } else if (mechController.getPOV() == 270){ + intakeRollerSubsystem.setAllRollSpeed(-.3, -.3); + } else { + intakeRollerSubsystem.setAllRollSpeed(0, 0); + } + })); + + + + if(baseSwerveSubsystem instanceof SwerveSubsystem){ final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java index 02665c44..c480fb0e 100644 --- a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotExtendedCommand.java @@ -1,6 +1,6 @@ package frc.robot.commands.intake.pivot; -import static frc.robot.Constants.RollerandPivotConstants.pivotcounterclockwise; +import static frc.robot.Constants.IntakeConstants.pivotcounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakePivotSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java index ac7154bf..1af95cec 100644 --- a/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/intake/pivot/IntakePivotVerticalCommand.java @@ -2,7 +2,7 @@ package frc.robot.commands.intake.pivot; -import static frc.robot.Constants.RollerandPivotConstants.pivotclockwise; +import static frc.robot.Constants.IntakeConstants.pivotclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakePivotSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java index ac71e654..ee1233bb 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.pastsensortime; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java index fab8ca37..8b593209 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java index e5e7c805..e347cb41 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java @@ -1,9 +1,9 @@ package frc.robot.commands.intake.roller; -import static frc.robot.Constants.RollerandPivotConstants.pastsensortime; -import static frc.robot.Constants.RollerandPivotConstants.rollersclockwise; -import static frc.robot.Constants.RollerandPivotConstants.rollerscounterclockwise; +import static frc.robot.Constants.IntakeConstants.pastsensortime; +import static frc.robot.Constants.IntakeConstants.rollersclockwise; +import static frc.robot.Constants.IntakeConstants.rollerscounterclockwise; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.intake.IntakeRollersSubsystem; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java index 5e09e34b..7499a9a3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java @@ -1,6 +1,6 @@ package frc.robot.subsystems.intake; -import static frc.robot.Constants.RollerandPivotConstants.*; +import static frc.robot.Constants.IntakeConstants.*; //import edu.wpi.first.wpilibj.Encoder; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -9,13 +9,13 @@ public class IntakePivotSubsystem extends SubsystemBase{ - private final com.ctre.phoenix.motorcontrol.can.TalonSRX motor1; + private final com.ctre.phoenix.motorcontrol.can.TalonSRX pivotMotor; // private final Encoder intakeencoder; private final DigitalInput extendedlimitswitch; private final DigitalInput retractedlimitswitch; public IntakePivotSubsystem(){ - motor1 = new TalonSRX(motor1ID); + pivotMotor = new TalonSRX(PIVOT_MOTOR_ID); //intakeencoder = new Encoder(1,2); extendedlimitswitch = new DigitalInput(extendedlimitswitchID); retractedlimitswitch = new DigitalInput(retractedlimitswitchID); @@ -30,7 +30,7 @@ public IntakePivotSubsystem(){ // } public void movePivot(double speed){ - motor1.set(TalonSRXControlMode.PercentOutput, speed); + pivotMotor.set(TalonSRXControlMode.PercentOutput, speed); } public boolean pivotisextended(){ @@ -52,7 +52,7 @@ public boolean pivotisretracted(){ } public void setPivotSpeed(double right){ - motor1.set(TalonSRXControlMode.PercentOutput, right); + pivotMotor.set(TalonSRXControlMode.PercentOutput, right); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java index 47a52dff..dd76d00d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -6,7 +6,7 @@ package frc.robot.subsystems.intake; -import static frc.robot.Constants.RollerandPivotConstants.*; +import static frc.robot.Constants.IntakeConstants.*; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -14,17 +14,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollersSubsystem extends SubsystemBase { - private final TalonSRX topmotor; - private final TalonSRX bottommotor; - private final TalonSRX lastmotor; + private final TalonSRX frontMotor; + private final TalonSRX backMotor; + private final TalonSRX integrationMotor; private final AnalogPotentiometer sensor; /** Creates a new ExampleSubsystem. */ public IntakeRollersSubsystem() { - lastmotor = new TalonSRX(lastmotorID); - topmotor = new TalonSRX(topmotorID); - bottommotor = new TalonSRX(bottommotorID); + integrationMotor = new TalonSRX(INTEGRATION_MOTOR_ID); + frontMotor = new TalonSRX(FRONT_MOTOR_ID); + backMotor = new TalonSRX(BACK_MOTOR_ID); sensor = new AnalogPotentiometer(sensorID); } @@ -38,26 +38,26 @@ public boolean sensorNow(){ } public void setRollSpeed(double top, double bottom){ - topmotor.set(TalonSRXControlMode.PercentOutput, top); - bottommotor.set(TalonSRXControlMode.PercentOutput, bottom); + frontMotor.set(TalonSRXControlMode.PercentOutput, top); + backMotor.set(TalonSRXControlMode.PercentOutput, bottom); } public void setAllRollSpeed(double topone, double bottomone){ - topmotor.set(TalonSRXControlMode.PercentOutput, topone); - lastmotor.set(TalonSRXControlMode.PercentOutput, topone); - bottommotor.set(TalonSRXControlMode.PercentOutput, bottomone); + frontMotor.set(TalonSRXControlMode.PercentOutput, topone); + integrationMotor.set(TalonSRXControlMode.PercentOutput, topone); + backMotor.set(TalonSRXControlMode.PercentOutput, bottomone); } public void setRollersOutwards(Boolean pressedA){ if(pressedA==true) - topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); - bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + frontMotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + backMotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); } public void setRollersInwards(Boolean pressedB){ if(pressedB==true) - topmotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); - bottommotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); + frontMotor.set(TalonSRXControlMode.PercentOutput, rollersclockwise); + backMotor.set(TalonSRXControlMode.PercentOutput, rollerscounterclockwise); } From 265ee88f7be09b9bdb235b074139d3c86deca7ca Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 31 Jan 2024 14:07:10 -0800 Subject: [PATCH 03/25] test --- .../frc/robot/commands/elevator/ElevatorSetAutoCommand.java | 2 +- .../frc/robot/commands/elevator/ElevatorSetManualCommand.java | 2 +- .../frc/robot/commands/elevator/ElevatorToAMPCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToChuteCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToGroundCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToSpeakerCommand.java | 4 ++-- .../subsystems/{Elevator => elevatoring}/ElevatorState.java | 2 +- .../{Elevator => elevatoring}/ElevatorSubsystem.java | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) rename src/main/java/frc/robot/subsystems/{Elevator => elevatoring}/ElevatorState.java (94%) rename src/main/java/frc/robot/subsystems/{Elevator => elevatoring}/ElevatorSubsystem.java (99%) diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java index 5b2ad74b..c6b6f552 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorSetAutoCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java index b5ea4819..a32db353 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorSetManualCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java index b762881b..f7db4d9b 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorState; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorToAMPCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java index d6349e1b..3cf4ecfa 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorState; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorToChuteCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java index c2a47e17..d9471f6d 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorState; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorToGroundCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java index c87f0aab..bd46fca0 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevatoring.ElevatorState; +import frc.robot.subsystems.elevatoring.ElevatorSubsystem; public class ElevatorToSpeakerCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java similarity index 94% rename from src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java rename to src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java index 6c70dcff..dd7e0dd5 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevatoring; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java similarity index 99% rename from src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java rename to src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java index 97953664..e035914c 100644 --- a/src/main/java/frc/robot/subsystems/Elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator; +package frc.robot.subsystems.elevatoring; import java.util.EnumSet; From bc5df1e17e714ff64646a19bda162d0ed519ec21 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 31 Jan 2024 14:11:42 -0800 Subject: [PATCH 04/25] rename Elevator to elevator --- .../frc/robot/commands/elevator/ElevatorSetAutoCommand.java | 2 +- .../frc/robot/commands/elevator/ElevatorSetManualCommand.java | 2 +- .../frc/robot/commands/elevator/ElevatorToAMPCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToChuteCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToGroundCommand.java | 4 ++-- .../frc/robot/commands/elevator/ElevatorToSpeakerCommand.java | 4 ++-- .../subsystems/{elevatoring => elevator}/ElevatorState.java | 2 +- .../{elevatoring => elevator}/ElevatorSubsystem.java | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) rename src/main/java/frc/robot/subsystems/{elevatoring => elevator}/ElevatorState.java (94%) rename src/main/java/frc/robot/subsystems/{elevatoring => elevator}/ElevatorSubsystem.java (99%) diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java index c6b6f552..5b2ad74b 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetAutoCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorSetAutoCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java index a32db353..b5ea4819 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorSetManualCommand.java @@ -1,7 +1,7 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorSetManualCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java index f7db4d9b..b762881b 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorState; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorToAMPCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java index 3cf4ecfa..d6349e1b 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorState; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorToChuteCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java index d9471f6d..c2a47e17 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorState; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorToGroundCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java index bd46fca0..c87f0aab 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java @@ -1,8 +1,8 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevatoring.ElevatorState; -import frc.robot.subsystems.elevatoring.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.elevator.ElevatorSubsystem; public class ElevatorToSpeakerCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; diff --git a/src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java similarity index 94% rename from src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index dd7e0dd5..6c70dcff 100644 --- a/src/main/java/frc/robot/subsystems/elevatoring/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevatoring; +package frc.robot.subsystems.elevator; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java similarity index 99% rename from src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index e035914c..97953664 100644 --- a/src/main/java/frc/robot/subsystems/elevatoring/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevatoring; +package frc.robot.subsystems.elevator; import java.util.EnumSet; From 8d3c5264be207d46cd5698b73ea28a55f8c9bda6 Mon Sep 17 00:00:00 2001 From: Pointy Ice Date: Wed, 31 Jan 2024 18:55:21 -0800 Subject: [PATCH 05/25] updated elevator to integration --- src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 174 ++++++++++-------- .../subsystems/elevator/ElevatorState.java | 2 +- .../elevator/ElevatorSubsystem.java | 125 ++++++++----- 4 files changed, 180 insertions(+), 130 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99fccf14..71599460 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,13 @@ public static class ElevatorConstants { public static final int TRAP_POSITION = 0; public static final int START_POSITION = 0; - public static final double POSITIONCONVERSIONFACTOR = 0; - public static final double VELOCITYCONVERSIONFACTOR = 0; + public static final double EXTENSION_P= 2.4; + public static final double EXTENSION_I= 0; + public static final double EXTENSION_D= 0; + public static final double EXTENSION_TOLERANCE= 0.003; + + public static final double POSITION_CONVERSION_FACTOR = 0; + public static final double VELOCITY_CONVERSION_FACTOR = 0; } public static class OperatorConstants { public static final int kDriverControllerPort = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a9fa18dc..b12e6e94 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc.robot.commands.climb.ClimbLowerCommand; import frc.robot.commands.climb.ClimbRaiseCommand; import frc.robot.subsystems.climb.ClimbSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem; import frc.robot.subsystems.swerve.SwerveModule; @@ -49,6 +50,7 @@ public class RobotContainer { private final ClimbSubsystem climbSubsystem; + private final ElevatorSubsystem elevatorSubsystem; private final XboxController mechController = new XboxController(2); private final JoystickButton aButton = new JoystickButton(mechController, XboxController.Button.kA.value); @@ -77,6 +79,8 @@ public RobotContainer() { climbSubsystem = new ClimbSubsystem(); + elevatorSubsystem = new ElevatorSubsystem(); + traj = Choreo.getTrajectory("Curve"); // Configure the trigger bindings @@ -87,94 +91,104 @@ public RobotContainer() { private void configureBindings() { - bButton.onTrue(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(.7); - })); - - bButton.onFalse(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(0); - })); - - xButton.onTrue(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(-.7); - })); - - xButton.onFalse(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(0); - })); + elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { + if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){ + elevatorSubsystem.setManual(); + elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); + } + else{ + elevatorSubsystem.setAuto(); + } + })); - shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); - // pivotSubsystem.printCurrentAngle(); - }, shooterPivotSubsystem)); + bButton.onTrue(new InstantCommand(() -> { + shooterFeederSubsystem.setFeederMotorSpeed(.7); + })); + + bButton.onFalse(new InstantCommand(() -> { + shooterFeederSubsystem.setFeederMotorSpeed(0); + })); - aButton.onTrue(new InstantCommand(() -> { - shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); - })); + xButton.onTrue(new InstantCommand(() -> { + shooterFeederSubsystem.setFeederMotorSpeed(-.7); + })); + + xButton.onFalse(new InstantCommand(() -> { + shooterFeederSubsystem.setFeederMotorSpeed(0); + })); + + shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { + shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); + // pivotSubsystem.printCurrentAngle(); + }, shooterPivotSubsystem)); + + aButton.onTrue(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); + })); + + aButton.onFalse(new InstantCommand(() -> { + shooterFlywheelSubsystem.setShooterMotorSpeed(0); + })); + + intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { + if(mechController.getPOV() == 90){ + intakeRollerSubsystem.setAllRollSpeed(.3, .3); + } else if (mechController.getPOV() == 270){ + intakeRollerSubsystem.setAllRollSpeed(-.3, -.3); + } else { + intakeRollerSubsystem.setAllRollSpeed(0, 0); + } + })); + - aButton.onFalse(new InstantCommand(() -> { - shooterFlywheelSubsystem.setShooterMotorSpeed(0); - })); + - intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { - if(mechController.getPOV() == 90){ - intakeRollerSubsystem.setAllRollSpeed(.3, .3); - } else if (mechController.getPOV() == 270){ - intakeRollerSubsystem.setAllRollSpeed(-.3, -.3); - } else { - intakeRollerSubsystem.setAllRollSpeed(0, 0); - } - })); - + if(baseSwerveSubsystem instanceof SwerveSubsystem){ + final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; - + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); + } + , swerveSubsystem)); - if(baseSwerveSubsystem instanceof SwerveSubsystem){ - final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; - - swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); - } - , swerveSubsystem)); - - driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { - swerveSubsystem.resetDriverHeading(); - } - )); - - } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ - final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; - // LBumper.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.decrementTest(); - // System.out.println(testSwerveSubsystem.getTest()); - // } - // )); - - // RBumper.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.incrementTest(); - // System.out.println(testSwerveSubsystem.getTest()); - // } - // )); - - // AButton.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.toggletoRun(); - // System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); - // })); - - } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ - final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; - - swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - } - , swerveSubsystem)); - - driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { - swerveSubsystem.toggletoRun(); - })); + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + swerveSubsystem.resetDriverHeading(); + } + )); + } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ + final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; + // LBumper.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.decrementTest(); + // System.out.println(testSwerveSubsystem.getTest()); + // } + // )); + + // RBumper.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.incrementTest(); + // System.out.println(testSwerveSubsystem.getTest()); + // } + // )); + + // AButton.onTrue(new InstantCommand(() -> { + // testSwerveSubsystem.toggletoRun(); + // System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); + // })); + + } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ + final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; + + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); } + , swerveSubsystem)); + + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + swerveSubsystem.toggletoRun(); + })); + + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index 6c70dcff..63ddf6b5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -18,7 +18,7 @@ private ElevatorState(double extendDistanceMeters){ this.extendDistanceMeters = extendDistanceMeters; } - public double getExtension(){ + public double getExtendDistanceMeters(){ return this.extendDistanceMeters; } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 97953664..510ff281 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -11,6 +11,8 @@ import com.revrobotics.SparkPIDController.ArbFFUnits; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; @@ -20,17 +22,19 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableEvent.Kind; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.ElevatorConstants.*; public class ElevatorSubsystem extends SubsystemBase{ private NetworkTableInstance elevatorNetworkTableInstance; private NetworkTable elevatorNetworkTable; + private NetworkTableEntry elevatorNetworkTablePositionEntry; - private volatile boolean IS_MANUAL = false; + private volatile boolean isManual = false; + private double manualPower = 0; private ElevatorState state = ElevatorState.GROUND; private ElevatorState targetState = ElevatorState.START; @@ -40,83 +44,107 @@ public class ElevatorSubsystem extends SubsystemBase{ private SparkPIDController extensionPidController; //PID Values - private static final double extensionP = 2.4; - private static final double extensionI = 0; - private static final double extensionD = 0; - private static final double extensionTolerance = 0.003; private final CANSparkMax extensionFollow; private final DigitalInput zeroLimitSwitch; //Controller for testing. - private final XboxController mechController; - public ElevatorSubsystem(){ + public ElevatorSubsystem() { //Print out current position for debug & measurement //System.out.print(extensionEncoder.getPosition()); + this.zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); elevatorNetworkTableInstance = NetworkTableInstance.getDefault(); elevatorNetworkTable = elevatorNetworkTableInstance.getTable("elevator"); - elevatorNetworkTable.addListener("target_position", EnumSet.of(NetworkTableEvent.Kind.kValueAll), this::acceptNewPosition); - - extensionMotor = new CANSparkMax(Constants.ElevatorConstants.EXTENSION_ID, MotorType.kBrushless); + elevatorNetworkTablePositionEntry = elevatorNetworkTable.getEntry("target_position"); + elevatorNetworkTable.addListener("target_position", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (NetworkTable table, String key, NetworkTableEvent event) -> { + String message = event.valueData.value.getString(); + System.out.println(message); + if(message.equals("GROUND")){ + System.out.println("Setting Target to Ground"); + this.setTargetState(ElevatorState.GROUND); + } + else if(message.equals("SPEAKER")){ + System.out.println("Setting Target to SPEAKER"); + this.setTargetState(ElevatorState.SPEAKER); + } + else if(message.equals("AMP")){ + System.out.println("Setting Target to AMP"); + this.setTargetState(ElevatorState.AMP); + } + else return; + ElevatorState currentTargetState = this.getTargetState(); + if(currentTargetState.equals(ElevatorState.AMP)){ + System.out.println("New target state is AMP!"); + } + else if(currentTargetState.equals(ElevatorState.GROUND)){ + System.out.println("New target state is GROUND!"); + } + else if(currentTargetState.equals(ElevatorState.SPEAKER)){ + System.out.println("New target state is SPEAKER!"); + } + }); + //this entry is working! + extensionMotor = new CANSparkMax(ElevatorConstants.EXTENSION_ID, MotorType.kBrushless); extensionMotor.setIdleMode(IdleMode.kBrake); extensionEncoder = extensionMotor.getEncoder(); - extensionEncoder.setPositionConversionFactor(Constants.ElevatorConstants.POSITIONCONVERSIONFACTOR); - extensionEncoder.setVelocityConversionFactor(Constants.ElevatorConstants.VELOCITYCONVERSIONFACTOR); + extensionEncoder.setPositionConversionFactor(ElevatorConstants.POSITION_CONVERSION_FACTOR); + extensionEncoder.setVelocityConversionFactor(ElevatorConstants.VELOCITY_CONVERSION_FACTOR); extensionEncoder.setPosition(0); - extensionFollow = new CANSparkMax(Constants.ElevatorConstants.EXTENSION_FOLLOW_ID, MotorType.kBrushless); + extensionFollow = new CANSparkMax(ElevatorConstants.EXTENSION_FOLLOW_ID, MotorType.kBrushless); extensionFollow.follow(extensionMotor); extensionFollow.setIdleMode(IdleMode.kBrake); extensionPidController = extensionMotor.getPIDController(); - extensionPidController.setP(extensionP); - extensionPidController.setI(extensionI); - extensionPidController.setD(extensionD); - extensionPidController.setSmartMotionAllowedClosedLoopError(extensionTolerance, 0); - - zeroLimitSwitch = new DigitalInput(Constants.ElevatorConstants.ZERO_LIMIT_ID); - - //Controller for testing. - mechController = new XboxController(Constants.OperatorConstants.kDriverControllerPort); + extensionPidController.setP(ElevatorConstants.EXTENSION_P); + extensionPidController.setI(ElevatorConstants.EXTENSION_I); + extensionPidController.setD(ElevatorConstants.EXTENSION_D); + extensionPidController.setSmartMotionAllowedClosedLoopError(ElevatorConstants.EXTENSION_TOLERANCE, 0); } @Override public void periodic(){ - - if(IS_MANUAL){ + //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); + System.out.println(this.getExtensionMeters()); + //System.out.println(this.getTargetState()); + if(isManual){ //Add some factors for better control. - extensionMotor.set(mechController.getRightY()); + extensionMotor.set(this.manualPower); return; } + if (zeroLimitSwitch != null && !zeroLimitSwitch.get()){ extensionEncoder.setPosition(0); } - - extensionMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + + //extensionMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + //this through overun when no motor connected. //Start move to target posision if (targetState != state){ - extensionPidController.setReference(targetState.getExtension(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut); + extensionPidController.setReference(targetState.getExtendDistanceMeters(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut); } - - if(mechController.getAButtonPressed()){ - this.setTargetState(ElevatorState.GROUND); - } - else if(mechController.getBButtonPressed()){ - this.setTargetState(ElevatorState.AMP); - } - else if(mechController.getXButtonPressed()){ - this.setTargetState(ElevatorState.SPEAKER); - } - else if(mechController.getYButtonPressed()){ - this.setTargetState(ElevatorState.CHUTE); + if(atState(this.targetState)){ + this.setState(this.getTargetState()); } } + public boolean atState(ElevatorState state){ + double distance = Math.abs(this.getExtensionMeters() - state.getExtendDistanceMeters()); + if(distance < ElevatorConstants.EXTENSION_TOLERANCE){ + return true; + } + else{ + return false; + } + } + public void setState(ElevatorState state) { this.state = state; return; @@ -128,12 +156,12 @@ public void setTargetState(ElevatorState targetState){ } public void setManual(){ - this.IS_MANUAL = true; + this.isManual = true; return; } public void setAuto(){ - this.IS_MANUAL = false; + this.isManual = false; return; } public double getExtensionMeters() { @@ -148,9 +176,12 @@ public ElevatorState getTargetState(){ return this.targetState; } + public void setManualPower(double power){ + this.manualPower = power; + } - private void acceptNewPosition(NetworkTable table, String key, NetworkTableEvent event){ + /* private void acceptNewPosition(NetworkTable table, String key, NetworkTableEvent event){ System.out.println("got networktablex"); System.out.println(event.valueData.toString()); - } -} \ No newline at end of file + } */ +} From 4c3faec43ef9475f51b5a14cd9abe82cdce446c3 Mon Sep 17 00:00:00 2001 From: MysticalApple Date: Wed, 31 Jan 2024 19:16:57 -0800 Subject: [PATCH 06/25] removed weird .gradle thing --- .gradle 8.30.16 PM/7.5.1/checksums/checksums.lock | Bin 17 -> 0 bytes .../dependencies-accessors.lock | Bin 17 -> 0 bytes .../7.5.1/dependencies-accessors/gc.properties | 0 .../7.5.1/fileChanges/last-build.bin | Bin 1 -> 0 bytes .../7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 0 bytes .gradle 8.30.16 PM/7.5.1/gc.properties | 0 .../buildOutputCleanup/buildOutputCleanup.lock | Bin 17 -> 0 bytes .../buildOutputCleanup/cache.properties | 2 -- .gradle 8.30.16 PM/vcs-1/gc.properties | 0 9 files changed, 2 deletions(-) delete mode 100644 .gradle 8.30.16 PM/7.5.1/checksums/checksums.lock delete mode 100644 .gradle 8.30.16 PM/7.5.1/dependencies-accessors/dependencies-accessors.lock delete mode 100644 .gradle 8.30.16 PM/7.5.1/dependencies-accessors/gc.properties delete mode 100644 .gradle 8.30.16 PM/7.5.1/fileChanges/last-build.bin delete mode 100644 .gradle 8.30.16 PM/7.5.1/fileHashes/fileHashes.lock delete mode 100644 .gradle 8.30.16 PM/7.5.1/gc.properties delete mode 100644 .gradle 8.30.16 PM/buildOutputCleanup/buildOutputCleanup.lock delete mode 100644 .gradle 8.30.16 PM/buildOutputCleanup/cache.properties delete mode 100644 .gradle 8.30.16 PM/vcs-1/gc.properties diff --git a/.gradle 8.30.16 PM/7.5.1/checksums/checksums.lock b/.gradle 8.30.16 PM/7.5.1/checksums/checksums.lock deleted file mode 100644 index b9e649527c48dc34379ee903a761a550e1be1aea..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17 TcmZR!$ Date: Wed, 31 Jan 2024 19:22:29 -0800 Subject: [PATCH 07/25] don't track .vscode either --- .gitignore | 2 +- .vscode/launch.json | 20 -------------------- .vscode/settings.json | 30 ------------------------------ 3 files changed, 1 insertion(+), 51 deletions(-) delete mode 100644 .vscode/launch.json delete mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 5528d4f6..1b2a374d 100644 --- a/.gitignore +++ b/.gitignore @@ -104,7 +104,7 @@ Temporary Items .apdisk ### VisualStudioCode ### -.vscode/* +.vscode/ !.vscode/settings.json !.vscode/tasks.json !.vscode/launch.json diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index 4dbe5d3d..00000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 565b0808..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,30 +0,0 @@ -{ - "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", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" -} From c65fa4824eb87c6e39b7ee012016bb8b23cf30d7 Mon Sep 17 00:00:00 2001 From: MysticalApple Date: Wed, 31 Jan 2024 19:25:32 -0800 Subject: [PATCH 08/25] also .OutlineViewer --- .OutlineViewer/outlineviewer.json | 68 ------------------------------- .gitignore | 1 + 2 files changed, 1 insertion(+), 68 deletions(-) delete mode 100644 .OutlineViewer/outlineviewer.json diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json deleted file mode 100644 index c73712d6..00000000 --- a/.OutlineViewer/outlineviewer.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "Clients": { - "open": true - }, - "Connections": { - "open": true - }, - "NT3@10.1.92.206:59486": { - "Publishers": { - "open": true - }, - "Subscribers": { - "open": true - }, - "open": true - }, - "NT3@10.1.92.206:59984": { - "Publishers": { - "open": true - }, - "Subscribers": { - "open": true - }, - "open": true - }, - "NT3@10.1.92.206:61719": { - "Publishers": { - "open": true - }, - "open": true - }, - "NT3@10.1.92.206:65507": { - "Publishers": { - "open": true - }, - "open": true - }, - "NetworkTables Settings": { - "mode": "Client (NT4)", - "serverTeam": "192" - }, - "Server": { - "open": true - }, - "outlineviewer@5": { - "open": true - }, - "retained": { - "elevator": { - "open": true - } - }, - "shuffleboard@2": { - "Subscribers": { - "open": true - } - }, - "transitory": { - "Shuffleboard": { - "Auton": { - "Field": { - "open": true - }, - "open": true - } - } - } -} diff --git a/.gitignore b/.gitignore index 1b2a374d..3dd36b82 100644 --- a/.gitignore +++ b/.gitignore @@ -170,6 +170,7 @@ out/ # Simulation GUI and other tools window save file *-window.json +.OutlineViewer/ # Simulation data log directory logs/ From 228908c6a4fab28e82ade70a745a1ec3531246c2 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 31 Jan 2024 20:55:27 -0800 Subject: [PATCH 09/25] testing changes --- src/main/java/frc/robot/Constants.java | 14 +++++------ src/main/java/frc/robot/RobotContainer.java | 25 ++++++------------- .../frc/robot/commands/ShootModeCommand.java | 20 +++++++++++++++ .../frc/robot/subsystems/climb/ClimbArm.java | 2 +- .../intake/IntakeRollersSubsystem.java | 5 ++-- .../shooter/ShooterFeederSubsystem.java | 4 ++- .../shooter/ShooterPivotSubsystem.java | 4 +-- 7 files changed, 44 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ShootModeCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 71599460..dc303ade 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,7 @@ public static class ElevatorConstants { public static final float EXTENSION_LIMIT_METERS = 0; - public static final int ZERO_LIMIT_ID = 0; + public static final int ZERO_LIMIT_ID = 2; public static final int GROUND_POSITION = 0; public static final int SPEAKER_POSITION = 0; @@ -78,8 +78,8 @@ public static class IntakeConstants { public static final int PIVOT_MOTOR_ID = 16; public static final int sensorID = 0; - public static final int extendedlimitswitchID = 1; - public static final int retractedlimitswitchID = 2; + public static final int extendedlimitswitchID = 5; + public static final int retractedlimitswitchID = 6; //public static final int intakeencoderID = 3; public static final double rollersclockwise = 1; @@ -94,11 +94,11 @@ public static class IntakeConstants { public static class ClimbConstants { - public static final int LEFT_WINCH_MOTOR_ID = 21; - public static final int LEFT_ZERO_LIMIT_ID = 11; + public static final int LEFT_WINCH_MOTOR_ID = 10; + public static final int LEFT_ZERO_LIMIT_ID = 0; - public static final int RIGHT_WINCH_MOTOR_ID = 22; - public static final int RIGHT_ZERO_LIMIT_ID = 12; + public static final int RIGHT_WINCH_MOTOR_ID = 11; + public static final int RIGHT_ZERO_LIMIT_ID = 1; public static final double WINCH_REDUCTION = 9.49; public static final double AXLE_PERIMETER_METERS = 6 * Units.inchesToMeters(.289) ; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b12e6e94..58241e71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,7 +70,7 @@ public RobotContainer() { //construct Test // module = new SwerveModule(6, 7, 0); // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); - baseSwerveSubsystem = new SwerveSubsystem(); + baseSwerveSubsystem = null;// new SwerveSubsystem(); intakePivotSubsystem = new IntakePivotSubsystem(); shooterFeederSubsystem = new ShooterFeederSubsystem(); @@ -88,21 +88,11 @@ public RobotContainer() { } - private void configureBindings() { + private void configureBindings() { - elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { - if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){ - elevatorSubsystem.setManual(); - elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); - } - else{ - elevatorSubsystem.setAuto(); - } - })); - bButton.onTrue(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(.7); + shooterFeederSubsystem.setFeederMotorSpeed(.4); })); bButton.onFalse(new InstantCommand(() -> { @@ -132,13 +122,15 @@ private void configureBindings() { intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { if(mechController.getPOV() == 90){ - intakeRollerSubsystem.setAllRollSpeed(.3, .3); + intakeRollerSubsystem.setAllRollSpeed(1, .8); } else if (mechController.getPOV() == 270){ - intakeRollerSubsystem.setAllRollSpeed(-.3, -.3); + intakeRollerSubsystem.setAllRollSpeed(-.8, -.5); } else { intakeRollerSubsystem.setAllRollSpeed(0, 0); } - })); + }, intakeRollerSubsystem)); + + @@ -189,7 +181,6 @@ private void configureBindings() { })); } - } diff --git a/src/main/java/frc/robot/commands/ShootModeCommand.java b/src/main/java/frc/robot/commands/ShootModeCommand.java new file mode 100644 index 00000000..901fa8f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootModeCommand.java @@ -0,0 +1,20 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.intake.pivot.IntakePivotVerticalCommand; +import frc.robot.commands.shooter.feed.ShooterFeedShootCommand; +import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand; +import frc.robot.subsystems.intake.IntakePivotSubsystem; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; + +public class ShootModeCommand extends SequentialCommandGroup { + IntakePivotSubsystem intakePivotSubsystem; + ShooterFlywheelSubsystem shooterFlywheelSubsystem; + ShooterFeederSubsystem shooterFeederSubsystem; + + public ShootModeCommand(){ + addCommands(new IntakePivotVerticalCommand(intakePivotSubsystem).alongWith(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem)), + new ShooterFeedShootCommand(shooterFeederSubsystem)); + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java index ba3bb542..bae7b07a 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java @@ -34,7 +34,7 @@ public ClimbArm(int WINCH_MOTOR_ID, int ZERO_LIMIT_ID) { sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true); }); - zeroLimitSwitch = new DigitalInput(LEFT_ZERO_LIMIT_ID); + zeroLimitSwitch = new DigitalInput(ZERO_LIMIT_ID); } public void update() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java index dd76d00d..2f2f7c82 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -24,6 +24,7 @@ public class IntakeRollersSubsystem extends SubsystemBase { public IntakeRollersSubsystem() { integrationMotor = new TalonSRX(INTEGRATION_MOTOR_ID); frontMotor = new TalonSRX(FRONT_MOTOR_ID); + frontMotor.setInverted(true); backMotor = new TalonSRX(BACK_MOTOR_ID); sensor = new AnalogPotentiometer(sensorID); } @@ -44,8 +45,8 @@ public void setRollSpeed(double top, double bottom){ public void setAllRollSpeed(double topone, double bottomone){ frontMotor.set(TalonSRXControlMode.PercentOutput, topone); - integrationMotor.set(TalonSRXControlMode.PercentOutput, topone); - backMotor.set(TalonSRXControlMode.PercentOutput, bottomone); + integrationMotor.set(TalonSRXControlMode.PercentOutput, bottomone); + backMotor.set(TalonSRXControlMode.PercentOutput, topone); } public void setRollersOutwards(Boolean pressedA){ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java index c3acc43b..ab2010c5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -22,7 +22,9 @@ public class ShooterFeederSubsystem extends SubsystemBase{ public ShooterFeederSubsystem(){ //motors - feederMotor = new TalonSRX(10); + feederMotor = new TalonSRX(15); + + feederMotor.setInverted(true); //sensors shooterSensor = new ColorSensorV3(I2C.Port.kMXP); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index b8afd04f..2361aee6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -16,7 +16,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { public final double PIVOT_SPEED = 0.1; final double GEARBOX_RATIO = 18.16; //ask cadders public final int ERRORTOLERANCE = 5; //error tolerance for pid - final int LIMIT_SWITCH_ID = 1; //placeholder + final int LIMIT_SWITCH_ID = 3; //placeholder final double CONVERSION_FACTOR = Math.PI/(2*4.57); //motors @@ -142,7 +142,7 @@ public void periodic(){ setAngle(getAutoAimAngle(getDistance())); } - printCurrentAngle(); + // printCurrentAngle(); // System.out.println("current pos" + rotationEncoder.getPosition()); From ed9f15d8d46e7f8c72f71edb2dd64954c6bfb9b8 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Thu, 1 Feb 2024 15:17:21 -0800 Subject: [PATCH 10/25] update constants --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dc303ade..0dd1bfee 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,8 +17,8 @@ */ public final class Constants { public static class ElevatorConstants { - public static final int EXTENSION_ID = 0; - public static final int EXTENSION_FOLLOW_ID = 0; + public static final int EXTENSION_ID = 10; + public static final int EXTENSION_FOLLOW_ID = 11; public static final float EXTENSION_LIMIT_METERS = 0; @@ -94,10 +94,10 @@ public static class IntakeConstants { public static class ClimbConstants { - public static final int LEFT_WINCH_MOTOR_ID = 10; + public static final int LEFT_WINCH_MOTOR_ID = 8; public static final int LEFT_ZERO_LIMIT_ID = 0; - public static final int RIGHT_WINCH_MOTOR_ID = 11; + public static final int RIGHT_WINCH_MOTOR_ID = 9; public static final int RIGHT_ZERO_LIMIT_ID = 1; public static final double WINCH_REDUCTION = 9.49; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 58241e71..d34dbb4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,6 +90,15 @@ public RobotContainer() { private void configureBindings() { + elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { + if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){ + elevatorSubsystem.setManual(); + elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); + } + else{ + elevatorSubsystem.setAuto(); + } + })); bButton.onTrue(new InstantCommand(() -> { shooterFeederSubsystem.setFeederMotorSpeed(.4); From efa1de8e62a6a20dc6f8fe7a394e4a8e3ba64991 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Thu, 1 Feb 2024 17:08:20 -0800 Subject: [PATCH 11/25] elevator tuning --- src/main/java/frc/robot/Constants.java | 20 +++++++-------- src/main/java/frc/robot/RobotContainer.java | 19 +++++++------- .../elevator/ElevatorToAMPCommand.java | 9 ++++--- .../elevator/ElevatorToChuteCommand.java | 5 +--- .../elevator/ElevatorToGroundCommand.java | 16 ++++++++---- .../elevator/ElevatorToSpeakerCommand.java | 5 +--- .../elevator/ElevatorSubsystem.java | 25 +++++++++++++------ .../shooter/ShooterPivotSubsystem.java | 3 ++- 8 files changed, 57 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0dd1bfee..f915b1f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,20 +24,20 @@ public static class ElevatorConstants { public static final int ZERO_LIMIT_ID = 2; - public static final int GROUND_POSITION = 0; - public static final int SPEAKER_POSITION = 0; - public static final int AMP_POSITION = 0; - public static final int CHUTE_POSITION = 0; - public static final int TRAP_POSITION = 0; - public static final int START_POSITION = 0; - - public static final double EXTENSION_P= 2.4; + public static final double GROUND_POSITION = 0; + public static final double SPEAKER_POSITION = 0; + public static final double AMP_POSITION = Units.inchesToMeters(28); + public static final double CHUTE_POSITION = Units.inchesToMeters(25); + public static final double TRAP_POSITION = Units.inchesToMeters(30); + public static final double START_POSITION = 0; + + public static final double EXTENSION_P= 3.5; public static final double EXTENSION_I= 0; public static final double EXTENSION_D= 0; public static final double EXTENSION_TOLERANCE= 0.003; - public static final double POSITION_CONVERSION_FACTOR = 0; - public static final double VELOCITY_CONVERSION_FACTOR = 0; + public static final double POSITION_CONVERSION_FACTOR = Units.inchesToMeters(30.)/63.5; + public static final double VELOCITY_CONVERSION_FACTOR = 1; } public static class OperatorConstants { public static final int kDriverControllerPort = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d34dbb4a..8f61d22c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,8 @@ import frc.robot.controllers.XboxDriveController; import frc.robot.commands.climb.ClimbLowerCommand; import frc.robot.commands.climb.ClimbRaiseCommand; +import frc.robot.commands.elevator.ElevatorToAMPCommand; +import frc.robot.commands.elevator.ElevatorToGroundCommand; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; @@ -91,14 +93,10 @@ public RobotContainer() { private void configureBindings() { elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { - if(mechController.getLeftTriggerAxis() != 0 && mechController.getRightTriggerAxis() != 0){ - elevatorSubsystem.setManual(); - elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); - } - else{ - elevatorSubsystem.setAuto(); - } - })); + + elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); + + }, elevatorSubsystem)); bButton.onTrue(new InstantCommand(() -> { shooterFeederSubsystem.setFeederMotorSpeed(.4); @@ -117,7 +115,7 @@ private void configureBindings() { })); shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); + // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); // pivotSubsystem.printCurrentAngle(); }, shooterPivotSubsystem)); @@ -139,6 +137,9 @@ private void configureBindings() { } }, intakeRollerSubsystem)); + rightBumper.onTrue(new ElevatorToAMPCommand(elevatorSubsystem)); + leftBumper.onTrue(new ElevatorToGroundCommand(elevatorSubsystem)); + diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java index b762881b..862d8574 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToAMPCommand.java @@ -13,6 +13,10 @@ public ElevatorToAMPCommand(ElevatorSubsystem elevatorSubsystem){ @Override public void end(boolean interrupted){ + System.out.println("TO AMP FINISHED"); + if (interrupted){ + System.out.println("TO AMP INTERUPTED"); + } return; } @@ -23,9 +27,6 @@ public void execute(){ @Override public boolean isFinished(){ - if(this.elevatorSubsystem.getState()==ElevatorState.AMP){ - return true; - } - else return false; + return elevatorSubsystem.atState(ElevatorState.AMP); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java index d6349e1b..4dfa57b7 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToChuteCommand.java @@ -23,9 +23,6 @@ public void execute(){ @Override public boolean isFinished(){ - if(this.elevatorSubsystem.getState()==ElevatorState.CHUTE){ - return true; - } - else return false; + return elevatorSubsystem.atState(ElevatorState.CHUTE); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java index c2a47e17..68dae26b 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java @@ -13,19 +13,25 @@ public ElevatorToGroundCommand(ElevatorSubsystem elevatorSubsystem){ @Override public void end(boolean interrupted){ + System.out.println("TO GROUND FINISHED"); + if (interrupted){ + System.out.println("TO GROUND INTERUPTED"); + } return; } + @Override + public void initialize() { + + } + @Override public void execute(){ - this.elevatorSubsystem.setTargetState(ElevatorState.GROUND); + elevatorSubsystem.setTargetState(ElevatorState.GROUND); } @Override public boolean isFinished(){ - if(this.elevatorSubsystem.getState()==ElevatorState.GROUND){ - return true; - } - else return false; + return elevatorSubsystem.atState(ElevatorState.GROUND); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java index c87f0aab..4070d4f6 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java @@ -23,9 +23,6 @@ public void execute(){ @Override public boolean isFinished(){ - if(this.elevatorSubsystem.getState()==ElevatorState.SPEAKER){ - return true; - } - else return false; + return elevatorSubsystem.atState(ElevatorState.SPEAKER); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 510ff281..cb09e081 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -12,7 +12,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; - +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; @@ -42,6 +42,7 @@ public class ElevatorSubsystem extends SubsystemBase{ private final CANSparkMax extensionMotor; private RelativeEncoder extensionEncoder; private SparkPIDController extensionPidController; + private final Timer timer = new Timer(); //PID Values @@ -54,6 +55,8 @@ public class ElevatorSubsystem extends SubsystemBase{ public ElevatorSubsystem() { //Print out current position for debug & measurement //System.out.print(extensionEncoder.getPosition()); + + timer.start(); this.zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); elevatorNetworkTableInstance = NetworkTableInstance.getDefault(); @@ -91,6 +94,9 @@ else if(currentTargetState.equals(ElevatorState.SPEAKER)){ //this entry is working! extensionMotor = new CANSparkMax(ElevatorConstants.EXTENSION_ID, MotorType.kBrushless); extensionMotor.setIdleMode(IdleMode.kBrake); + extensionMotor.setInverted(true); + extensionMotor.setClosedLoopRampRate(0.3); + extensionEncoder = extensionMotor.getEncoder(); extensionEncoder.setPositionConversionFactor(ElevatorConstants.POSITION_CONVERSION_FACTOR); @@ -100,17 +106,22 @@ else if(currentTargetState.equals(ElevatorState.SPEAKER)){ extensionFollow = new CANSparkMax(ElevatorConstants.EXTENSION_FOLLOW_ID, MotorType.kBrushless); extensionFollow.follow(extensionMotor); extensionFollow.setIdleMode(IdleMode.kBrake); + extensionFollow.setInverted(true); extensionPidController = extensionMotor.getPIDController(); extensionPidController.setP(ElevatorConstants.EXTENSION_P); extensionPidController.setI(ElevatorConstants.EXTENSION_I); extensionPidController.setD(ElevatorConstants.EXTENSION_D); extensionPidController.setSmartMotionAllowedClosedLoopError(ElevatorConstants.EXTENSION_TOLERANCE, 0); + extensionPidController.setOutputRange(-0.7, 1); } @Override public void periodic(){ - //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); - System.out.println(this.getExtensionMeters()); + //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); + if(timer.advanceIfElapsed(.2)){ + System.out.println(Units.metersToInches(getExtensionMeters())); + } + //System.out.println(this.getTargetState()); if(isManual){ //Add some factors for better control. @@ -127,12 +138,10 @@ public void periodic(){ //Start move to target posision if (targetState != state){ - extensionPidController.setReference(targetState.getExtendDistanceMeters(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut); - } - if(atState(this.targetState)){ - this.setState(this.getTargetState()); + } + } public boolean atState(ElevatorState state){ @@ -151,7 +160,7 @@ public void setState(ElevatorState state) { } public void setTargetState(ElevatorState targetState){ - this.targetState = targetState; + extensionPidController.setReference(targetState.getExtendDistanceMeters(), ControlType.kPosition, 0, 0.03, ArbFFUnits.kPercentOut); return; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 2361aee6..6e27e262 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -17,7 +17,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { final double GEARBOX_RATIO = 18.16; //ask cadders public final int ERRORTOLERANCE = 5; //error tolerance for pid final int LIMIT_SWITCH_ID = 3; //placeholder - final double CONVERSION_FACTOR = Math.PI/(2*4.57); + final double CONVERSION_FACTOR = Math.PI/(2.*4.57); //motors private final CANSparkMax pivotMotor; @@ -66,6 +66,7 @@ public ShooterPivotSubsystem(boolean alliance){ //encoder stuff rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR); + rotationEncoder.setVelocityConversionFactor(CONVERSION_FACTOR * 60); //rotationEncoder.setInverted(true); rotationPIDController.setSmartMotionAllowedClosedLoopError(ERRORTOLERANCE, 0); //what does 0 do (slotID is from 0-3) From c9638d37dba19cb085d74d1fe5745c9157782e0e Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Thu, 1 Feb 2024 17:17:23 -0800 Subject: [PATCH 12/25] edited constants --- src/main/java/frc/robot/Constants.java | 7 +++++++ .../robot/subsystems/shooter/ShooterFeederSubsystem.java | 5 ++--- .../robot/subsystems/shooter/ShooterFlywheelSubsystem.java | 7 +++++-- .../robot/subsystems/shooter/ShooterPivotSubsystem.java | 4 +++- 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f915b1f0..fbe16664 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,8 +90,15 @@ public static class IntakeConstants { public static final double pastsensortime = 0.5; } + public static class ShooterConstants { + public static final int LIMIT_SWITCH_ID = 1; + public static final int PIVOT_MOTOR_ID = 12; + public static final int SHOOTER_MOTOR_ONE_ID = 13; + public static final int SHOOTER_MOTOR_TWO_ID = 14; + public static final int FEEDER_MOTOR_ID = 10; + } public static class ClimbConstants { public static final int LEFT_WINCH_MOTOR_ID = 8; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java index ab2010c5..a7f6527e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -3,7 +3,7 @@ import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.revrobotics.ColorSensorV3; - +import static frc.robot.Constants.ShooterConstants.*; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,8 +22,7 @@ public class ShooterFeederSubsystem extends SubsystemBase{ public ShooterFeederSubsystem(){ //motors - feederMotor = new TalonSRX(15); - + feederMotor = new TalonSRX(FEEDER_MOTOR_ID); feederMotor.setInverted(true); //sensors diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index c86fcc4e..a24abbd2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -4,6 +4,7 @@ //create enums, expecting, holding, firing, and no note package frc.robot.subsystems.shooter; +import static frc.robot.Constants.ShooterConstants.*; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -13,6 +14,8 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { //change later (ALL OF THEM ARE PLACEHOLDERS) int IDNUMBER = 10; //so I remember to change them later + // public static final int SHOOTER_MOTOR_ONE_ID = 13; + // public static final int SHOOTER_MOTOR_TWO_ID = 14; public final double SHOOTER_MOTOR_SPEED = 1; //motors @@ -26,8 +29,8 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { public ShooterFlywheelSubsystem(){ //motors - shooterMotor = new CANSparkMax(13, MotorType.kBrushless); - shooterMotorTwo = new CANSparkMax(14, MotorType.kBrushless); + shooterMotor = new CANSparkMax(SHOOTER_MOTOR_ONE_ID, MotorType.kBrushless); + shooterMotorTwo = new CANSparkMax(SHOOTER_MOTOR_TWO_ID, MotorType.kBrushless); shooterMotor.setIdleMode(IdleMode.kCoast); shooterMotorTwo.setIdleMode(IdleMode.kCoast); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 6e27e262..e5144f58 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.ShooterConstants.*; + public class ShooterPivotSubsystem extends SubsystemBase { @@ -49,7 +51,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { public ShooterPivotSubsystem(boolean alliance){ //motors - pivotMotor = new CANSparkMax(12, MotorType.kBrushless); + pivotMotor = new CANSparkMax(PIVOT_MOTOR_ID, MotorType.kBrushless); pivotMotor.setInverted(true); //devices From 71418ac28915a16368f111262c13a765a61496a4 Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Thu, 1 Feb 2024 17:24:11 -0800 Subject: [PATCH 13/25] commands --- .../pivot/ShooterPivotVerticalCommand.java | 28 +++++++++++++++++-- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java index 1e419cda..29e711c4 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java @@ -1,5 +1,27 @@ package frc.robot.commands.shooter.pivot; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; -public class ShooterPivotVerticalCommand { - -} +public class ShooterPivotVerticalCommand extends Command{ + ShooterPivotSubsystem pivotSubsystem; + + public ShooterPivotVerticalCommand(ShooterPivotSubsystem pivotSubsystem){ + this.pivotSubsystem = pivotSubsystem; + addRequirements(pivotSubsystem); + } + + @Override + public void initialize() { + pivotSubsystem.setAutoAimBoolean(false); + pivotSubsystem.setAngle(90.0); + } + + @Override + public boolean isFinished() { + if(Math.abs(pivotSubsystem.getPosition()) - 90 < pivotSubsystem.ERRORTOLERANCE){ + return true; + } + + return false; + } +} \ No newline at end of file From 17abb8b3db7d480bc37750d0575f10352983c22e Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Thu, 1 Feb 2024 17:29:11 -0800 Subject: [PATCH 14/25] set custom angle command --- .../shooter/pivot/ShooterPivotSetAngle.java | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java new file mode 100644 index 00000000..b1c13075 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java @@ -0,0 +1,29 @@ +package frc.robot.commands.shooter.pivot; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; + +public class ShooterPivotSetAngle extends Command{ + ShooterPivotSubsystem pivotSubsystem; + double angle; + + public ShooterPivotSetAngle(ShooterPivotSubsystem pivotSubsystem, double angle){ + this.pivotSubsystem = pivotSubsystem; + addRequirements(pivotSubsystem); + this.angle = angle; + } + + @Override + public void initialize() { + pivotSubsystem.setAutoAimBoolean(false); + pivotSubsystem.setAngle(angle); + } + + @Override + public boolean isFinished() { + if(Math.abs(pivotSubsystem.getPosition()) - angle < pivotSubsystem.ERRORTOLERANCE){ + return true; + } + + return false; + } +} \ No newline at end of file From 35f70432c21f7733eb2227b3ac67c156b22ca741 Mon Sep 17 00:00:00 2001 From: siyoyoCode Date: Thu, 1 Feb 2024 17:43:04 -0800 Subject: [PATCH 15/25] fixed distance functions --- .../shooter/pivot/ShooterPivotAimCommand.java | 5 +--- .../shooter/pivot/ShooterPivotSetAngle.java | 6 +--- .../pivot/ShooterPivotVerticalCommand.java | 6 +--- .../shooter/ShooterPivotSubsystem.java | 30 +++++++++---------- 4 files changed, 17 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java index b089c0e1..dc2977c2 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotAimCommand.java @@ -21,10 +21,7 @@ public void end(){ } public boolean isFinished(){ - if(Math.abs(shooterPivotSubsystem.getPosition()) - shooterPivotSubsystem.getCurrentAngle() < shooterPivotSubsystem.ERRORTOLERANCE){ - return true; - } + return(Math.abs(shooterPivotSubsystem.getPosition() - shooterPivotSubsystem.getAutoAimAngle()) < shooterPivotSubsystem.ERRORTOLERANCE); - return false; } } diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java index b1c13075..f72aeabf 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java @@ -20,10 +20,6 @@ public void initialize() { @Override public boolean isFinished() { - if(Math.abs(pivotSubsystem.getPosition()) - angle < pivotSubsystem.ERRORTOLERANCE){ - return true; - } - - return false; + return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java index 29e711c4..097d1bbe 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java @@ -18,10 +18,6 @@ public void initialize() { @Override public boolean isFinished() { - if(Math.abs(pivotSubsystem.getPosition()) - 90 < pivotSubsystem.ERRORTOLERANCE){ - return true; - } - - return false; + return (Math.abs(pivotSubsystem.getPosition() - 90) < pivotSubsystem.ERRORTOLERANCE); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index e5144f58..897d44cb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -38,6 +38,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { private boolean alliance; //true equals red alliance private boolean autoAim; private double currentEncoderAngle; + private double currentDistance; private Pose2d currentField = new Pose2d(); //center of red speaker: (652.73 218.42) @@ -92,34 +93,31 @@ public void setAngle(double angle){ //check if it works public void setFieldPosition(Pose2d field){ //System.out.println("setting field position"); currentField = field; - } - - public double getAutoAimAngle(double distance){ - double speakerHeight = Units.inchesToMeters(80.51); - //System.out.println("Angle of shooter" + Math.atan(speakerHeight/distance)); - return Math.atan(speakerHeight/distance); - } - - public void printCurrentAngle(){ - System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29); - } - - public double getDistance(){ if(alliance){ //true = red double xLength = Math.pow(currentField.getX()-RED_X, 2); double yLength = Math.pow(currentField.getY()-RED_Y, 2); //System.out.println("alliance red:" + alliance); - return Math.sqrt(xLength + yLength); + currentDistance = Math.sqrt(xLength + yLength); } else { double xLength = Math.pow(currentField.getX()-BLUE_X, 2); double yLength = Math.pow(currentField.getY()-BLUE_Y, 2); - return Math.sqrt(xLength + yLength); + currentDistance = Math.sqrt(xLength + yLength); } } + public double getAutoAimAngle(){ + double speakerHeight = Units.inchesToMeters(80.51); + //System.out.println("Angle of shooter" + Math.atan(speakerHeight/distance)); + return Math.atan(speakerHeight/currentDistance); + } + + public void printCurrentAngle(){ + System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29); + } + public double getPosition(){ //System.out.println("rotation encoder position: " + rotationEncoder.getPosition()); return rotationEncoder.getPosition(); @@ -142,7 +140,7 @@ public void periodic(){ // } if(autoAim){ - setAngle(getAutoAimAngle(getDistance())); + setAngle(getAutoAimAngle()); } // printCurrentAngle(); From 5df109b0a364e016b76c02d6ae605714680a4ad3 Mon Sep 17 00:00:00 2001 From: Pointy Ice Date: Fri, 2 Feb 2024 09:05:49 -0800 Subject: [PATCH 16/25] changed everything related to speaker to trap --- ...orToSpeakerCommand.java => ElevatorToTrapCommand.java} | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) rename src/main/java/frc/robot/commands/elevator/{ElevatorToSpeakerCommand.java => ElevatorToTrapCommand.java} (66%) diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToTrapCommand.java similarity index 66% rename from src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java rename to src/main/java/frc/robot/commands/elevator/ElevatorToTrapCommand.java index 4070d4f6..df8c5ee4 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToTrapCommand.java @@ -4,9 +4,9 @@ import frc.robot.subsystems.elevator.ElevatorState; import frc.robot.subsystems.elevator.ElevatorSubsystem; -public class ElevatorToSpeakerCommand extends Command{ +public class ElevatorToTrapCommand extends Command{ private ElevatorSubsystem elevatorSubsystem; - public ElevatorToSpeakerCommand(ElevatorSubsystem elevatorSubsystem){ + public ElevatorToTrapCommand(ElevatorSubsystem elevatorSubsystem){ this.addRequirements(elevatorSubsystem); this.elevatorSubsystem = elevatorSubsystem; } @@ -18,11 +18,11 @@ public void end(boolean interrupted){ @Override public void execute(){ - this.elevatorSubsystem.setTargetState(ElevatorState.SPEAKER); + this.elevatorSubsystem.setTargetState(ElevatorState.TRAP); } @Override public boolean isFinished(){ - return elevatorSubsystem.atState(ElevatorState.SPEAKER); + return elevatorSubsystem.atState(ElevatorState.TRAP); } } \ No newline at end of file From 7588e91f29dbc85b729429fa49d6b6145836260a Mon Sep 17 00:00:00 2001 From: Pointy Ice Date: Fri, 2 Feb 2024 09:05:58 -0800 Subject: [PATCH 17/25] removed speaker position --- src/main/java/frc/robot/subsystems/elevator/ElevatorState.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index 63ddf6b5..dd150ffb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -6,7 +6,6 @@ public enum ElevatorState { GROUND(Constants.ElevatorConstants.GROUND_POSITION), - SPEAKER(Constants.ElevatorConstants.SPEAKER_POSITION), AMP(Constants.ElevatorConstants.AMP_POSITION), CHUTE(Constants.ElevatorConstants.CHUTE_POSITION), TRAP(Constants.ElevatorConstants.TRAP_POSITION), From de020f60d65a1b2bbae544d1332d1deb04aab2d4 Mon Sep 17 00:00:00 2001 From: Pointy Ice Date: Fri, 2 Feb 2024 09:06:22 -0800 Subject: [PATCH 18/25] removed unnecessaey code --- .../elevator/ElevatorSubsystem.java | 24 ++++--------------- 1 file changed, 5 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index cb09e081..feffc2dc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -22,11 +22,9 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableEvent.Kind; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.ElevatorConstants.*; public class ElevatorSubsystem extends SubsystemBase{ private NetworkTableInstance elevatorNetworkTableInstance; @@ -72,8 +70,8 @@ public ElevatorSubsystem() { this.setTargetState(ElevatorState.GROUND); } else if(message.equals("SPEAKER")){ - System.out.println("Setting Target to SPEAKER"); - this.setTargetState(ElevatorState.SPEAKER); + System.out.println("Setting Target to TRAP"); + this.setTargetState(ElevatorState.TRAP); } else if(message.equals("AMP")){ System.out.println("Setting Target to AMP"); @@ -87,8 +85,8 @@ else if(message.equals("AMP")){ else if(currentTargetState.equals(ElevatorState.GROUND)){ System.out.println("New target state is GROUND!"); } - else if(currentTargetState.equals(ElevatorState.SPEAKER)){ - System.out.println("New target state is SPEAKER!"); + else if(currentTargetState.equals(ElevatorState.TRAP)){ + System.out.println("New target state is TRAP!"); } }); //this entry is working! @@ -135,13 +133,6 @@ public void periodic(){ //extensionMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); //this through overun when no motor connected. - - //Start move to target posision - if (targetState != state){ - - } - - } public boolean atState(ElevatorState state){ @@ -188,9 +179,4 @@ public ElevatorState getTargetState(){ public void setManualPower(double power){ this.manualPower = power; } - - /* private void acceptNewPosition(NetworkTable table, String key, NetworkTableEvent event){ - System.out.println("got networktablex"); - System.out.println(event.valueData.toString()); - } */ -} +} \ No newline at end of file From 639a21755a09e3cad65f1d215f16d2aff23470e7 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Fri, 2 Feb 2024 14:20:36 -0800 Subject: [PATCH 19/25] shooter testing changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 19 ++++++------- .../shooter/pivot/ShooterPivotSetAngle.java | 8 ++++++ .../pivot/ShooterPivotVerticalCommand.java | 13 +++++++-- .../elevator/ElevatorSubsystem.java | 2 +- .../shooter/ShooterPivotSubsystem.java | 28 ++++++++++++++----- 6 files changed, 50 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fbe16664..a9d2a7ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,7 +97,7 @@ public static class ShooterConstants { public static final int SHOOTER_MOTOR_ONE_ID = 13; public static final int SHOOTER_MOTOR_TWO_ID = 14; - public static final int FEEDER_MOTOR_ID = 10; + public static final int FEEDER_MOTOR_ID = 15; } public static class ClimbConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8f61d22c..cdc16478 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,8 @@ import frc.robot.commands.climb.ClimbRaiseCommand; import frc.robot.commands.elevator.ElevatorToAMPCommand; import frc.robot.commands.elevator.ElevatorToGroundCommand; +import frc.robot.commands.shooter.pivot.ShooterPivotSetAngle; +import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; @@ -114,10 +116,10 @@ private void configureBindings() { shooterFeederSubsystem.setFeederMotorSpeed(0); })); - shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); - // pivotSubsystem.printCurrentAngle(); - }, shooterPivotSubsystem)); + // shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { + // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); + // // pivotSubsystem.printCurrentAngle(); + // }, shooterPivotSubsystem)); aButton.onTrue(new InstantCommand(() -> { shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); @@ -137,13 +139,8 @@ private void configureBindings() { } }, intakeRollerSubsystem)); - rightBumper.onTrue(new ElevatorToAMPCommand(elevatorSubsystem)); - leftBumper.onTrue(new ElevatorToGroundCommand(elevatorSubsystem)); - - - - - + rightBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(40))); + leftBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(60))); if(baseSwerveSubsystem instanceof SwerveSubsystem){ final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java index f72aeabf..5c3107de 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java @@ -22,4 +22,12 @@ public void initialize() { public boolean isFinished() { return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE); } + + public void end(boolean interrupted) { + if(interrupted){ + System.out.println("ANGLE INTERRUPTED"); + } else { + System.out.println("ANGLE ARRIVED"); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java index 097d1bbe..ebd6e243 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotVerticalCommand.java @@ -13,11 +13,20 @@ public ShooterPivotVerticalCommand(ShooterPivotSubsystem pivotSubsystem){ @Override public void initialize() { pivotSubsystem.setAutoAimBoolean(false); - pivotSubsystem.setAngle(90.0); + pivotSubsystem.setAngle(0.0); } @Override public boolean isFinished() { - return (Math.abs(pivotSubsystem.getPosition() - 90) < pivotSubsystem.ERRORTOLERANCE); + return (Math.abs(pivotSubsystem.getPosition()) < pivotSubsystem.ERRORTOLERANCE); + } + + @Override + public void end(boolean interrupted) { + if(interrupted){ + System.out.println("VERTICAL INTERRUPTED"); + } else { + System.out.println("VERTICAL ARRIVED"); + } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index feffc2dc..d812fbff 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -117,7 +117,7 @@ else if(currentTargetState.equals(ElevatorState.TRAP)){ public void periodic(){ //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); if(timer.advanceIfElapsed(.2)){ - System.out.println(Units.metersToInches(getExtensionMeters())); + // System.out.println(Units.metersToInches(getExtensionMeters())); } //System.out.println(this.getTargetState()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 897d44cb..7925c5f8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static frc.robot.Constants.ShooterConstants.*; @@ -17,7 +18,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { //final vars public final double PIVOT_SPEED = 0.1; final double GEARBOX_RATIO = 18.16; //ask cadders - public final int ERRORTOLERANCE = 5; //error tolerance for pid + public final double ERRORTOLERANCE = Math.toRadians(2); //error tolerance for pid final int LIMIT_SWITCH_ID = 3; //placeholder final double CONVERSION_FACTOR = Math.PI/(2.*4.57); @@ -30,9 +31,10 @@ public class ShooterPivotSubsystem extends SubsystemBase { private final DigitalInput limitSwitch; //angle PID (CHANGE LATER) - private static final double ANGLE_P = 2.4; - private static final double ANGLE_I = 0; - private static final double ANGLE_D = 0; + private static final double ANGLE_P = 0.5; + private static final double ANGLE_I = 0.001; + private static final double ANGLE_D = 15; + private static final double ANGLE_FF = -.5; //field private boolean alliance; //true equals red alliance @@ -49,8 +51,12 @@ public class ShooterPivotSubsystem extends SubsystemBase { double BLUE_X = Units.inchesToMeters(-1.5+9.05); //9.05 is half of 18.1 which is length of overhang of speaker-- we want halfway point double BLUE_Y = Units.inchesToMeters(218.42); + private final Timer timer = new Timer(); + public ShooterPivotSubsystem(boolean alliance){ + timer.start(); + //motors pivotMotor = new CANSparkMax(PIVOT_MOTOR_ID, MotorType.kBrushless); pivotMotor.setInverted(true); @@ -59,6 +65,7 @@ public ShooterPivotSubsystem(boolean alliance){ rotationEncoder = pivotMotor.getEncoder(); rotationEncoder.setPosition(0); rotationPIDController = pivotMotor.getPIDController(); + rotationPIDController.setOutputRange(-.4, 0.07); limitSwitch = new DigitalInput(LIMIT_SWITCH_ID); @@ -66,6 +73,8 @@ public ShooterPivotSubsystem(boolean alliance){ rotationPIDController.setP(ANGLE_P); rotationPIDController.setI(ANGLE_I); rotationPIDController.setD(ANGLE_D); + rotationPIDController.setFF(0); + System.out.println(rotationPIDController.getFF()); //encoder stuff rotationEncoder.setPositionConversionFactor(CONVERSION_FACTOR); @@ -87,7 +96,8 @@ public void setPivotMotorSpeed(double speed){ public void setAngle(double angle){ //check if it works rotationPIDController.setReference(angle, CANSparkMax.ControlType.kPosition); - //System.out.println("setting angle to: " + angle); + System.out.println("setting angle to: " + angle); + } public void setFieldPosition(Pose2d field){ @@ -115,7 +125,9 @@ public double getAutoAimAngle(){ } public void printCurrentAngle(){ - System.out.println("radians: " + rotationEncoder.getPosition() + "degrees: " + rotationEncoder.getPosition() * 57.29); + System.out.println("radians: " + rotationEncoder.getPosition() + " degrees: " + rotationEncoder.getPosition() * 57.29); + // System.out.println(pivotMotor.get()); + System.out.println(rotationPIDController.getFF()); } public double getPosition(){ @@ -143,7 +155,9 @@ public void periodic(){ setAngle(getAutoAimAngle()); } - // printCurrentAngle(); + if(timer.advanceIfElapsed(.2)){ + printCurrentAngle(); + } // System.out.println("current pos" + rotationEncoder.getPosition()); From 24735a8bcff1597cad4d143bc9c18e4442aba51e Mon Sep 17 00:00:00 2001 From: penguin212 Date: Fri, 2 Feb 2024 20:59:35 -0800 Subject: [PATCH 20/25] swerve update to vortexes --- src/main/java/frc/robot/Constants.java | 10 ++-- src/main/java/frc/robot/RobotContainer.java | 56 +++++++++---------- .../controllers/BaseDriveController.java | 4 ++ .../DualJoystickDriveController.java | 8 +++ .../controllers/XboxDriveController.java | 10 +++- .../shooter/ShooterPivotSubsystem.java | 4 +- .../swerve/SingleModuleSwerveSubsystem.java | 18 ++++-- .../robot/subsystems/swerve/SwerveModule.java | 35 +++++++----- .../subsystems/swerve/SwerveSubsystem.java | 11 ++-- .../TestSingleModuleSwerveSubsystem.java | 11 +--- .../swerve/drivemotors/FalconDriveMotor.java | 2 +- .../swerve/drivemotors/SwerveDriveMotor.java | 2 +- .../swerve/drivemotors/VortexDriveMotor.java | 23 +++++--- src/main/java/frc/robot/util/MotorUtil.java | 10 ++++ src/main/java/frc/robot/util/Util.java | 7 +++ 15 files changed, 133 insertions(+), 78 deletions(-) create mode 100644 src/main/java/frc/robot/util/Util.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a9d2a7ae..0ef38914 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -48,21 +48,21 @@ public static class TestSingleModuleSwerveConstants { } public static class SwerveConstants { - public static final int FL_DRIVE = 0; + public static final int FL_DRIVE = 20; public static final int FL_STEER = 1; - public static final double FL_OFFSET = 3.34 - Math.PI / 4; + public static final double FL_OFFSET = 3.36 - 1. * Math.PI / 4.; public static final int FR_DRIVE = 2; public static final int FR_STEER = 3; - public static final double FR_OFFSET = 2.94 - Math.PI * 3 / 4; + public static final double FR_OFFSET = 3.02 - Math.PI * 3 / 4; public static final int BL_DRIVE = 4; public static final int BL_STEER = 5; - public static final double BL_OFFSET = 3.14 + Math.PI / 4; + public static final double BL_OFFSET = 2.83 + Math.PI / 4; public static final int BR_DRIVE = 6; public static final int BR_STEER = 7; - public static final double BR_OFFSET = 2.43 + Math.PI * 3.0 / 4.0; + public static final double BR_OFFSET = 2.66 + Math.PI * 3.0 / 4.0; private static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0); public static final Translation2d FL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdc16478..bd9f936e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - private final BaseDriveController driveController = new DualJoystickDriveController(); + private final BaseDriveController driveController = new XboxDriveController(); private final BaseSwerveSubsystem baseSwerveSubsystem; private final IntakePivotSubsystem intakePivotSubsystem; @@ -72,23 +72,23 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { //construct Test - // module = new SwerveModule(6, 7, 0); + // module = new SwerveModule(6, 7, 0, true); // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); - baseSwerveSubsystem = null;// new SwerveSubsystem(); - intakePivotSubsystem = new IntakePivotSubsystem(); - shooterFeederSubsystem = new ShooterFeederSubsystem(); + baseSwerveSubsystem = new SwerveSubsystem(); + intakePivotSubsystem = new IntakePivotSubsystem(); + shooterFeederSubsystem = new ShooterFeederSubsystem(); - shooterPivotSubsystem = new ShooterPivotSubsystem(false); - shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); + shooterPivotSubsystem = new ShooterPivotSubsystem(false); + shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); - climbSubsystem = new ClimbSubsystem(); + climbSubsystem = new ClimbSubsystem(); - elevatorSubsystem = new ElevatorSubsystem(); + elevatorSubsystem = new ElevatorSubsystem(); - traj = Choreo.getTrajectory("Curve"); + traj = Choreo.getTrajectory("Curve"); - // Configure the trigger bindings - configureBindings(); + // Configure the trigger bindings + configureBindings(); } @@ -158,22 +158,22 @@ private void configureBindings() { } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; - // LBumper.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.decrementTest(); - // System.out.println(testSwerveSubsystem.getTest()); - // } - // )); - - // RBumper.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.incrementTest(); - // System.out.println(testSwerveSubsystem.getTest()); - // } - // )); - - // AButton.onTrue(new InstantCommand(() -> { - // testSwerveSubsystem.toggletoRun(); - // System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); - // })); + driveController.getLeftBumper().onTrue(new InstantCommand(() -> { + testSwerveSubsystem.decrementTest(); + System.out.println(testSwerveSubsystem.getTest()); + } + )); + + driveController.getRightBumper().onTrue(new InstantCommand(() -> { + testSwerveSubsystem.incrementTest(); + System.out.println(testSwerveSubsystem.getTest()); + } + )); + + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + testSwerveSubsystem.toggletoRun(); + System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running"); + })); } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java index 062fc2ca..e7a7b405 100644 --- a/src/main/java/frc/robot/controllers/BaseDriveController.java +++ b/src/main/java/frc/robot/controllers/BaseDriveController.java @@ -22,4 +22,8 @@ public abstract class BaseDriveController { public abstract double getRotatePower(); public abstract JoystickButton getFieldResetButton(); + + public abstract JoystickButton getLeftBumper(); + + public abstract JoystickButton getRightBumper(); } diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java index 8c58e32b..2f3ef506 100644 --- a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java +++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java @@ -68,5 +68,13 @@ private double getTurnScaling() { public JoystickButton getFieldResetButton() { return right2; } + + public JoystickButton getRightBumper(){ + return rightMiddleRightButton; + } + + public JoystickButton getLeftBumper(){ + return rightMiddleLeftButton; + } } diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java index 5a4a4ccc..eefd3034 100644 --- a/src/main/java/frc/robot/controllers/XboxDriveController.java +++ b/src/main/java/frc/robot/controllers/XboxDriveController.java @@ -27,7 +27,7 @@ public double getForwardPower() { @Override public double getLeftPower() { - return -driveController.getLeftX(); + return driveController.getLeftX(); } @Override @@ -38,4 +38,12 @@ public double getRotatePower() { public JoystickButton getFieldResetButton() { return driveAButton; } + + public JoystickButton getLeftBumper() { + return driveLBumper; + } + + public JoystickButton getRightBumper() { + return driveRBumper; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 7925c5f8..75d8e8b9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -125,9 +125,9 @@ public double getAutoAimAngle(){ } public void printCurrentAngle(){ - System.out.println("radians: " + rotationEncoder.getPosition() + " degrees: " + rotationEncoder.getPosition() * 57.29); + // System.out.println("radians: " + rotationEncoder.getPosition() + " degrees: " + rotationEncoder.getPosition() * 57.29); // System.out.println(pivotMotor.get()); - System.out.println(rotationPIDController.getFF()); + // System.out.println(rotationPIDController.getFF()); } public double getPosition(){ diff --git a/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java index 816289a5..d207f486 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; +import frc.robot.util.Util; public class SingleModuleSwerveSubsystem extends BaseSwerveSubsystem{ @@ -33,13 +34,15 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){ public void setDrivePowers(double xPower, double yPower){ double velocity = MAX_VEL * Math.sqrt(yPower * yPower + xPower * xPower) / Math.sqrt(2); - if(Math.abs(xPower) < .01 && Math.abs(yPower) < .01 ){ - System.out.println(Math.atan2(0, 0)); - module.setRawPowers(0, 0); - return; - } + // if(Math.abs(xPower) < .01 && Math.abs(yPower) < .01 ){ + // // System.out.println(Math.atan2(0, 0)); + // module.setRawPowers(0, 0); + // return; + // } double angle = Math.atan2(yPower, xPower); + // System.out.println(velocity); + module.setDesiredState(new SwerveModuleState(velocity, new Rotation2d(angle))); } @@ -73,5 +76,10 @@ public void toggletoRun(){ System.out.println(toRun); } + @Override + public void periodic() { + System.out.println("error " + Util.twoDecimals(module.getDriveError()) + " setpoint" + Util.twoDecimals(module.getDriveSetpoint())); + } + } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 6a814ca3..794e573d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.swerve.drivemotors.SwerveDriveMotor; import frc.robot.subsystems.swerve.drivemotors.VortexDriveMotor; import frc.robot.util.MotorUtil; +import frc.robot.util.Util; public class SwerveModule { private final SwerveDriveMotor driveMotor; @@ -32,8 +33,8 @@ public class SwerveModule { private boolean verbose; - private static final double DRIVE_METERS_PER_ROTATION = (13.0 / 90.0) * Math.PI * Units.inchesToMeters(4.0); - private static final double DRIVE_ROTATIONS_PER_METER = 1.0 / DRIVE_METERS_PER_ROTATION; + private static final double DRIVE_METERS_PER_ROTATION = (13.0 / 90.0) * Math.PI * Units.inchesToMeters(4.0); // .0461 + private static final double DRIVE_ROTATIONS_PER_METER = 1.0 / DRIVE_METERS_PER_ROTATION; // 21.69 private static final double STEER_ROTATIONS_PER_RADIAN = (130.0 / 1776.0) * 2.0 * Math.PI; // Useful for steer relative encoder if we ever use that private static final double STEER_VOLTS_RADIANS = 2 * Math.PI / 3.3 ; // https://docs.revrobotics.com/sparkmax/feature-description/data-port#analog-input //The encoder board maps the 5V output of the encoder to 3.3V of the Spark Max @@ -47,7 +48,7 @@ public class SwerveModule { private static final double VORTEX_DRIVE_P = 0; private static final double VORTEX_DRIVE_I = 0; private static final double VORTEX_DRIVE_D = 0; - private static final double VORTEX_DRIVE_FF = 0; + private static final double VORTEX_DRIVE_FF = .22591262 * 3.6 / 4; // rotations/m * max vel private static final double STEER_P = .68; // .7 private static final double STEER_I = 0; // 0 @@ -64,17 +65,17 @@ public class SwerveModule { * @param drivePort The CAN ID of the drive motor * @param steerPort The CAN ID of the steer motor * @param offsetRads The offset of the absolute encoder - * @param falcon Whether this is a falcon or not + * @param vortex Whether this is a falcon or not */ - public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean falcon) { + public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean vortex) { - driveMotor = falcon ? new FalconDriveMotor(drivePort) : new VortexDriveMotor(drivePort); + driveMotor = vortex ? new VortexDriveMotor(drivePort) : new FalconDriveMotor(drivePort); - if(falcon){ - driveMotor.configPID(FALCON_DRIVE_P, FALCON_DRIVE_I, FALCON_DRIVE_D, FALCON_DRIVE_FF); - } else { + if(vortex){ driveMotor.configPID(VORTEX_DRIVE_P, VORTEX_DRIVE_I, VORTEX_DRIVE_D, VORTEX_DRIVE_FF); + } else { + driveMotor.configPID(FALCON_DRIVE_P, FALCON_DRIVE_I, FALCON_DRIVE_D, FALCON_DRIVE_FF); } // untested for vortexes @@ -159,7 +160,7 @@ public void setDesiredState(SwerveModuleState state) { public void setVerbose(){ if (crimor.advanceIfElapsed(.1)){ - System.out.println(" current " + twoDecimals(getWrappedAngle().getDegrees())); + System.out.println(" current " + Util.twoDecimals(getWrappedAngle().getDegrees())); // System.out.println(" target " + twoDecimals(Math.toDegrees(MathUtil.angleModulus(targetAngleRads)))); // System.out.print(" error " + twoDecimals(driveMotor.getError())); // System.out.println(" target " + twoDecimals(driveMotor.getSetPoint())); @@ -190,7 +191,7 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){ double targetAngleRads = angleRads - offsetRads; driveMotor.setPower(drivePower); - System.out.println(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees())); + // System.out.println(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees())); //System.out.print("target " + new Rotation2d(targetAngleRads).getDegrees() + "--------"); // System.out.print("error " + (angleRads.minus(currentAngle).getRadians()) + "--------"); @@ -228,10 +229,6 @@ public Rotation2d getRawAngle(){ return new Rotation2d(steerAbsoluteEncoder.getPosition()); } - public double twoDecimals(double num){ - return ((int) (num * 100)) / 100.d; - } - public double getDriveAmpDraws(){ return driveMotor.getAmpDraw(); } @@ -239,4 +236,12 @@ public double getDriveAmpDraws(){ public double getSteerAmpDraws(){ return steerMotor.getOutputCurrent(); } + + public double getDriveError(){ + return driveMotor.getError(); + } + + public double getDriveSetpoint(){ + return driveMotor.getSetpoint(); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 9c2819ab..35a08763 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -77,10 +77,10 @@ public class SwerveSubsystem extends BaseSwerveSubsystem{ public SwerveSubsystem() { ahrs = new AHRS(SPI.Port.kMXP); - frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET); - frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET); - backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET); - backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET); + frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET, true); + frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET, true); + backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET, true); + backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET, true); kinematics = new SwerveDriveKinematics(FL_POS, FR_POS, BL_POS, BR_POS); @@ -120,6 +120,8 @@ public SwerveSubsystem() { } public void periodic() { + + // System.out.println(frontLeftModule.getDriveSetpoint()); // if (crimer.advanceIfElapsed(.1)){ // //System.out.println("BR : " + backRightModule.getRawAngle()); @@ -155,6 +157,7 @@ public void periodic() { frontRightModule.setDesiredState(states[1]); backLeftModule.setDesiredState(states[2]); backRightModule.setDesiredState(states[3]); + } public void setDrivePowers(double xPower, double yPower, double angularPower){ diff --git a/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java index ec2fd86e..29f4b41a 100644 --- a/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java @@ -5,12 +5,13 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import frc.robot.util.Util; public class TestSingleModuleSwerveSubsystem extends SingleModuleSwerveSubsystem { public static final double STEER_POWER = .4; - public static final double DRIVE_POWER = .3; + public static final double DRIVE_POWER = 1; private int testCase; private boolean toRun; @@ -138,12 +139,8 @@ public void periodic() { super.setRawPowersWithAngle(drive, steer); } else{ - System.out.println(drive); super.setRawPowers(drive, steer); } - - - // System.out.println(testCase); } public void incrementTest(){ @@ -164,10 +161,6 @@ public boolean getRunning(){ return toRun; } - public double twoDecimals(double num){ - return ((int) (num * 100)) / 100.d; - } - public void toggletoRun(){ toRun = !toRun; System.out.println(toRun); diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java index 79ba21bd..ecc5bca4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java @@ -60,7 +60,7 @@ public double getError(){ return motor.getClosedLoopError().getValue(); } - public double getSetPoint(){ + public double getSetpoint(){ return targetRps; } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java index 9631df0d..b44ae973 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java @@ -18,7 +18,7 @@ public interface SwerveDriveMotor { public double getError(); - public double getSetPoint(); + public double getSetpoint(); public double getAmpDraw(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java index 16fcca62..d8b4d174 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.swerve.drivemotors; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; @@ -11,21 +12,28 @@ public class VortexDriveMotor implements SwerveDriveMotor { - private CANSparkMax motor; + private CANSparkFlex motor; private RelativeEncoder encoder; private SparkPIDController pidController; + private double conversionFactor = 1; + + private double lastReference; public VortexDriveMotor (int port){ - motor = new CANSparkMax(port, MotorType.kBrushless); + motor = new CANSparkFlex(port, MotorType.kBrushless); motor.setIdleMode(IdleMode.kBrake); encoder = motor.getEncoder(); encoder.setVelocityConversionFactor(1); //STUB - pidController = MotorUtil.createSparkPIDController(motor, encoder); + pidController = motor.getPIDController(); + + // pidController = MotorUtil.createSparkPIDController(motor, encoder); } public void setVelocity(double velocity){ + lastReference = velocity; + // motor.set(velocity / 4.9); pidController.setReference(velocity, ControlType.kVelocity); } @@ -50,6 +58,7 @@ public double getVelocity(){ public void setVelocityConversionFactor(double factor){ encoder.setVelocityConversionFactor(factor); + conversionFactor = factor; } public void setPositionConversionFactor(double factor){ @@ -57,14 +66,14 @@ public void setPositionConversionFactor(double factor){ } public double getError(){ - return 0; //STUB + return (encoder.getVelocity() * 4.90245766303 / 8870000) - getSetpoint(); } - public double getSetPoint(){ - return 0; //STUB + public double getSetpoint(){ + return lastReference; //STUB } public double getAmpDraw(){ - return 0; //STUB + return motor.getOutputCurrent(); //STUB } } diff --git a/src/main/java/frc/robot/util/MotorUtil.java b/src/main/java/frc/robot/util/MotorUtil.java index ce8d3be0..5a50972a 100644 --- a/src/main/java/frc/robot/util/MotorUtil.java +++ b/src/main/java/frc/robot/util/MotorUtil.java @@ -2,6 +2,7 @@ import java.util.function.Consumer; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; @@ -96,6 +97,15 @@ public static SparkPIDController createSparkPIDController(CANSparkMax spark, Mot return pidController; } + public static SparkPIDController createSparkPIDController(CANSparkFlex spark, MotorFeedbackSensor encoder) { + SparkPIDController pidController = spark.getPIDController(); + + // Set feedback device + checkError(spark.getDeviceId(), pidController.setFeedbackDevice(encoder), "PID feedback device"); + + return pidController; + } + /** * Checks a CANSparkMax configuration call for an error, reporting it if it exists. * @param id The CAN ID of the SparkMax. diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java new file mode 100644 index 00000000..1971a554 --- /dev/null +++ b/src/main/java/frc/robot/util/Util.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public class Util { + public static double twoDecimals(double num){ + return ((int) (num * 100)) / 100.d; + } +} From 1ffe51f5c2fd98c712d71d15be93db038b0b778b Mon Sep 17 00:00:00 2001 From: penguin212 Date: Sun, 4 Feb 2024 01:39:27 -0800 Subject: [PATCH 21/25] mech bindings, command sequences, sensor tuning --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 77 ++++++++----------- .../java/frc/robot/commands/IdleCommand.java | 37 +++++++++ .../frc/robot/commands/ShootModeCommand.java | 20 ----- .../commands/sequences/ShootModeSequence.java | 54 +++++++++++++ .../shooter/feed/ShooterFeedLoadCommand.java | 2 +- .../shooter/feed/ShooterFeedShootCommand.java | 2 +- .../flywheel/ShooterFlywheelReadyCommand.java | 5 ++ ...s.java => ShooterFlywheelStopCommand.java} | 4 +- ....java => ShooterPivotSetAngleCommand.java} | 4 +- .../intake/IntakeRollersSubsystem.java | 2 +- .../shooter/ShooterFeederSubsystem.java | 8 +- .../subsystems/superstructure/RobotState.java | 14 ++++ .../SuperstructureSubsystem.java | 7 ++ .../robot/util/ConditionalWaitCommand.java | 18 +++++ 15 files changed, 179 insertions(+), 79 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IdleCommand.java delete mode 100644 src/main/java/frc/robot/commands/ShootModeCommand.java create mode 100644 src/main/java/frc/robot/commands/sequences/ShootModeSequence.java rename src/main/java/frc/robot/commands/shooter/flywheel/{ShooterFlywheelStopCommands.java => ShooterFlywheelStopCommand.java} (77%) rename src/main/java/frc/robot/commands/shooter/pivot/{ShooterPivotSetAngle.java => ShooterPivotSetAngleCommand.java} (84%) create mode 100644 src/main/java/frc/robot/subsystems/superstructure/RobotState.java create mode 100644 src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java create mode 100644 src/main/java/frc/robot/util/ConditionalWaitCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0ef38914..d9f353f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -84,7 +84,7 @@ public static class IntakeConstants { public static final double rollersclockwise = 1; public static final double rollerscounterclockwise = -1; - public static final double sensorreached = 1; + public static final double sensorreached = .3; public static final double pivotclockwise = 1; public static final double pivotcounterclockwise = -1; public static final double pastsensortime = 0.5; @@ -98,6 +98,8 @@ public static class ShooterConstants { public static final int SHOOTER_MOTOR_TWO_ID = 14; public static final int FEEDER_MOTOR_ID = 15; + + public static final double FEED_ANGLE = Units.degreesToRadians(70); } public static class ClimbConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bd9f936e..d4000283 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,11 +13,18 @@ import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.XboxDriveController; +import frc.robot.commands.IdleCommand; import frc.robot.commands.climb.ClimbLowerCommand; import frc.robot.commands.climb.ClimbRaiseCommand; import frc.robot.commands.elevator.ElevatorToAMPCommand; import frc.robot.commands.elevator.ElevatorToGroundCommand; -import frc.robot.commands.shooter.pivot.ShooterPivotSetAngle; +import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; +import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; +import frc.robot.commands.intake.roller.IntakeRollerOutakeCommand; +import frc.robot.commands.sequences.ShootModeSequence; +import frc.robot.commands.shooter.feed.ShooterFeedLoadCommand; +import frc.robot.commands.shooter.feed.ShooterFeedShootCommand; +import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand; import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; @@ -26,6 +33,8 @@ import frc.robot.subsystems.swerve.SwerveModule; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.TestSingleModuleSwerveSubsystem; +import frc.robot.util.ConditionalWaitCommand; + import java.util.function.BooleanSupplier; import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoTrajectory; @@ -36,6 +45,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import static frc.robot.Constants.SwerveConstants.*; @@ -93,54 +104,27 @@ public RobotContainer() { private void configureBindings() { + aButton.onTrue(new IntakeRollerIntakeCommand(intakeRollerSubsystem)); - elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { - - elevatorSubsystem.setManualPower(mechController.getRightTriggerAxis()-mechController.getLeftTriggerAxis()); - - }, elevatorSubsystem)); + bButton.onTrue(new IdleCommand(intakePivotSubsystem, intakeRollerSubsystem, + elevatorSubsystem, + shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, + climbSubsystem + )); - bButton.onTrue(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(.4); - })); - - bButton.onFalse(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(0); - })); + leftBumper.onTrue(new ShootModeSequence(intakeRollerSubsystem, + elevatorSubsystem, + shooterFeederSubsystem, shooterFlywheelSubsystem, shooterPivotSubsystem + ).andThen( + new ConditionalWaitCommand(() -> mechController.getRightTriggerAxis() > .1).andThen( + new ShooterFeedShootCommand(shooterFeederSubsystem) + ))); - xButton.onTrue(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(-.7); - })); - - xButton.onFalse(new InstantCommand(() -> { - shooterFeederSubsystem.setFeederMotorSpeed(0); - })); - - // shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - // shooterPivotSubsystem.setPivotMotorSpeed((.2 * mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); - // // pivotSubsystem.printCurrentAngle(); - // }, shooterPivotSubsystem)); - - aButton.onTrue(new InstantCommand(() -> { - shooterFlywheelSubsystem.setShooterMotorSpeed(shooterFlywheelSubsystem.SHOOTER_MOTOR_SPEED); - })); - - aButton.onFalse(new InstantCommand(() -> { - shooterFlywheelSubsystem.setShooterMotorSpeed(0); - })); - - intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> { - if(mechController.getPOV() == 90){ - intakeRollerSubsystem.setAllRollSpeed(1, .8); - } else if (mechController.getPOV() == 270){ - intakeRollerSubsystem.setAllRollSpeed(-.8, -.5); - } else { - intakeRollerSubsystem.setAllRollSpeed(0, 0); - } - }, intakeRollerSubsystem)); + rightBumper.onTrue(new ElevatorToAMPCommand(elevatorSubsystem).andThen( + new ConditionalWaitCommand(() -> mechController.getRightTriggerAxis() > .1).andThen( + new IntakeRollerOutakeCommand(intakeRollerSubsystem) + ))); - rightBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(40))); - leftBumper.onTrue(new ShooterPivotSetAngle(shooterPivotSubsystem, Math.toRadians(60))); if(baseSwerveSubsystem instanceof SwerveSubsystem){ final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; @@ -148,8 +132,7 @@ private void configureBindings() { swerveSubsystem.setDefaultCommand(new RunCommand(() -> { swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); - } - , swerveSubsystem)); + }, swerveSubsystem)); driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { swerveSubsystem.resetDriverHeading(); diff --git a/src/main/java/frc/robot/commands/IdleCommand.java b/src/main/java/frc/robot/commands/IdleCommand.java new file mode 100644 index 00000000..42de2c05 --- /dev/null +++ b/src/main/java/frc/robot/commands/IdleCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.commands.climb.ClimbLowerCommand; +import frc.robot.commands.elevator.ElevatorToGroundCommand; +import frc.robot.commands.shooter.flywheel.ShooterFlywheelStopCommand; +import frc.robot.subsystems.climb.ClimbSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.intake.IntakePivotSubsystem; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; + +public class IdleCommand extends ParallelCommandGroup{ + public IdleCommand(IntakePivotSubsystem intakePivotSubsystem, + IntakeRollersSubsystem intakeRollersSubsystem, + ElevatorSubsystem elevatorSubsystem, + ShooterPivotSubsystem shooterPivotSubsystem, + ShooterFeederSubsystem shooterFeederSubsystem, + ShooterFlywheelSubsystem shooterFlywheelSubsystem, + ClimbSubsystem climbSubsystem){ + addRequirements(intakePivotSubsystem, intakeRollersSubsystem, + elevatorSubsystem, + shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, + climbSubsystem + ); + + addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0)), + new ElevatorToGroundCommand(elevatorSubsystem), + new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0)), + new ShooterFlywheelStopCommand(shooterFlywheelSubsystem)//, + // new ClimbLowerCommand(climbSubsystem) // NOT USING CLIMB FOR NOW + ); + } +} diff --git a/src/main/java/frc/robot/commands/ShootModeCommand.java b/src/main/java/frc/robot/commands/ShootModeCommand.java deleted file mode 100644 index 901fa8f9..00000000 --- a/src/main/java/frc/robot/commands/ShootModeCommand.java +++ /dev/null @@ -1,20 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.intake.pivot.IntakePivotVerticalCommand; -import frc.robot.commands.shooter.feed.ShooterFeedShootCommand; -import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand; -import frc.robot.subsystems.intake.IntakePivotSubsystem; -import frc.robot.subsystems.shooter.ShooterFeederSubsystem; -import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; - -public class ShootModeCommand extends SequentialCommandGroup { - IntakePivotSubsystem intakePivotSubsystem; - ShooterFlywheelSubsystem shooterFlywheelSubsystem; - ShooterFeederSubsystem shooterFeederSubsystem; - - public ShootModeCommand(){ - addCommands(new IntakePivotVerticalCommand(intakePivotSubsystem).alongWith(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem)), - new ShooterFeedShootCommand(shooterFeederSubsystem)); - } -} diff --git a/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java b/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java new file mode 100644 index 00000000..e133c8f7 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java @@ -0,0 +1,54 @@ +package frc.robot.commands.sequences; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.elevator.ElevatorToGroundCommand; +import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; +import frc.robot.commands.shooter.feed.ShooterFeedLoadCommand; +import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand; +import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.subsystems.shooter.ShooterFeederSubsystem; +import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; +import frc.robot.subsystems.shooter.ShooterPivotSubsystem; +import static frc.robot.Constants.ShooterConstants.*; + +public class ShootModeSequence extends SequentialCommandGroup { + + /* + * START + * / | `---------. + * Angle shooter lower Start Flywheels + * for intaking elevator | + * \ / | + * | | + * / \ | + * Intake feed Shooter recieve | + * to shooter Note | + * \ / / + * Aim shooter / + * \ / + * FINISH + */ + + public ShootModeSequence( + IntakeRollersSubsystem intakeRollerSubsystem, + ElevatorSubsystem elevatorSubsystem, + ShooterFeederSubsystem shooterFeederSubsystem, + ShooterFlywheelSubsystem shooterFlywheelSubsystem, + ShooterPivotSubsystem shooterPivotSubsystem + ){ + addCommands(new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem).alongWith( + new SequentialCommandGroup( + new ShooterPivotSetAngleCommand(shooterPivotSubsystem, FEED_ANGLE).alongWith( + new ElevatorToGroundCommand(elevatorSubsystem) + ), + new ParallelDeadlineGroup(new ShooterFeedLoadCommand(shooterFeederSubsystem), + new IntakeRollerFeedCommand(intakeRollerSubsystem)), + new ShooterPivotSetAngleCommand(shooterPivotSubsystem, Units.degreesToRadians(20)) //STUB FOR AUTOAIM + ) + )); + } +} diff --git a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java index 5c4f0665..66812492 100644 --- a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java +++ b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedLoadCommand.java @@ -19,7 +19,7 @@ public void initialize() { @Override public boolean isFinished() { - if(feederSubsystem.getProximity() > feederSubsystem.TOLERANCE){ + if(feederSubsystem.getRed() > feederSubsystem.TOLERANCE){ return true; } return false; diff --git a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java index cf0fe23a..98db7b9f 100644 --- a/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java +++ b/src/main/java/frc/robot/commands/shooter/feed/ShooterFeedShootCommand.java @@ -16,7 +16,7 @@ public void initialize() { @Override public boolean isFinished() { - if(feederSubsystem.getProximity() < feederSubsystem.NO_NOTE_TOLERANCE){ + if(feederSubsystem.getRed() < feederSubsystem.NO_NOTE_TOLERANCE){ return true; } return false; diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java index 06545d36..76c41e9d 100644 --- a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java @@ -16,6 +16,11 @@ public void initialize() { shooterSubsystem.setShooterMotorSpeed(shooterSubsystem.SHOOTER_MOTOR_SPEED); } + @Override + public boolean isFinished() { + return true; //STUB + } + //pivot: vertical, auto-aim //feed: load, shoot //shooter: stop, ready shooters diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommand.java similarity index 77% rename from src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java rename to src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommand.java index 4f083141..6c4e4ec9 100644 --- a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommands.java +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelStopCommand.java @@ -2,10 +2,10 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; -public class ShooterFlywheelStopCommands extends Command{ +public class ShooterFlywheelStopCommand extends Command{ ShooterFlywheelSubsystem shooterSubsystem; - public ShooterFlywheelStopCommands(ShooterFlywheelSubsystem shooterSubsystem){ + public ShooterFlywheelStopCommand(ShooterFlywheelSubsystem shooterSubsystem){ this.shooterSubsystem = shooterSubsystem; addRequirements(shooterSubsystem); } diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java similarity index 84% rename from src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java rename to src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java index 5c3107de..49e55c84 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngle.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java @@ -2,11 +2,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.shooter.ShooterPivotSubsystem; -public class ShooterPivotSetAngle extends Command{ +public class ShooterPivotSetAngleCommand extends Command{ ShooterPivotSubsystem pivotSubsystem; double angle; - public ShooterPivotSetAngle(ShooterPivotSubsystem pivotSubsystem, double angle){ + public ShooterPivotSetAngleCommand(ShooterPivotSubsystem pivotSubsystem, double angle){ this.pivotSubsystem = pivotSubsystem; addRequirements(pivotSubsystem); this.angle = angle; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java index 2f2f7c82..c8e505b6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -30,7 +30,7 @@ public IntakeRollersSubsystem() { } public boolean sensorNow(){ - if (sensor.get()<=sensorreached){ + if (sensor.get()>=sensorreached){ return true; } else{ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java index a7f6527e..3c1e3841 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -11,8 +11,8 @@ public class ShooterFeederSubsystem extends SubsystemBase{ //finals public final double FEEDER_MOTOR_SPEED = 0.1; - public final int NO_NOTE_TOLERANCE = 10; //must test with no note in front of sensor - public final int TOLERANCE = 10; //represents the value when half note is in front of sensor + public final int NO_NOTE_TOLERANCE = 500; //must test with no note in front of sensor + public final int TOLERANCE = 7000; //represents the value when half note is in front of sensor //motors private final TalonSRX feederMotor; @@ -35,9 +35,9 @@ public void setFeederMotorSpeed(double speed){ } - public int getProximity(){ + public int getRed(){ System.out.println("proximity: " + shooterSensor.getProximity()); - return shooterSensor.getProximity(); + return shooterSensor.getRed(); } public ColorSensorV3 getSensor(){ diff --git a/src/main/java/frc/robot/subsystems/superstructure/RobotState.java b/src/main/java/frc/robot/subsystems/superstructure/RobotState.java new file mode 100644 index 00000000..c0c68c87 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/RobotState.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.superstructure; + +public enum RobotState { + INTAKE(), + AMP(), + SHOOT(), + CLIMB(), + IDLE(); + + + private RobotState(){ + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java new file mode 100644 index 00000000..f15bcdf2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureSubsystem.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.superstructure; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SuperstructureSubsystem extends SubsystemBase { + +} diff --git a/src/main/java/frc/robot/util/ConditionalWaitCommand.java b/src/main/java/frc/robot/util/ConditionalWaitCommand.java new file mode 100644 index 00000000..fd5c3b50 --- /dev/null +++ b/src/main/java/frc/robot/util/ConditionalWaitCommand.java @@ -0,0 +1,18 @@ +package frc.robot.util; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ConditionalWaitCommand extends Command{ + BooleanSupplier op; + + public ConditionalWaitCommand(BooleanSupplier condition){ + op = condition; + } + + @Override + public boolean isFinished() { + return op.getAsBoolean(); + } +} From 7cc48a4b4fea05d0bcc59a6c85120dfe1c8d73c1 Mon Sep 17 00:00:00 2001 From: penguin212 Date: Mon, 5 Feb 2024 17:14:39 -0800 Subject: [PATCH 22/25] testing chganges --- .../choreo/{2X1MCurve.traj => Curve.traj} | 0 src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++++++++-- .../java/frc/robot/commands/IdleCommand.java | 14 +++++++------- .../elevator/ElevatorToGroundCommand.java | 1 + .../intake/roller/IntakeRollerFeedCommand.java | 3 ++- .../roller/IntakeRollerIntakeCommand.java | 1 + .../roller/IntakeRollerOutakeCommand.java | 4 +++- .../commands/sequences/ShootModeSequence.java | 2 +- .../pivot/ShooterPivotSetAngleCommand.java | 2 ++ .../intake/IntakeRollersSubsystem.java | 1 + .../shooter/ShooterFeederSubsystem.java | 11 ++++++++--- .../subsystems/swerve/SwerveSubsystem.java | 2 +- 13 files changed, 45 insertions(+), 19 deletions(-) rename src/main/deploy/choreo/{2X1MCurve.traj => Curve.traj} (100%) diff --git a/src/main/deploy/choreo/2X1MCurve.traj b/src/main/deploy/choreo/Curve.traj similarity index 100% rename from src/main/deploy/choreo/2X1MCurve.traj rename to src/main/deploy/choreo/Curve.traj diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d9f353f0..365fcd64 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,14 +27,14 @@ public static class ElevatorConstants { public static final double GROUND_POSITION = 0; public static final double SPEAKER_POSITION = 0; public static final double AMP_POSITION = Units.inchesToMeters(28); - public static final double CHUTE_POSITION = Units.inchesToMeters(25); + public static final double CHUTE_POSITION = Units.inchesToMeters(1); public static final double TRAP_POSITION = Units.inchesToMeters(30); public static final double START_POSITION = 0; public static final double EXTENSION_P= 3.5; public static final double EXTENSION_I= 0; public static final double EXTENSION_D= 0; - public static final double EXTENSION_TOLERANCE= 0.003; + public static final double EXTENSION_TOLERANCE= 0.3; public static final double POSITION_CONVERSION_FACTOR = Units.inchesToMeters(30.)/63.5; public static final double VELOCITY_CONVERSION_FACTOR = 1; @@ -83,7 +83,7 @@ public static class IntakeConstants { //public static final int intakeencoderID = 3; public static final double rollersclockwise = 1; - public static final double rollerscounterclockwise = -1; + public static final double rollerscounterclockwise = 1; public static final double sensorreached = .3; public static final double pivotclockwise = 1; public static final double pivotcounterclockwise = -1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4000283..2988a504 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import frc.robot.commands.climb.ClimbLowerCommand; import frc.robot.commands.climb.ClimbRaiseCommand; import frc.robot.commands.elevator.ElevatorToAMPCommand; +import frc.robot.commands.elevator.ElevatorToChuteCommand; import frc.robot.commands.elevator.ElevatorToGroundCommand; import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; @@ -51,9 +52,11 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import static frc.robot.Constants.SwerveConstants.*; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.UsbCamera; public class RobotContainer { // The robot's subsystems and commands are defined here... - private final BaseDriveController driveController = new XboxDriveController(); + private final BaseDriveController driveController = new DualJoystickDriveController(); private final BaseSwerveSubsystem baseSwerveSubsystem; private final IntakePivotSubsystem intakePivotSubsystem; @@ -75,6 +78,8 @@ public class RobotContainer { private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value); + private UsbCamera camera1; + private MjpegServer mjpgserver1; //private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); ChoreoTrajectory traj; @@ -100,11 +105,19 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); + + camera1 = new UsbCamera("camera1", 0); + camera1.setFPS(60); + camera1.setBrightness(45); + camera1.setResolution(176, 144); + mjpgserver1 = new MjpegServer("m1", 1181); + mjpgserver1.setSource(camera1); } private void configureBindings() { - aButton.onTrue(new IntakeRollerIntakeCommand(intakeRollerSubsystem)); + aButton.onTrue(new ElevatorToChuteCommand(elevatorSubsystem).andThen( + new IntakeRollerIntakeCommand(intakeRollerSubsystem))); bButton.onTrue(new IdleCommand(intakePivotSubsystem, intakeRollerSubsystem, elevatorSubsystem, diff --git a/src/main/java/frc/robot/commands/IdleCommand.java b/src/main/java/frc/robot/commands/IdleCommand.java index 42de2c05..adca8045 100644 --- a/src/main/java/frc/robot/commands/IdleCommand.java +++ b/src/main/java/frc/robot/commands/IdleCommand.java @@ -21,15 +21,15 @@ public IdleCommand(IntakePivotSubsystem intakePivotSubsystem, ShooterFeederSubsystem shooterFeederSubsystem, ShooterFlywheelSubsystem shooterFlywheelSubsystem, ClimbSubsystem climbSubsystem){ - addRequirements(intakePivotSubsystem, intakeRollersSubsystem, - elevatorSubsystem, - shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, - climbSubsystem - ); + // addRequirements(intakePivotSubsystem, intakeRollersSubsystem, + // elevatorSubsystem, + // shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem, + // climbSubsystem + // ); - addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0)), + addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0), intakeRollersSubsystem), new ElevatorToGroundCommand(elevatorSubsystem), - new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0)), + new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0), shooterFeederSubsystem), new ShooterFlywheelStopCommand(shooterFlywheelSubsystem)//, // new ClimbLowerCommand(climbSubsystem) // NOT USING CLIMB FOR NOW ); diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java index 68dae26b..23ac90d1 100644 --- a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java +++ b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java @@ -32,6 +32,7 @@ public void execute(){ @Override public boolean isFinished(){ + System.out.println("TO GROUND RUNNING"); return elevatorSubsystem.atState(ElevatorState.GROUND); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java index ee1233bb..868bfb79 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -15,12 +15,13 @@ public class IntakeRollerFeedCommand extends Command{ public IntakeRollerFeedCommand(IntakeRollersSubsystem intakeSubsystem){ this.intakeSubsystem = intakeSubsystem; timer = new TrackingTimer(); + addRequirements(intakeSubsystem); } @Override public void initialize() { // TODO Auto-generated method stub - intakeSubsystem.setAllRollSpeed(rollerscounterclockwise,rollersclockwise); + intakeSubsystem.setAllRollSpeed(rollerscounterclockwise, rollersclockwise); } @Override diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java index 8b593209..17719673 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -12,6 +12,7 @@ public class IntakeRollerIntakeCommand extends Command{ public IntakeRollerIntakeCommand(IntakeRollersSubsystem intakeSubsystem){ this.intakeSubsystem = intakeSubsystem; + addRequirements(intakeSubsystem); } @Override diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java index e347cb41..465e4f83 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java @@ -16,11 +16,13 @@ public class IntakeRollerOutakeCommand extends Command{ public IntakeRollerOutakeCommand(IntakeRollersSubsystem intakeSubsystem){ this.intakeSubsystem = intakeSubsystem; timer = new TrackingTimer(); + addRequirements(intakeSubsystem); } @Override public void initialize() { - intakeSubsystem.setRollSpeed(rollersclockwise,rollerscounterclockwise); + timer.reset(); + intakeSubsystem.setRollSpeed(-1,-1); } @Override diff --git a/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java b/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java index e133c8f7..13409c91 100644 --- a/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java +++ b/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java @@ -19,7 +19,7 @@ public class ShootModeSequence extends SequentialCommandGroup { /* * START - * / | `---------. + * / | `---------. * Angle shooter lower Start Flywheels * for intaking elevator | * \ / | diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java index 49e55c84..c66f38b9 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java @@ -14,12 +14,14 @@ public ShooterPivotSetAngleCommand(ShooterPivotSubsystem pivotSubsystem, double @Override public void initialize() { + System.out.println("SET ANGLE"); pivotSubsystem.setAutoAimBoolean(false); pivotSubsystem.setAngle(angle); } @Override public boolean isFinished() { + System.out.println("NOT FINISHED"); return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java index c8e505b6..72a5b7dc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java @@ -64,6 +64,7 @@ public void setRollersInwards(Boolean pressedB){ @Override public void periodic() { + // System.out.println(sensor.get()); // This method will be called once per scheduler run } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java index 3c1e3841..1b259d84 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -10,9 +10,9 @@ public class ShooterFeederSubsystem extends SubsystemBase{ //finals - public final double FEEDER_MOTOR_SPEED = 0.1; + public final double FEEDER_MOTOR_SPEED = .8; public final int NO_NOTE_TOLERANCE = 500; //must test with no note in front of sensor - public final int TOLERANCE = 7000; //represents the value when half note is in front of sensor + public final int TOLERANCE = 1000; //represents the value when half note is in front of sensor //motors private final TalonSRX feederMotor; @@ -36,7 +36,7 @@ public void setFeederMotorSpeed(double speed){ } public int getRed(){ - System.out.println("proximity: " + shooterSensor.getProximity()); + // System.out.println("proximity: " + shooterSensor.getProximity()); return shooterSensor.getRed(); } @@ -44,4 +44,9 @@ public ColorSensorV3 getSensor(){ System.out.println("returning shooter sensor"); return shooterSensor; } + + @Override + public void periodic() { + System.out.println(getRed()); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 35a08763..2ee6fa9e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -218,7 +218,7 @@ public void resetDriverHeading() { } private Rotation2d getGyroHeading() { - return Rotation2d.fromDegrees(ahrs.getAngle()); + return Rotation2d.fromDegrees(-ahrs.getAngle()); } public Rotation2d getDriverHeading() { From ce97e4d94a70e6e572eaa56fdf37709c712824de Mon Sep 17 00:00:00 2001 From: penguin212 Date: Mon, 5 Feb 2024 17:56:49 -0800 Subject: [PATCH 23/25] leds! --- src/main/java/frc/robot/Constants.java | 5 + src/main/java/frc/robot/RobotContainer.java | 10 + .../frc/robot/subsystems/leds/LEDLayer.java | 117 +++++++++++ .../frc/robot/subsystems/leds/LEDStrip.java | 54 +++++ .../robot/subsystems/leds/LEDSubsystem.java | 184 ++++++++++++++++++ .../robot/subsystems/leds/RotaryLEDLayer.java | 61 ++++++ .../shooter/ShooterFeederSubsystem.java | 2 +- 7 files changed, 432 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDLayer.java create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDStrip.java create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 365fcd64..d898df0d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -117,4 +117,9 @@ public static class ClimbConstants { public static final double MAX_WINCH_POWER = 0.6; } + + public static class LEDConstants { + public static final int LED_LENGTH = 140; + public static final int LED_PWM_PORT = 0; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2988a504..509a5c27 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import frc.robot.subsystems.shooter.ShooterPivotSubsystem; import frc.robot.subsystems.intake.IntakePivotSubsystem; import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.subsystems.leds.LEDSubsystem; import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.XboxDriveController; @@ -40,6 +41,7 @@ import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoTrajectory; 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.XboxController; import edu.wpi.first.wpilibj2.command.Command; @@ -70,6 +72,8 @@ public class RobotContainer { private final ElevatorSubsystem elevatorSubsystem; + private final LEDSubsystem ledSubsystem = new LEDSubsystem(); + private final XboxController mechController = new XboxController(2); private final JoystickButton aButton = new JoystickButton(mechController, XboxController.Button.kA.value); private final JoystickButton bButton = new JoystickButton(mechController, XboxController.Button.kB.value); @@ -138,10 +142,16 @@ private void configureBindings() { new IntakeRollerOutakeCommand(intakeRollerSubsystem) ))); + + if(baseSwerveSubsystem instanceof SwerveSubsystem){ final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; + ledSubsystem.setDefaultCommand(new RunCommand(() -> { + ledSubsystem.setDriverHeading(-swerveSubsystem.getDriverHeading().getRadians());// - swerveSubsystem.getRobotPosition().getRotation().getRadians()); + }, ledSubsystem)); + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDLayer.java b/src/main/java/frc/robot/subsystems/leds/LEDLayer.java new file mode 100644 index 00000000..424634b2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDLayer.java @@ -0,0 +1,117 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.util.Color; + +public class LEDLayer { + protected final Color[] colorArray; + protected final double[] opacityArray; + protected final int ledLength; + + public LEDLayer(int length) { + ledLength = length; + colorArray = new Color[length]; + opacityArray = new double[length]; + } + + /** + * Sets an LED at a specified index. + * @param i The LED index to set. + * @param color The color to set the LED at index i to (null is equivalent to transparent). + * @param opacity The opacity to set the LED at index i to + */ + public void setLED(int i, Color color, double opacity) { + colorArray[i] = color; + opacityArray[i] = opacity; + } + + public void setLED(int i, Color color){ + setLED(i, color, 1); + } + + /** + * Gets the color of the LED at a specified index. + * @param i The LED index to retrieve. + * @return The color of the LED at index i. + */ + public Color getLEDColor(int i) { + return colorArray[i]; + } + + /** + * Gets the opacity of the LED at a specified index + * @param i The LED index to retrieve. + * @return The opacity of the LED at index i. + */ + public double getLEDOpacity(int i){ + return opacityArray[i]; + } + + /** + * Moves the leds up by an increment + * @param inc The number of leds to move up by + * @param color The color to set at the bottom + * @param opacity The opacity to set the new leds at + */ + public void incrementColors(int inc, Color color, double opacity) { + for (int i = 0; i < colorArray.length - inc; i++) { + setLED(i, getLEDColor(i + inc), getLEDOpacity(i +inc)); + } + for (int i = colorArray.length - inc; i < colorArray.length; i++) { + setLED(i, color, opacity); + } + } + + public void incrementColors(int inc, Color color){ + incrementColors(inc, color, 1); + } + + /** + * Fills the layer with a solid color. + * @param color The color to fill the layer with. + * @param opacity The opacity to fill the layer with + */ + public void fillColor(Color color, double opacity) { + for (int i = 0; i < colorArray.length; i++) { + setLED(i, color, opacity); + } + } + + public void fillColor(Color color){ + fillColor(color, 1); + } + + /** + * Fills the layer with alternating groups of "on" and "off" LEDs. "off" leds are set to null (transparent). + * @param onGroupLength The length of the "on" group. + * @param offGroupLength The length of the "off" group. + * @param borderLength The length of a gradient border which fades in at the edge of each "on" length + * @param color The color to set the "on" LEDs. + * @param opacity The opacity of the "on" LEDs. + * @param offset The number of LEDs to offset the base by + */ + public void fillGrouped(int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset) { + for (int i = 0; i < colorArray.length; i++) { + int ledNumInSegment = (i + offset) % (2 * borderLength + onGroupLength + offGroupLength); + if (ledNumInSegment < borderLength){ + setLED(i, color, opacity * (ledNumInSegment + 1) / (borderLength + 1)); + } else if (ledNumInSegment < onGroupLength + borderLength) { + setLED(i, color, opacity); + } else if(ledNumInSegment < onGroupLength + borderLength * 2){ + setLED(i, color, opacity * (1 - ((ledNumInSegment - onGroupLength - borderLength + 1.) / (borderLength + 1)))); + } else { + setLED(i, null, opacity); + } + } + } + + public void fillGrouped(int onGroupLength, int offGroupLength, Color color){ + fillGrouped(onGroupLength, offGroupLength, 0, color, 1, 0); + } + + /** + * Resets the layer by setting all LEDs to null (transparent). + */ + public void reset() { + fillColor(null, 1); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/leds/LEDStrip.java b/src/main/java/frc/robot/subsystems/leds/LEDStrip.java new file mode 100644 index 00000000..0a5ae16c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDStrip.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.util.Color; + +public class LEDStrip { + private final AddressableLED led; + private final AddressableLEDBuffer ledBuffer; + + private int ledLength; + + public LEDStrip(int ledPort, int ledLength) { + this.ledLength = ledLength; + led = new AddressableLED(ledPort); + ledBuffer = new AddressableLEDBuffer(ledLength); + led.setLength(ledBuffer.getLength()); + led.start(); + } + + /** + * Sets the LED data to the current buffer. + */ + public void setBuffer() { + led.setData(ledBuffer); + } + + /** + * Applies an `LEDLayer` on top of the LED buffer. + * @param layer The layer to be added on top of the buffer. Null leds are considered transparent and "fall through" to the previous layer's color. + */ + public void addLayer(LEDLayer layer) { + for (int i = 0; i < ledLength; i++) { + if (layer.getLEDColor(i) != null) { + ledBuffer.setLED(i, calcColorWithOpacity(ledBuffer.getLED(i), layer.getLEDColor(i), layer.getLEDOpacity(i))); + } + } + } + + /** + * Calculates the desired color when led layering + * @param baseColor The color of the lower layer led + * @param topColor The color of the led being added + * @param opacity The opacity of the top led + * @return The color of the led layers combined + */ + public Color calcColorWithOpacity(Color baseColor, Color topColor, double opacity){ + double r = (((1 - opacity) * baseColor.red) + (opacity * topColor.red)); + double g = (((1 - opacity) * baseColor.green) + (opacity * topColor.green)); + double b = (((1 - opacity) * baseColor.blue) + (opacity * topColor.blue)); + + return(new Color(r, g, b)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java new file mode 100644 index 00000000..b2cc83c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java @@ -0,0 +1,184 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.util.TrackingTimer; +import static frc.robot.Constants.LEDConstants.*; + +public class LEDSubsystem extends SubsystemBase { + private final LEDStrip ledStrip; + // private final LEDStrip rishayStrip; + private final LEDLayer baseLayer; + private final RotaryLEDLayer driveAngleLayer; + private final LEDLayer aprilDetectedLayer; + + private final Timer blinkTimer; + private static final double BLINK_DURATION_SECONDS = 0.5; + private static final double BLINK_OFF_TO_ON_RATIO = 4; + private boolean blinking = false; + + private final TrackingTimer aprilBlinkTimer = new TrackingTimer(); + private static final double APRIL_BLINK_DURATION_SECONDS = 0.05; + + private static final Timer fadeTimer = new Timer(); + private static final double COLOR_SENSOR_FADE_PERIOD_SECONDS = .5; + private final double COLOR_PULSE_SPEED = .2; + private boolean colorSensorOff = false; + private double colorOffset = 0; + + private static final double BRIGHTNESS_SCALE_FACTOR = .5; + private static final double INPUT_DEADZONE = 0.35; + private static final int LEDS_PER_SEC = 150; + + private Color pieceColor = CUBE_COLOR; + private Color manualColor = new Color(0, 0, 0); + + private boolean manual = false; // If the driver is directly controlling leds + public boolean pieceGrabbed = false; + + + private static final Color BASE_COLOR = scaleDownColorBrightness(new Color(255, 0, 0)); + private static final Color APRIL_COLOR = scaleDownColorBrightness(new Color(252, 255, 236)); + private static final Color CUBE_COLOR = scaleDownColorBrightness(new Color(192, 8, 254)); + private static final Color CONE_COLOR = scaleDownColorBrightness(new Color(255, 100, 0)); + private static final Color WHITE = scaleDownColorBrightness(new Color(255,255,255)); + private static final Color COLOR_SENSOR_OFF_COLOR = scaleDownColorBrightness(new Color(255, 0, 0)); + + private final Timer ledTimer; // TODO: better naming + + private int offset = 0; + private double driverHeading = 0.0; + + public LEDSubsystem() { + ledStrip = new LEDStrip(LED_PWM_PORT, LED_LENGTH); + + baseLayer = new LEDLayer(LED_LENGTH); + driveAngleLayer = new RotaryLEDLayer(LED_LENGTH); + aprilDetectedLayer = new LEDLayer(LED_LENGTH); + + blinkTimer = new Timer(); + ledTimer = new Timer(); + ledTimer.start(); + + fadeTimer.start(); + + // rishayStrip = new LEDStrip(1, LED_LENGTH); + } + + @Override + public void periodic() { + + // System.out.println("asdf"); + // Number of leds to increment each continuous led layer by + int inc = Math.min((int) Math.ceil(ledTimer.get() * LEDS_PER_SEC), LED_LENGTH); + ledTimer.reset(); + ledTimer.start(); + + offset += inc; + offset = offset % LED_LENGTH; + + // Update baseLayer - the piece color indicated by the mech driver, or the blink color if a piece + // is held and we are blinking. + baseLayer.fillColor(BASE_COLOR); + // baseLayer.setLED(LED_LENGTH - 1, new Color(0,255,0)); + + + if(!DriverStation.isEnabled()){ + driverHeading = 2 * Math.PI * ((double) offset) / (double) LED_LENGTH; + } + driveAngleLayer.setAngleGroup(driverHeading, 2, 5, WHITE, .7); + + // Update aprilDetectedLayer - white pulses to indicate an april tag detection. + if (!aprilBlinkTimer.hasElapsed(APRIL_BLINK_DURATION_SECONDS) && aprilBlinkTimer.hasStarted()) { + aprilDetectedLayer.fillGrouped(3, 6, 1, APRIL_COLOR, .7, offset); + } else { + aprilDetectedLayer.incrementColors(inc, null); + } + + // Add layers to buffer, set leds + ledStrip.addLayer(baseLayer); + ledStrip.addLayer(driveAngleLayer); + // ledStrip.addLayer(aprilDetectedLayer); + ledStrip.setBuffer(); + + // rishayStrip.addLayer(baseLayer); + // rishayStrip.addLayer(driveAngleLayer); + } + + /** + * Toggles whether drivers are manually controlling the color of the LEDs. + */ + public void toggleManual() { + this.manual = !manual; + } + + /** + * Displays that an AprilTag has been detected by sending a pulse down the LEDs. + */ + public void displayTagDetected() { + aprilBlinkTimer.start(); + aprilBlinkTimer.advanceIfElapsed(APRIL_BLINK_DURATION_SECONDS * BLINK_OFF_TO_ON_RATIO); + } + + /** + * Cross-fades between two colors using a sinusoidal scaling function. + * @param currentTimeSeconds The current elapsed time of fading. + * @param periodSeconds The period of the fade function, in seconds. + * @return The scale to set the opacity to + */ + private static Color crossFadeWithTime(Color color, Color fadeColor, double periodSeconds) { + // The [0.0, 1.0] brightness scale to scale the color by. Scale = 1/2 * cos(t) + 1/2 where + // t is scaled to produce the desired period. + double scale = 0.5 * Math.cos(fadeTimer.get() * 2 * Math.PI / periodSeconds) + 0.5; + + return new Color( + color.red * scale + fadeColor.red * (1 - scale), + color.green * scale + fadeColor.green * (1 - scale), + color.blue * scale + fadeColor.blue * (1 - scale) + ); + } + + /** + * Scales a color's brightness by the BRIGHTNESS_SCALE_FACTOR. + * @param color The color to scale down. + * @return The scaled down color. + */ + private static Color scaleDownColorBrightness(Color color) { + return new Color( + color.red * BRIGHTNESS_SCALE_FACTOR, + color.green * BRIGHTNESS_SCALE_FACTOR, + color.blue * BRIGHTNESS_SCALE_FACTOR + ); + } + + /** + * Sets the color of the LEDs commanded by driver input. If manual input is enabled, this sets the color + * to the HSV color created by the angle of the joystick. Otherwise, set the color to CUBE if the joystick + * is pushed right and CONE if it is pushed left. + * + * @param x The x input of the joystick. + * @param y The y input of the joystick. + */ + public void setDriverColors(double x, double y){ + double angleRads = MathUtil.inputModulus(Math.atan2(y, x), 0, 2 * Math.PI); + manualColor = Color.fromHSV( + (int) (Math.toDegrees(angleRads) / 2.0), + (int) 255, + (int) (255 * BRIGHTNESS_SCALE_FACTOR) + ); + + if (x > INPUT_DEADZONE) { + pieceColor = CUBE_COLOR; + } else if (x < -INPUT_DEADZONE) { + pieceColor = CONE_COLOR; + } + } + + public void setDriverHeading(double heading){ + driverHeading = heading; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java new file mode 100644 index 00000000..aee7a311 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems.leds; + +import edu.wpi.first.wpilibj.util.Color; + +public class RotaryLEDLayer extends LEDLayer { + + public RotaryLEDLayer(int length) { + super(length); + } + + public void incrementColors(int inc) { + for (int i = 0; i < colorArray.length - inc; i++) { + setLED(i, getLEDColor(i + inc), getLEDOpacity(i + inc)); + } + for (int i = colorArray.length - inc; i < colorArray.length; i++) { + setLED(i, getLEDColor(i - colorArray.length + inc), getLEDOpacity(i - colorArray.length + inc)); + } + } + + public Color getLEDColor(int i){ + return colorArray[i % ledLength]; + } + + public void setLED(int i, Color color, double opacity) { + colorArray[(i + ledLength) % ledLength] = color; + opacityArray[(i + ledLength) % ledLength] = opacity; + } + + public void setLED(int i, Color color){ + setLED(i, color, 1); + } + + /** + * + * @param angle + * @param radius + * @param borderLength + * @param color + * @param opacity + */ + public void setAngleGroup(double angle, int radius, int borderLength, Color color, double opacity) { + + //center + int offset = (int) (ledLength * angle / (2 * Math.PI)); + + fillColor(null); + + for (int i = 0; i < borderLength; i++) { + setLED(offset - borderLength - radius - 1 + i, color, opacity * (i + 1) / (borderLength + 1)); + } + + for (int i = 0; i < radius * 2 + 1; i++) { + // System.out.println(offset - radius - 1); + setLED(offset - radius - 1 + i, color, opacity); + } + + for (int i = 0; i < borderLength; i++) { + setLED(offset + radius + i, color, opacity * (1 - (i / (borderLength + 1.)))); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java index 1b259d84..96336d07 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java @@ -47,6 +47,6 @@ public ColorSensorV3 getSensor(){ @Override public void periodic() { - System.out.println(getRed()); + // System.out.println(getRed()); } } From 84c9a0bdf890511dfd75a1adbb797b2d72c171ef Mon Sep 17 00:00:00 2001 From: Pointy Ice Date: Mon, 5 Feb 2024 18:27:17 -0800 Subject: [PATCH 24/25] Implemented camera subsystem --- src/main/java/frc/robot/Constants.java | 4 + .../subsystems/camera/CameraSubsystem.java | 73 +++++++++++++++++++ 2 files changed, 77 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/camera/CameraSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d898df0d..b4c12a15 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,6 +16,10 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + public static class CameraConstants{ + public static final int resolutionX = 176; + public static final int resolutionY = 144; + } public static class ElevatorConstants { public static final int EXTENSION_ID = 10; public static final int EXTENSION_FOLLOW_ID = 11; diff --git a/src/main/java/frc/robot/subsystems/camera/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/camera/CameraSubsystem.java new file mode 100644 index 00000000..53860273 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraSubsystem.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants; + +import java.util.EnumSet; + +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class CameraSubsystem extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + private UsbCamera camera1; + private UsbCamera camera2; + private MjpegServer mjpegServer1; + private double currentCameraID; + private NetworkTableInstance cameraNetworkTableInstance; + private NetworkTable cameraNetworkTable; + public CameraSubsystem() { + camera1 = new UsbCamera("camera1", 0); + camera1.setFPS(60); + camera1.setResolution(Constants.CameraConstants.resolutionX, Constants.CameraConstants.resolutionY); + camera1.setBrightness(45); + + camera2 = new UsbCamera("camera2", 1); + camera2.setFPS(60); + camera2.setResolution(Constants.CameraConstants.resolutionX, Constants.CameraConstants.resolutionY); + camera2.setBrightness(45); + + mjpegServer1 = new MjpegServer("m1", 1181); + mjpegServer1.setSource(camera1); + currentCameraID = 1.; + cameraNetworkTableInstance = NetworkTableInstance.getDefault(); + cameraNetworkTable = cameraNetworkTableInstance.getTable("camera"); + cameraNetworkTable.addListener("id", + EnumSet.of(NetworkTableEvent.Kind.kValueAll), + (NetworkTable table, String key, NetworkTableEvent event) -> { + double message = event.valueData.value.getDouble(); + System.out.println(message); + if(message == currentCameraID){ + // System.out.print("Still the same"); + } + else if(message == 1.0){ + // System.out.println("Set to cam1"); + switchTo(1); + } + else{ + // System.out.println("Set to cam2"); + switchTo(2); + } + }); + } + public void switchTo(int cameraID){ + if(cameraID == 1){ + currentCameraID = 1; + mjpegServer1.setSource(camera1); + currentCameraID = 1; + } + else if(cameraID == 2){ + mjpegServer1.setSource(camera2); + currentCameraID = 2; + } + return; + } +} From 074f923b63bc7d7f8fd8222dea84526cc4f580ca Mon Sep 17 00:00:00 2001 From: penguin212 Date: Wed, 7 Feb 2024 11:32:09 -0800 Subject: [PATCH 25/25] testing changes --- src/main/java/frc/robot/Constants.java | 9 +++--- src/main/java/frc/robot/RobotContainer.java | 10 +++++++ .../roller/IntakeRollerFeedCommand.java | 1 + .../pivot/ShooterPivotSetAngleCommand.java | 2 +- .../elevator/ElevatorSubsystem.java | 9 +++--- .../robot/subsystems/leds/LEDSubsystem.java | 29 +++++++++++++++---- .../robot/subsystems/leds/RotaryLEDLayer.java | 11 +++++++ .../shooter/ShooterPivotSubsystem.java | 2 +- 8 files changed, 57 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b4c12a15..a4ce1485 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,11 +26,11 @@ public static class ElevatorConstants { public static final float EXTENSION_LIMIT_METERS = 0; - public static final int ZERO_LIMIT_ID = 2; + public static final int ZERO_LIMIT_ID = 3; - public static final double GROUND_POSITION = 0; + public static final double GROUND_POSITION = Units.inchesToMeters(0.5); public static final double SPEAKER_POSITION = 0; - public static final double AMP_POSITION = Units.inchesToMeters(28); + public static final double AMP_POSITION = Units.inchesToMeters(29); public static final double CHUTE_POSITION = Units.inchesToMeters(1); public static final double TRAP_POSITION = Units.inchesToMeters(30); public static final double START_POSITION = 0; @@ -91,7 +91,7 @@ public static class IntakeConstants { public static final double sensorreached = .3; public static final double pivotclockwise = 1; public static final double pivotcounterclockwise = -1; - public static final double pastsensortime = 0.5; + public static final double pastsensortime = 3; } public static class ShooterConstants { @@ -125,5 +125,6 @@ public static class ClimbConstants { public static class LEDConstants { public static final int LED_LENGTH = 140; public static final int LED_PWM_PORT = 0; + public static final double BRIGHTNESS_SCALE_FACTOR = .5; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 509a5c27..514b03a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,6 +43,7 @@ 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.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -81,6 +82,9 @@ public class RobotContainer { private final JoystickButton rightBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value); private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value); private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value); + + private final GenericHID switchboard = new GenericHID(3); + private final JoystickButton redButton = new JoystickButton(switchboard, 5); private UsbCamera camera1; private MjpegServer mjpgserver1; @@ -142,6 +146,8 @@ private void configureBindings() { new IntakeRollerOutakeCommand(intakeRollerSubsystem) ))); + xButton.onTrue(new IntakeRollerOutakeCommand(intakeRollerSubsystem)); + @@ -194,6 +200,10 @@ private void configureBindings() { })); } + + redButton.onTrue(new RunCommand(() -> {ledSubsystem.setRainbow(true);}, ledSubsystem)); + redButton.onFalse(new RunCommand(() -> {ledSubsystem.setRainbow(false);}, ledSubsystem)); + } diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java index 868bfb79..6058e399 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -22,6 +22,7 @@ public IntakeRollerFeedCommand(IntakeRollersSubsystem intakeSubsystem){ public void initialize() { // TODO Auto-generated method stub intakeSubsystem.setAllRollSpeed(rollerscounterclockwise, rollersclockwise); + timer.reset(); } @Override diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java index c66f38b9..078baec6 100644 --- a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java +++ b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java @@ -21,7 +21,7 @@ public void initialize() { @Override public boolean isFinished() { - System.out.println("NOT FINISHED"); + System.out.println("NOT FINISHED" + Math.abs(pivotSubsystem.getPosition() - angle)); return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index d812fbff..a99ffddb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -56,7 +56,7 @@ public ElevatorSubsystem() { timer.start(); - this.zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); + zeroLimitSwitch = new DigitalInput(ElevatorConstants.ZERO_LIMIT_ID); elevatorNetworkTableInstance = NetworkTableInstance.getDefault(); elevatorNetworkTable = elevatorNetworkTableInstance.getTable("elevator"); elevatorNetworkTablePositionEntry = elevatorNetworkTable.getEntry("target_position"); @@ -116,9 +116,9 @@ else if(currentTargetState.equals(ElevatorState.TRAP)){ @Override public void periodic(){ //System.out.println(elevatorNetworkTablePositionEntry.getString("default")); - if(timer.advanceIfElapsed(.2)){ - // System.out.println(Units.metersToInches(getExtensionMeters())); - } + // if(timer.advanceIfElapsed(.2)){ + // System.out.println(zeroLimitSwitch.get()); + // } //System.out.println(this.getTargetState()); if(isManual){ @@ -128,6 +128,7 @@ public void periodic(){ } if (zeroLimitSwitch != null && !zeroLimitSwitch.get()){ + // System.out.println("RESET LIMIT"); extensionEncoder.setPosition(0); } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java index b2cc83c4..72594734 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java @@ -14,6 +14,8 @@ public class LEDSubsystem extends SubsystemBase { // private final LEDStrip rishayStrip; private final LEDLayer baseLayer; private final RotaryLEDLayer driveAngleLayer; + private final RotaryLEDLayer driveBackAngleLayer; + private final RotaryLEDLayer rainbowLayer; private final LEDLayer aprilDetectedLayer; private final Timer blinkTimer; @@ -30,15 +32,16 @@ public class LEDSubsystem extends SubsystemBase { private boolean colorSensorOff = false; private double colorOffset = 0; - private static final double BRIGHTNESS_SCALE_FACTOR = .5; + private static final double INPUT_DEADZONE = 0.35; private static final int LEDS_PER_SEC = 150; private Color pieceColor = CUBE_COLOR; private Color manualColor = new Color(0, 0, 0); - private boolean manual = false; // If the driver is directly controlling leds + private boolean rainbow = false; // If the driver is directly controlling leds public boolean pieceGrabbed = false; + private boolean enabled = false; private static final Color BASE_COLOR = scaleDownColorBrightness(new Color(255, 0, 0)); @@ -47,6 +50,7 @@ public class LEDSubsystem extends SubsystemBase { private static final Color CONE_COLOR = scaleDownColorBrightness(new Color(255, 100, 0)); private static final Color WHITE = scaleDownColorBrightness(new Color(255,255,255)); private static final Color COLOR_SENSOR_OFF_COLOR = scaleDownColorBrightness(new Color(255, 0, 0)); + private static final Color BLUE = scaleDownColorBrightness(new Color(0,0,255)); private final Timer ledTimer; // TODO: better naming @@ -58,6 +62,8 @@ public LEDSubsystem() { baseLayer = new LEDLayer(LED_LENGTH); driveAngleLayer = new RotaryLEDLayer(LED_LENGTH); + driveBackAngleLayer = new RotaryLEDLayer(LED_LENGTH); + rainbowLayer = new RotaryLEDLayer(LED_LENGTH); aprilDetectedLayer = new LEDLayer(LED_LENGTH); blinkTimer = new Timer(); @@ -89,8 +95,12 @@ public void periodic() { if(!DriverStation.isEnabled()){ driverHeading = 2 * Math.PI * ((double) offset) / (double) LED_LENGTH; + driveBackAngleLayer.fillColor(null); + } else { + driveBackAngleLayer.setAngleGroup(driverHeading + Math.PI, 5, 5, BLUE, .7); } - driveAngleLayer.setAngleGroup(driverHeading, 2, 5, WHITE, .7); + driveAngleLayer.setAngleGroup(driverHeading, 5, 5, WHITE, .7); + // Update aprilDetectedLayer - white pulses to indicate an april tag detection. if (!aprilBlinkTimer.hasElapsed(APRIL_BLINK_DURATION_SECONDS) && aprilBlinkTimer.hasStarted()) { @@ -99,10 +109,17 @@ public void periodic() { aprilDetectedLayer.incrementColors(inc, null); } + if(rainbow){ + rainbowLayer.setRainbow(offset); + } else { + rainbowLayer.fillColor(null); + } + // Add layers to buffer, set leds ledStrip.addLayer(baseLayer); ledStrip.addLayer(driveAngleLayer); - // ledStrip.addLayer(aprilDetectedLayer); + ledStrip.addLayer(driveBackAngleLayer); + ledStrip.addLayer(rainbowLayer); ledStrip.setBuffer(); // rishayStrip.addLayer(baseLayer); @@ -112,8 +129,8 @@ public void periodic() { /** * Toggles whether drivers are manually controlling the color of the LEDs. */ - public void toggleManual() { - this.manual = !manual; + public void setRainbow(boolean rainbow) { + this.rainbow = rainbow; } /** diff --git a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java index aee7a311..b89eae38 100644 --- a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java +++ b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.leds; import edu.wpi.first.wpilibj.util.Color; +import static frc.robot.Constants.LEDConstants.*; public class RotaryLEDLayer extends LEDLayer { @@ -58,4 +59,14 @@ public void setAngleGroup(double angle, int radius, int borderLength, Color colo setLED(offset + radius + i, color, opacity * (1 - (i / (borderLength + 1.)))); } } + + public void setRainbow(int offset){ + for(int i = 0; i < colorArray.length; i++){ + setLED(i + offset, Color.fromHSV( + (int) (((double) i) * 180.0 / colorArray.length), + (int) 255, + (int) (255 * BRIGHTNESS_SCALE_FACTOR) + )); + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index 75d8e8b9..54d30881 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -19,7 +19,7 @@ public class ShooterPivotSubsystem extends SubsystemBase { public final double PIVOT_SPEED = 0.1; final double GEARBOX_RATIO = 18.16; //ask cadders public final double ERRORTOLERANCE = Math.toRadians(2); //error tolerance for pid - final int LIMIT_SWITCH_ID = 3; //placeholder + final int LIMIT_SWITCH_ID = 4; //placeholder final double CONVERSION_FACTOR = Math.PI/(2.*4.57); //motors