From e2a780452626f8421fbdef6f3a582540aff26dd1 Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Mon, 22 Jan 2024 15:15:24 -0500 Subject: [PATCH 01/14] Receieve code --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ .../robot/subsystems/ShooterSubsystem.java | 19 +++++++++++++++++++ 3 files changed, 28 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fa922ab..6e8701e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,6 +55,7 @@ public static final class Shooter { public static final int speakerStrength = 15760; public static final int ampStrength = 2560; + public static final int receive = -100; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d6de79..ae294b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,7 +1,9 @@ package frc.robot; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.*; import frc.robot.util.*; @@ -22,6 +24,7 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + configureButtonBindings(); SwerveSubsystem.setDefaultCommand( new TeleopSwerve( SwerveSubsystem, @@ -44,6 +47,11 @@ public RobotContainer() { () -> ShooterSubsystem.speakerShot(CONTROLLER.getXButton()), ShooterSubsystem)); } + private void configureButtonBindings() { + new JoystickButton(CONTROLLER, 4) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); + } public Command getAutonomousCommand() { return AutonomousManager.getCommand(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6a524f3..d893e94 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -106,4 +106,23 @@ public void ampShot(boolean shoot) { ControlType.kVelocity); } } + public void receive(boolean shoot) { + if (shoot) { + shooterPidController_1.setReference( + Constants.Shooter.receive, + ControlType.kVelocity); + shooterPidController_2.setReference( + Constants.Shooter.receive, + ControlType.kVelocity); + } else { + shooterPidController_1.setReference( + 0, + ControlType.kVelocity); + shooterMotor_1.set(0); + shooterPidController_2.setReference( + 0, + ControlType.kVelocity); + shooterMotor_2.set(0); + } + } } From 87ccf2e0f92469d3366ffc505d699a5b2e6b6609 Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Mon, 22 Jan 2024 16:33:09 -0500 Subject: [PATCH 02/14] maybe intake --- src/main/java/frc/robot/Constants.java | 10 +++++----- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../robot/subsystems/ShooterSubsystem.java | 19 ++++++++++--------- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e8701e..4c68abc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,16 +53,16 @@ public static final class Shooter { public static final int driveContinuousCurrentLimit = 10; public static final double voltageComp = 12.0; - public static final int speakerStrength = 15760; + public static final int speakerStrength = -1000; public static final int ampStrength = 2560; - public static final int receive = -100; + public static final int receive = 100; } public static final class Swerve { - public static final double fastDriveSpeedMultiplier = 1.0; - public static final double normalDriveSpeedMultiplier = 1.0; - public static final double slowDriveSpeedMultiplier = 1.0; + public static final double fastDriveSpeedMultiplier = .8; + public static final double normalDriveSpeedMultiplier = .6; + public static final double slowDriveSpeedMultiplier = .4; public static final int PIGEON2_ID = 10; //SET ME Each Run... public static final double robotOffset = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae294b3..ed64154 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,9 +48,9 @@ public RobotContainer() { ShooterSubsystem)); } private void configureButtonBindings() { - new JoystickButton(CONTROLLER, 4) - .whileTrue( - new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); + // new JoystickButton(CONTROLLER, 5) + //.whileTrue( + // new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d893e94..7389d8f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -114,15 +114,16 @@ public void receive(boolean shoot) { shooterPidController_2.setReference( Constants.Shooter.receive, ControlType.kVelocity); - } else { - shooterPidController_1.setReference( - 0, - ControlType.kVelocity); - shooterMotor_1.set(0); - shooterPidController_2.setReference( - 0, - ControlType.kVelocity); - shooterMotor_2.set(0); + } + else { + // shooterPidController_1.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_1.set(0); + //shooterPidController_2.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_2.set(0); } } } From 9b281d625c006220b43f9f422fef7ae6344e7844 Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Wed, 24 Jan 2024 14:08:08 -0500 Subject: [PATCH 03/14] bump motor --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../frc/robot/subsystems/ShooterSubsystem.java | 18 ++++++++++++------ 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4c68abc..786eef1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,6 +46,7 @@ public static final class Shooter { public static final int shooterID_1 = 22; public static final int shooterID_2 = 25; + public static final int bumpID = 26; /* Swerve Current Limiting */ @@ -53,7 +54,7 @@ public static final class Shooter { public static final int driveContinuousCurrentLimit = 10; public static final double voltageComp = 12.0; - public static final int speakerStrength = -1000; + public static final int speakerStrength = 5900; public static final int ampStrength = 2560; public static final int receive = 100; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ed64154..157757d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,9 +48,9 @@ public RobotContainer() { ShooterSubsystem)); } private void configureButtonBindings() { - // new JoystickButton(CONTROLLER, 5) - //.whileTrue( - // new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); + new JoystickButton(CONTROLLER, 5) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 7389d8f..479bade 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -13,6 +13,7 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax shooterMotor_1; private final CANSparkMax shooterMotor_2; + private final CANSparkMax bumpMotor; private final RelativeEncoder shooterRelativeEncoder_1; private final RelativeEncoder shooterRelativeEncoder_2; @@ -32,6 +33,8 @@ public ShooterSubsystem() { shooterMotor_2 = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless); shooterRelativeEncoder_2 = shooterMotor_2.getEncoder(); shooterPidController_2 = shooterMotor_2.getPIDController(); + + bumpMotor = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless); configShooterMotors(); } @@ -66,6 +69,13 @@ private void configShooterMotors() { shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_2.burnFlash(); shooterRelativeEncoder_2.setPosition(0.0); + + bumpMotor.restoreFactoryDefaults(); + CANSparkMaxUtil.setCANSparkMaxBusUsage(bumpMotor, Usage.kAll); + bumpMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); + bumpMotor.setInverted(!Constants.Shooter.driveInvert); + bumpMotor.setIdleMode(Constants.Shooter.driveNeutralMode); + bumpMotor.burnFlash(); } public void speakerShot(boolean shoot) { @@ -108,12 +118,8 @@ public void ampShot(boolean shoot) { } public void receive(boolean shoot) { if (shoot) { - shooterPidController_1.setReference( - Constants.Shooter.receive, - ControlType.kVelocity); - shooterPidController_2.setReference( - Constants.Shooter.receive, - ControlType.kVelocity); + shooterMotor_1.set(-.15); + shooterMotor_2.set(-.15); } else { // shooterPidController_1.setReference( From 5b17c0c7b98184c7e969eee4ac346b4255d0ff9d Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Wed, 24 Jan 2024 14:09:43 -0500 Subject: [PATCH 04/14] bump code --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../frc/robot/subsystems/ShooterSubsystem.java | 16 ++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 157757d..ceab54b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,6 +51,10 @@ private void configureButtonBindings() { new JoystickButton(CONTROLLER, 5) .whileTrue( new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); + + new JoystickButton(CONTROLLER, 6) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.bump(true), ShooterSubsystem)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 479bade..d0016d0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -131,5 +131,21 @@ public void receive(boolean shoot) { // ControlType.kVelocity); //shooterMotor_2.set(0); } + } + public void bump(boolean shoot) { + if (shoot) { + bumpMotor.set(1); + //shooterMotor_2.set(-.15); + } + else { + // shooterPidController_1.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_1.set(0); + //shooterPidController_2.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_2.set(0); + } } } From c30ef9d88489aeed06f22a70aef3a42e02742da7 Mon Sep 17 00:00:00 2001 From: neev Date: Wed, 24 Jan 2024 23:09:00 -0500 Subject: [PATCH 05/14] linear actuator with sparkmax brushed mode --- src/main/java/frc/robot/RobotContainer.java | 8 ++++- .../subsystems/LinearActuatorSubsystem.java | 34 +++++++++++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 157757d..5e22e08 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,5 @@ package frc.robot; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -17,6 +16,7 @@ public class RobotContainer { public final SwerveSubsystem SwerveSubsystem = new SwerveSubsystem(); public final ShooterSubsystem ShooterSubsystem = new ShooterSubsystem(); + public final LinearActuatorSubsystem LinearActuatorSubsystem = new LinearActuatorSubsystem(); private final AutonomousManager AutonomousManager = new AutonomousManager(SwerveSubsystem); Gamepad CONTROLLER = new Gamepad(Constants204.Controller.PORT); @@ -46,6 +46,12 @@ public RobotContainer() { new RunCommand( () -> ShooterSubsystem.speakerShot(CONTROLLER.getXButton()), ShooterSubsystem)); + + LinearActuatorSubsystem.setDefaultCommand( + new RunCommand( + () -> LinearActuatorSubsystem.shift(CONTROLLER.getPOV()<90||CONTROLLER.getPOV()>270, CONTROLLER.getPOV()>90||CONTROLLER.getPOV()<270) + ) + ); } private void configureButtonBindings() { new JoystickButton(CONTROLLER, 5) diff --git a/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java b/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java new file mode 100644 index 0000000..2ce3975 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.util.CANSparkMaxUtil; +import frc.lib.util.CANSparkMaxUtil.Usage; +import frc.robot.Constants; + +public class LinearActuatorSubsystem extends SubsystemBase { + private final CANSparkMax actuatorMotor; + + public LinearActuatorSubsystem() { + actuatorMotor = new CANSparkMax(27, MotorType.kBrushed); + + actuatorMotor.restoreFactoryDefaults(); + CANSparkMaxUtil.setCANSparkMaxBusUsage(actuatorMotor, Usage.kAll); + actuatorMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); + actuatorMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + actuatorMotor.enableVoltageCompensation(Constants.Shooter.voltageComp); + actuatorMotor.burnFlash(); + } + + public void shift(boolean up, boolean down) { + if (up && !down) { + actuatorMotor.set(-1.0); + } else if (!up && down) { + actuatorMotor.set(1.0); + } else { + actuatorMotor.set(0); + } + } +} From 75538e1a497121d044eb5ae16b5e90e523cd2d78 Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Thu, 25 Jan 2024 07:54:09 -0500 Subject: [PATCH 06/14] 40 amp limit --- src/main/java/frc/robot/Constants.java | 10 +++++----- .../frc/robot/subsystems/ShooterSubsystem.java | 17 +++++++++++------ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 786eef1..9900071 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -34,7 +34,7 @@ public static final class Shooter { public static final double shooterKS = 0.667; public static final double shooterKV = 2.44; public static final double shooterKA = 0.27; - public static final double shooterKP = 1.0; + public static final double shooterKP = 2.0; public static final double shooterKI = 0.0; public static final double shooterKD = 0.0; public static final double shooterKFF = 0.0; @@ -50,11 +50,11 @@ public static final class Shooter { /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 10; - public static final int driveContinuousCurrentLimit = 10; - public static final double voltageComp = 12.0; + public static final int angleContinuousCurrentLimit = 40; + public static final int driveContinuousCurrentLimit = 40; + public static final double voltageComp = 16.0; - public static final int speakerStrength = 5900; + public static final int speakerStrength = 10000; public static final int ampStrength = 2560; public static final int receive = 100; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d0016d0..2dc9310 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -34,7 +34,7 @@ public ShooterSubsystem() { shooterRelativeEncoder_2 = shooterMotor_2.getEncoder(); shooterPidController_2 = shooterMotor_2.getPIDController(); - bumpMotor = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless); + bumpMotor = new CANSparkMax(Constants.Shooter.bumpID, MotorType.kBrushless); configShooterMotors(); } @@ -51,7 +51,7 @@ private void configShooterMotors() { shooterPidController_1.setI(Constants.Shooter.shooterKI); shooterPidController_1.setD(Constants.Shooter.shooterKD); shooterPidController_1.setFF(Constants.Shooter.shooterKFF); - shooterMotor_1.enableVoltageCompensation(Constants.Shooter.voltageComp); + //shooterMotor_1.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_1.burnFlash(); shooterRelativeEncoder_1.setPosition(0.0); @@ -66,7 +66,7 @@ private void configShooterMotors() { shooterPidController_2.setI(Constants.Shooter.shooterKI); shooterPidController_2.setD(Constants.Shooter.shooterKD); shooterPidController_2.setFF(Constants.Shooter.shooterKFF); - shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); + //shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_2.burnFlash(); shooterRelativeEncoder_2.setPosition(0.0); @@ -80,12 +80,15 @@ private void configShooterMotors() { public void speakerShot(boolean shoot) { if (shoot) { + /* shooterPidController_1.setReference( Constants.Shooter.speakerStrength, ControlType.kVelocity); shooterPidController_2.setReference( Constants.Shooter.speakerStrength, - ControlType.kVelocity); + ControlType.kVelocity);*/ + shooterMotor_1.set(1.0); + shooterMotor_2.set(1.0); } else { shooterPidController_1.setReference( 0, @@ -95,6 +98,7 @@ public void speakerShot(boolean shoot) { 0, ControlType.kVelocity); shooterMotor_2.set(0); + bumpMotor.set(0); } } @@ -118,8 +122,9 @@ public void ampShot(boolean shoot) { } public void receive(boolean shoot) { if (shoot) { - shooterMotor_1.set(-.15); - shooterMotor_2.set(-.15); + shooterMotor_1.set(-.05); + shooterMotor_2.set(-.05); + bumpMotor.set(-.05); } else { // shooterPidController_1.setReference( From f277cb23b16d6fc60740cd3dbaee64650fa4acf2 Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Fri, 26 Jan 2024 14:31:14 -0500 Subject: [PATCH 07/14] 126changes --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../robot/subsystems/ShooterSubsystem.java | 20 +++++++++---------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9900071..46ecee5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -50,8 +50,8 @@ public static final class Shooter { /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 40; - public static final int driveContinuousCurrentLimit = 40; + public static final int angleContinuousCurrentLimit = 50; + public static final int driveContinuousCurrentLimit = 50; public static final double voltageComp = 16.0; public static final int speakerStrength = 10000; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b4feeb..0b9ac19 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,7 +49,8 @@ public RobotContainer() { LinearActuatorSubsystem.setDefaultCommand( new RunCommand( - () -> LinearActuatorSubsystem.shift(CONTROLLER.getPOV()<90||CONTROLLER.getPOV()>270, CONTROLLER.getPOV()>90||CONTROLLER.getPOV()<270) + () -> LinearActuatorSubsystem.shift(CONTROLLER.getPOV()==0, CONTROLLER.getPOV()==180), + LinearActuatorSubsystem ) ); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2dc9310..4480bd9 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -47,10 +47,10 @@ private void configShooterMotors() { shooterMotor_1.setIdleMode(Constants.Shooter.driveNeutralMode); // shooterRelativeEncoder_1.setVelocityConversionFactor(Constants.Shooter.driveConversionVelocityFactor); // shooterRelativeEncoder_1.setPositionConversionFactor(Constants.Shooter.driveConversionPositionFactor); - shooterPidController_1.setP(Constants.Shooter.shooterKP); - shooterPidController_1.setI(Constants.Shooter.shooterKI); - shooterPidController_1.setD(Constants.Shooter.shooterKD); - shooterPidController_1.setFF(Constants.Shooter.shooterKFF); + //shooterPidController_1.setP(Constants.Shooter.shooterKP); + //shooterPidController_1.setI(Constants.Shooter.shooterKI); + //shooterPidController_1.setD(Constants.Shooter.shooterKD); + //shooterPidController_1.setFF(Constants.Shooter.shooterKFF); //shooterMotor_1.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_1.burnFlash(); shooterRelativeEncoder_1.setPosition(0.0); @@ -62,10 +62,10 @@ private void configShooterMotors() { shooterMotor_2.setIdleMode(Constants.Shooter.driveNeutralMode); // shooterRelativeEncoder_1.setVelocityConversionFactor(Constants.Shooter.driveConversionVelocityFactor); // shooterRelativeEncoder_1.setPositionConversionFactor(Constants.Shooter.driveConversionPositionFactor); - shooterPidController_2.setP(Constants.Shooter.shooterKP); - shooterPidController_2.setI(Constants.Shooter.shooterKI); - shooterPidController_2.setD(Constants.Shooter.shooterKD); - shooterPidController_2.setFF(Constants.Shooter.shooterKFF); + // shooterPidController_2.setP(Constants.Shooter.shooterKP); + //shooterPidController_2.setI(Constants.Shooter.shooterKI); + //shooterPidController_2.setD(Constants.Shooter.shooterKD); + //shooterPidController_2.setFF(Constants.Shooter.shooterKFF); //shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_2.burnFlash(); shooterRelativeEncoder_2.setPosition(0.0); @@ -80,8 +80,8 @@ private void configShooterMotors() { public void speakerShot(boolean shoot) { if (shoot) { - /* - shooterPidController_1.setReference( + + /* shooterPidController_1.setReference( Constants.Shooter.speakerStrength, ControlType.kVelocity); shooterPidController_2.setReference( From 8660b580519220eb5b1e20e1af3ae326882e1673 Mon Sep 17 00:00:00 2001 From: neev Date: Fri, 26 Jan 2024 16:34:17 -0500 Subject: [PATCH 08/14] dual controller mode, constant adjustments --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/Constants204.java | 3 +- src/main/java/frc/robot/Robot.java | 11 +++++++ src/main/java/frc/robot/RobotContainer.java | 36 ++++++++++++++------- 4 files changed, 40 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 46ecee5..9900071 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -50,8 +50,8 @@ public static final class Shooter { /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 50; - public static final int driveContinuousCurrentLimit = 50; + public static final int angleContinuousCurrentLimit = 40; + public static final int driveContinuousCurrentLimit = 40; public static final double voltageComp = 16.0; public static final int speakerStrength = 10000; diff --git a/src/main/java/frc/robot/Constants204.java b/src/main/java/frc/robot/Constants204.java index d687160..9cef8f3 100644 --- a/src/main/java/frc/robot/Constants204.java +++ b/src/main/java/frc/robot/Constants204.java @@ -6,7 +6,8 @@ public static final class Autonomous { public static final String CHOREO_PATH_FILE = "TestingPath"; // omit file extension } public static final class Controller { - public static final int PORT = 1; + public static final int DRIVER_PORT = 1; + public static final int OPERATOR_PORT = 2; } // BELOW IS UNUSED diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1584e37..13de508 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,6 +2,8 @@ import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -17,6 +19,11 @@ public class Robot extends TimedRobot { private Command teleopCommand; private RobotContainer robotContainer; + public enum ControlMode { + SINGLE, COMPETITION + } + public static final SendableChooser ControlModeChooser = new SendableChooser<>(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -29,6 +36,10 @@ public void robotInit() { robotContainer = new RobotContainer(); //CameraServer.startAutomaticCapture(); // use for USB camera PortForwarder.add(8888, "10.2.4.69", 80); + + ControlModeChooser.setDefaultOption("Single Controller (Driver:1 Operator:1)", ControlMode.SINGLE); + ControlModeChooser.addOption("Competition (Driver:1 Operator:2)", ControlMode.COMPETITION); + SmartDashboard.putData("Control Mode", ControlModeChooser); } /** * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0b9ac19..652c9cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,9 @@ import frc.robot.subsystems.*; import frc.robot.util.*; +import static frc.robot.Robot.ControlModeChooser; +import static frc.robot.Robot.ControlMode; + /* * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -18,25 +21,36 @@ public class RobotContainer { public final ShooterSubsystem ShooterSubsystem = new ShooterSubsystem(); public final LinearActuatorSubsystem LinearActuatorSubsystem = new LinearActuatorSubsystem(); private final AutonomousManager AutonomousManager = new AutonomousManager(SwerveSubsystem); - Gamepad CONTROLLER = new Gamepad(Constants204.Controller.PORT); + Gamepad DRIVER = new Gamepad(Constants204.Controller.DRIVER_PORT); + Gamepad OPERATOR = new Gamepad(Constants204.Controller.DRIVER_PORT); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + ControlModeChooser.onChange((ControlMode mode) -> { + if (mode == ControlMode.SINGLE) { + OPERATOR = new Gamepad(Constants204.Controller.DRIVER_PORT); + } else { + OPERATOR = new Gamepad(Constants204.Controller.OPERATOR_PORT); + } + configureButtonBindings(); + System.out.println("Switched control mode to " + mode); + }); + configureButtonBindings(); SwerveSubsystem.setDefaultCommand( new TeleopSwerve( SwerveSubsystem, - () -> CONTROLLER.getLeftX(), - () -> -1 * CONTROLLER.getLeftY(), - () -> -1 * CONTROLLER.getRightX(), + () -> DRIVER.getLeftX(), + () -> -1 * DRIVER.getLeftY(), + () -> -1 * DRIVER.getRightX(), () -> false, - () -> CONTROLLER.getYButton(), - () -> CONTROLLER.getAButton())); + () -> DRIVER.getYButton(), + () -> DRIVER.getAButton())); - if (CONTROLLER.getBButton()) { + if (DRIVER.getBButton()) { SwerveSubsystem.gyro.reset(); // s_Swerve.m_gyro_P2.calib; System.out.println("you have calibed the gyro"); @@ -44,22 +58,22 @@ public RobotContainer() { ShooterSubsystem.setDefaultCommand( new RunCommand( - () -> ShooterSubsystem.speakerShot(CONTROLLER.getXButton()), + () -> ShooterSubsystem.speakerShot(OPERATOR.getXButton()), ShooterSubsystem)); LinearActuatorSubsystem.setDefaultCommand( new RunCommand( - () -> LinearActuatorSubsystem.shift(CONTROLLER.getPOV()==0, CONTROLLER.getPOV()==180), + () -> LinearActuatorSubsystem.shift(OPERATOR.getPOV()==0, OPERATOR.getPOV()==180), LinearActuatorSubsystem ) ); } private void configureButtonBindings() { - new JoystickButton(CONTROLLER, 5) + new JoystickButton(OPERATOR, 5) .whileTrue( new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); - new JoystickButton(CONTROLLER, 6) + new JoystickButton(OPERATOR, 6) .whileTrue( new RunCommand(() -> ShooterSubsystem.bump(true), ShooterSubsystem)); } From dc8c481b3ef151b57c957308c3d412c3fd79f05d Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Mon, 29 Jan 2024 16:10:31 -0500 Subject: [PATCH 09/14] amp update --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 +++++ .../frc/robot/subsystems/ShooterSubsystem.java | 15 +++++++-------- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9900071..eaf6229 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -55,7 +55,7 @@ public static final class Shooter { public static final double voltageComp = 16.0; public static final int speakerStrength = 10000; - public static final int ampStrength = 2560; + public static final double ampStrength = .095; public static final int receive = 100; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 652c9cf..590fe47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -76,6 +76,11 @@ private void configureButtonBindings() { new JoystickButton(OPERATOR, 6) .whileTrue( new RunCommand(() -> ShooterSubsystem.bump(true), ShooterSubsystem)); + + new JoystickButton(OPERATOR, 10) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.ampShot(true), ShooterSubsystem)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 4480bd9..5711c38 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -105,19 +105,18 @@ public void speakerShot(boolean shoot) { public void ampShot(boolean shoot) { if (shoot) { - shooterPidController_1.setReference( - Constants.Shooter.ampStrength, - ControlType.kVelocity); - shooterPidController_2.setReference( - Constants.Shooter.ampStrength, - ControlType.kVelocity); + shooterMotor_1.set(Constants.Shooter.ampStrength); + shooterMotor_2.set(Constants.Shooter.ampStrength-.025); } else { shooterPidController_1.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_1.set(0); shooterPidController_2.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_2.set(0); + bumpMotor.set(0); } } public void receive(boolean shoot) { From a222eedf0cfed6402ac0a054b263198f93cdf241 Mon Sep 17 00:00:00 2001 From: TJbots15 Date: Wed, 31 Jan 2024 16:28:41 -0500 Subject: [PATCH 10/14] Added new Constants, fixed errors from HookSubsystem that I brought directly out of my GrappleHook branch, and currently working on the new ShuffleboardSubsystem --- src/main/java/frc/robot/Constants.java | 8 +- .../frc/robot/subsystems/HookSubsystem.java | 129 ++++++++++++++++++ .../robot/subsystems/ShuffleboardSubsystem | 98 +++++++++++++ 3 files changed, 234 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/subsystems/HookSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShuffleboardSubsystem diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eaf6229..089cd40 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,13 @@ public static final class ModuleConstants { public static final double kPModuleDriveController = 1; } - + + public static final class Hook { + public static final int MOTOR_CAN_ID = 3; // Defines the CAN id of the Spark Max motor controller + public static final double EXTENDED_HOOK_HEIGHT = 5; //Defines Extended Height for Hook + public static final double WITHDRAWN_HOOK_HEIGHT = 0; //Defines Withdrawn height for Hook + public static final double LIFT_HOOK_HEIGHT = 5; //Defines Lifting height for Hook + } public static final class Shooter { public static final double shooterKS = 0.667; diff --git a/src/main/java/frc/robot/subsystems/HookSubsystem.java b/src/main/java/frc/robot/subsystems/HookSubsystem.java new file mode 100644 index 0000000..ac4c9c1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HookSubsystem.java @@ -0,0 +1,129 @@ +package frc.robot.subsystems; +import java.util.*; +import com.revrobotics.*; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class HookSubsystem extends SubsystemBase { + private CANSparkMax hookMotor; // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller + private RelativeEncoder hookEncoder; // Grab the attached encoder + private final SparkPIDController hookController; + + public HookSubsystem() { + hookMotor = new CANSparkMax(Constants.Hook.MOTOR_CAN_ID, MotorType.kBrushless); // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller + hookEncoder = hookMotor.getEncoder(); // Grab the attached encoder + hookController = hookMotor.getPIDController(); + hookMotor.restoreFactoryDefaults(); + hookMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); + hookMotor.setInverted(Constants.Shooter.driveInvert); + hookMotor.setIdleMode(Constants.Shooter.driveNeutralMode); + hookController.setP(1.0); + hookController.setI(0); + hookController.setD(0); + hookController.setFF(0); + hookMotor.burnFlash(); + hookEncoder.setPosition(0.0); + } + + + + + + double i = hookEncoder.getPosition(); + + //raises the hook motor + public void liftHook() { + //private CANSparkMax hookRaising = new CANSparkMax(Constants.MAXIMUMHOOK_HEIGHT, MotorType.kBrushless); + System.out.println("Raising Hook"); + hookController.setReference(Constants.Hook.LIFT_HOOK_HEIGHT, ControlType.kPosition); + } + + //lowers the hook motor + public void downHook() { + //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); + System.out.println("Down Hook"); + hookController.setReference(Constants.Hook.WITHDRAWN_HOOK_HEIGHT, ControlType.kPosition); + } + + //lowers the hook motor + public void extendHook() { + //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); + System.out.println("Down Hook"); + hookController.setReference(Constants.Hook.EXTENDED_HOOK_HEIGHT, ControlType.kPosition); + } + + //Shuffleboard + protected void execute() { + SmartDashboard.putNumber("Hook Lifting", hookEncoder.getPosition()); + SmartDashboard.putNumber("Hook Extension", hookEncoder.getPosition()); + SmartDashboard.putNumber("Hook Withdrawn", hookEncoder.getPosition()); + //SmartDashboard.putNumber("Shooter Angle", .getPosition()); + //SmartDashboard.putNumber("Speed", ); + //SmartDashboard.putNumber("Front Left Drive Encoder", 0); + //SmartDashboard.putNumber("Front Right Drive Encoder", 0); + //SmartDashboard.putNumber("Back Left Drive Encoder", 0); + //SmartDashboard.putNumber("Back Right Drive Encoder", 0); + //SmartDashboard.putNumber("Swerve Angle", 0); + } + Shuffleboard.getTab("Hook Lifting") + .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,0) + .getEntry(); + Shuffleboard.getTab("Hook Extension") + .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,275) + .getEntry(); + Shuffleboard.getTab("Hook Withdrawn") + .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", -5)) + .withSize(330,270) + .withPosition(500,550) + .getEntry(); + Shuffleboard.getTab("Shooter Angle") + .addPersistent("Shooter Angle", SHOOTER_ANGLE) + .withSize(800,530) + .withPosition(850,0) + .getEntry(); + Shuffleboard.getTab("Speed") + .addPersistent("Speed", SPEED) + .withSize(330,270) + .withPosition(500,825) + .getEntry(); + Shuffleboard.getTab("Front Left Drive Encoder") + .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,0) + .getEntry(); + Shuffleboard.getTab("Front Right Drive Encoder") + .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,270) + .getEntry(); + Shuffleboard.getTab("Back Left Drive Encoder") + .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,540) + .getEntry(); + Shuffleboard.getTab("Right Drive Encoder") + .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,810) + .getEntry(); + Shuffleboard.getTab("Swerve Angle") + .addPersistent("Swerve Angle", SWERVE_ANGLE) + .withSize(800,530) + .withPosition(850,545) + .getEntry(); +} diff --git a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem new file mode 100644 index 0000000..69cef56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem @@ -0,0 +1,98 @@ +package frc.robot.subsystems; + +import java.util.Map; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.StateHandler; +import frc.robot.util.StateVariables.GamePieceMode; +import frc.robot.util.StateVariables.IntakePositions; +import frc.robot.util.StateVariables.VerticalLocations; + +import frc.robot.Constants; + +//importing SUBSYSTEMS +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.HookSubsystem; +import frc.robot.subsystems.LinearActuatorSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SingleStrafeSubsystem; +import frc.robot.subsystems.StrafeSubsystem; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.StrafeModule; +import frc.robot.subsystems.SwerveModule; + +public class ShuffleboardSubsystem extends SubsystemBase { + private StateHandler stateHandler = StateHandler.getInstance(); + + public ShuffleboardTab driverDashboard = Shuffleboard.getTab("Driver Dashboard") + .withSize(20,2) + .withPosition(960,0); + + private GenericEntry hookLift = driverDashboard.add("HIGH", false) + .withSize(2, 1) + .withPosition(0, 0) + .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + .getEntry(); + + private GenericEntry mid = driverDashboard.add("MID", false) + .withSize(2, 1) + .withPosition(0, 1) + .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + .getEntry(); + + private GenericEntry low = driverDashboard.add("LOW", false) + .withSize(2, 1) + .withPosition(0, 2) + .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + .getEntry(); + + private GenericEntry desiredGamePiece = driverDashboard.add("Game Piece", false) + .withSize(2, 1) + .withPosition(2, 0) + .withProperties(Map.of("Color when false", "#7842f5", "Color when true", "#f5ef42")) + .getEntry(); + + private GenericEntry desiredIntakePosition = driverDashboard + .add("DESIRED INTAKE", IntakePositions.SHOOT_TALL.toString()) + .withSize(2, 1) + .withPosition(2, 2) + .getEntry(); + + + + @Override + public void periodic() { + VerticalLocations currentVerticalLocation = stateHandler.getCurrentVerticalLocation(); + + if (currentVerticalLocation == VerticalLocations.HIGH) { + high.setBoolean(true); + } else { + high.setBoolean(false); + } + + if (currentVerticalLocation == VerticalLocations.MID) { + mid.setBoolean(true); + } else { + mid.setBoolean(false); + } + + if (currentVerticalLocation == VerticalLocations.LOW) { + low.setBoolean(true); + } else { + low.setBoolean(false); + } + + GamePieceMode currentGamePiece = stateHandler.getGamePieceMode(); + if (currentGamePiece == GamePieceMode.CONE) { + desiredGamePiece.setBoolean(true); + } else { + desiredGamePiece.setBoolean(false); + } + + desiredIntakePosition.setString(stateHandler.getDesiredIntakePosition().toString()); + + } +} \ No newline at end of file From 815a14713e97cd59204ec7583d4d237bc131abfd Mon Sep 17 00:00:00 2001 From: Joe Coyne Date: Thu, 1 Feb 2024 14:34:53 -0500 Subject: [PATCH 11/14] robot update --- src/main/java/frc/robot/Constants.java | 4 +- .../robot/subsystems/ShooterSubsystem.java | 43 ++++++++++++++++--- 2 files changed, 40 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eaf6229..8ce7a13 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,8 +44,8 @@ public static final class Shooter { /* Motor Inverts */ public static final boolean driveInvert = true; - public static final int shooterID_1 = 22; - public static final int shooterID_2 = 25; + public static final int shooterID_1 = 25; + public static final int shooterID_2 = 22; public static final int bumpID = 26; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5711c38..c852e0f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,6 +5,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.util.CANSparkMaxUtil; import frc.lib.util.CANSparkMaxUtil.Usage; @@ -47,11 +49,11 @@ private void configShooterMotors() { shooterMotor_1.setIdleMode(Constants.Shooter.driveNeutralMode); // shooterRelativeEncoder_1.setVelocityConversionFactor(Constants.Shooter.driveConversionVelocityFactor); // shooterRelativeEncoder_1.setPositionConversionFactor(Constants.Shooter.driveConversionPositionFactor); - //shooterPidController_1.setP(Constants.Shooter.shooterKP); - //shooterPidController_1.setI(Constants.Shooter.shooterKI); - //shooterPidController_1.setD(Constants.Shooter.shooterKD); - //shooterPidController_1.setFF(Constants.Shooter.shooterKFF); - //shooterMotor_1.enableVoltageCompensation(Constants.Shooter.voltageComp); + shooterPidController_1.setP(Constants.Shooter.shooterKP); + shooterPidController_1.setI(Constants.Shooter.shooterKI); + shooterPidController_1.setD(Constants.Shooter.shooterKD); + shooterPidController_1.setFF(Constants.Shooter.shooterKFF); + shooterMotor_1.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_1.burnFlash(); shooterRelativeEncoder_1.setPosition(0.0); @@ -89,6 +91,35 @@ public void speakerShot(boolean shoot) { ControlType.kVelocity);*/ shooterMotor_1.set(1.0); shooterMotor_2.set(1.0); + /* shooterPidController_1.setReference( + Constants.Shooter.speakerStrength, + ControlType.kVelocity); + shooterPidController_2.setReference( + Constants.Shooter.speakerStrength, + ControlType.kVelocity);*/ + //Rotation2d r1 = Rotation2d.fromDegrees(720); + //shooterPidController_1.setReference(720.0, ControlType.kPosition); + //shooterMotor_2.set(1.0); + } else { + shooterPidController_1.setReference( + 0, + ControlType.kVelocity); + shooterMotor_1.set(0); + shooterPidController_2.setReference( + 0, + ControlType.kVelocity); + shooterMotor_2.set(0); + bumpMotor.set(0); + } + + } + public void speakerShot(boolean shoot, int i) { + if (shoot) { + + + //Rotation2d r1 = Rotation2d.fromDegrees(720); + shooterPidController_1.setReference(-1020.0, ControlType.kPosition); + //shooterMotor_2.set(1.0); } else { shooterPidController_1.setReference( 0, @@ -103,6 +134,8 @@ public void speakerShot(boolean shoot) { } + + public void ampShot(boolean shoot) { if (shoot) { shooterMotor_1.set(Constants.Shooter.ampStrength); From fa6d15fb4ba1b085e4a65c01b70dfd2f3b4e8a7f Mon Sep 17 00:00:00 2001 From: TJbots15 Date: Thu, 1 Feb 2024 15:29:58 -0500 Subject: [PATCH 12/14] Worked on Shuffleboard --- src/main/java/frc/robot/Constants.java | 6 ++ .../robot/subsystems/ShuffleboardSubsystem | 97 ++++++++++++------- 2 files changed, 70 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 089cd40..446acb6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,6 +29,12 @@ public static final class ModuleConstants { public static final double kPModuleDriveController = 1; } + public static final class DriveEncoders { + public static final double FRONTLEFT_DRIVE_ENCODER = 0; + public static final double FRONTRIGHT_DRIVE_ENCODER = 0; + public static final double BACKLEFT_DRIVE_ENCODER = 0; + public static final double BACKRIGHT_DRIVE_ENCODER = 0; + } public static final class Hook { public static final int MOTOR_CAN_ID = 3; // Defines the CAN id of the Spark Max motor controller public static final double EXTENDED_HOOK_HEIGHT = 5; //Defines Extended Height for Hook diff --git a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem index 69cef56..8795a32 100644 --- a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem +++ b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem @@ -31,58 +31,89 @@ public class ShuffleboardSubsystem extends SubsystemBase { .withSize(20,2) .withPosition(960,0); - private GenericEntry hookLift = driverDashboard.add("HIGH", false) - .withSize(2, 1) - .withPosition(0, 0) - .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + //Hook + private GenericEntry hookLift = driverDashboard.add("LiftingHook", false) + .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,0) .getEntry(); - - private GenericEntry mid = driverDashboard.add("MID", false) - .withSize(2, 1) - .withPosition(0, 1) - .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + private GenericEntry hookExtend = driverDashboard.add("ExtendingHook", false) + .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,275) .getEntry(); - - private GenericEntry low = driverDashboard.add("LOW", false) - .withSize(2, 1) - .withPosition(0, 2) - .withProperties(Map.of("Color when false", "#000000", "Color when true", "#17FC03")) + private GenericEntry hookWithdraw = driverDashboard.add("WithdrawingHook", false) + .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", -5)) + .withSize(330,270) + .withPosition(500,550) .getEntry(); - private GenericEntry desiredGamePiece = driverDashboard.add("Game Piece", false) - .withSize(2, 1) - .withPosition(2, 0) - .withProperties(Map.of("Color when false", "#7842f5", "Color when true", "#f5ef42")) + //Drive Encoders + private GenericEntry driveEncoderFL = driverDashboard.add("Front Left Drive Encoder") + .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,0) .getEntry(); - - private GenericEntry desiredIntakePosition = driverDashboard - .add("DESIRED INTAKE", IntakePositions.SHOOT_TALL.toString()) - .withSize(2, 1) - .withPosition(2, 2) + private GenericEntry driveEncoderFR = driverDashboard.add("Front Right Drive Encoder") + .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,270) + .getEntry(); + private GenericEntry driveEncoderBL = driverDashboard.add("Back Left Drive Encoder") + .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,540) + .getEntry(); + private GenericEntry driveEncoderBR = driverDashboard.add("Back Right Drive Encoder") + .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,810) + .getEntry(); + + private GenericEntry speed = driverDashboard.add("Speed") + .addPersistent("Speed", SPEED) + .withSize(330,270) + .withPosition(500,825) .getEntry(); + private GenericEntry shootAngle = driverDashBoard.add("Shooter Angle") + .addPersistent("Shooter Angle", SHOOTER_ANGLE) + .withSize(800,530) + .withPosition(850,0) + .getEntry(); + private GenericEntry shootAngle = driverDashBoard.add("Swerve Angle") + .addPersistent("Swerve Angle", SWERVE_ANGLE) + .withSize(800,530) + .withPosition(850,545) + .getEntry(); - @Override + /*@Override public void periodic() { VerticalLocations currentVerticalLocation = stateHandler.getCurrentVerticalLocation(); - if (currentVerticalLocation == VerticalLocations.HIGH) { - high.setBoolean(true); + if (currentVerticalLocation == VerticalLocations.LiftingHook) { + hookLift.setBoolean(true); } else { - high.setBoolean(false); + hookLift.setBoolean(false); } - if (currentVerticalLocation == VerticalLocations.MID) { - mid.setBoolean(true); + if (currentVerticalLocation == VerticalLocations.ExtendingHook) { + hookExtend.setBoolean(true); } else { - mid.setBoolean(false); + hookExtend.setBoolean(false); } - if (currentVerticalLocation == VerticalLocations.LOW) { - low.setBoolean(true); + if (currentVerticalLocation == VerticalLocations.WithdrawingHook) { + hookWithdraw.setBoolean(true); } else { - low.setBoolean(false); + hookWithdraw.setBoolean(false); } GamePieceMode currentGamePiece = stateHandler.getGamePieceMode(); From 73745ab99948b0400767c183cbf32185da3da777 Mon Sep 17 00:00:00 2001 From: TJbots15 Date: Fri, 2 Feb 2024 16:15:20 -0500 Subject: [PATCH 13/14] Added new entry Distance to Target, + Drive Encoder tabs have doubles to get data now --- src/main/java/frc/robot/Constants.java | 4 ++ .../robot/subsystems/ShuffleboardSubsystem | 38 +++++++++++++------ 2 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e5ca51..e80417f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,6 +29,10 @@ public static final class ModuleConstants { public static final double kPModuleDriveController = 1; } + public static final class Vision{ + public static final double DistanceFromTarget = 0; + } + public static final class DriveEncoders { public static final double FRONTLEFT_DRIVE_ENCODER = 0; public static final double FRONTRIGHT_DRIVE_ENCODER = 0; diff --git a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem index 8795a32..03a969f 100644 --- a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem +++ b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem @@ -12,6 +12,8 @@ import frc.robot.util.StateVariables.IntakePositions; import frc.robot.util.StateVariables.VerticalLocations; import frc.robot.Constants; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; //importing SUBSYSTEMS import frc.robot.subsystems.ArmSubsystem; @@ -59,22 +61,22 @@ public class ShuffleboardSubsystem extends SubsystemBase { .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) .withSize(495,269) .withPosition(0,0) - .getEntry(); + double frontleftDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); private GenericEntry driveEncoderFR = driverDashboard.add("Front Right Drive Encoder") .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) .withSize(495,269) .withPosition(0,270) - .getEntry(); + double FrontRightDriveEncoder = smartDashboard.getEntry("Front Right Drive Encoder").getDouble(); private GenericEntry driveEncoderBL = driverDashboard.add("Back Left Drive Encoder") .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) .withSize(495,269) - .withPosition(0,540) - .getEntry(); + .withPosition(0,540) + double backleftDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); private GenericEntry driveEncoderBR = driverDashboard.add("Back Right Drive Encoder") .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) .withSize(495,269) .withPosition(0,810) - .getEntry(); + double backrightDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); private GenericEntry speed = driverDashboard.add("Speed") .addPersistent("Speed", SPEED) @@ -94,7 +96,20 @@ public class ShuffleboardSubsystem extends SubsystemBase { .withPosition(850,545) .getEntry(); - /*@Override + class VisionCalculator { + private ShuffleboardTab tab = Shuffleboard.getTab("Vision"); + private NetworkTableEntry distanceEntry = + tab.add("Distance to target", DistanceFromTarget) + .withSize(4,2) + .withPosition(1916, 1078) + .getEntry(); + + public void calculate() { + double distance = ...; + distanceEntry.setDouble(distance); + } + } + @Override public void periodic() { VerticalLocations currentVerticalLocation = stateHandler.getCurrentVerticalLocation(); @@ -116,12 +131,11 @@ public class ShuffleboardSubsystem extends SubsystemBase { hookWithdraw.setBoolean(false); } - GamePieceMode currentGamePiece = stateHandler.getGamePieceMode(); - if (currentGamePiece == GamePieceMode.CONE) { - desiredGamePiece.setBoolean(true); - } else { - desiredGamePiece.setBoolean(false); - } + double leftDriveEncoder = smartDashboard.getEntry("Left Drive Encoder").getDouble(); + double rightDriveEncoder = smartDashboard.getEntry("Right Drive Encoder").getDouble(); + + System.out.println("Left Drive Encoder: " + leftDriveEncoder); + System.out.println("Right Drive Encoder: " + rightDriveEncoder); desiredIntakePosition.setString(stateHandler.getDesiredIntakePosition().toString()); From 544bba26f3b398fcf86c73c2dbd74c42a641e68b Mon Sep 17 00:00:00 2001 From: neev Date: Fri, 2 Feb 2024 16:35:21 -0500 Subject: [PATCH 14/14] auto work --- autonomous.chor | 191 +++++++------ src/main/deploy/choreo/TestingPath.traj | 181 ++++++------ src/main/java/frc/robot/Constants.java | 10 +- .../frc/robot/subsystems/HookSubsystem.java | 258 +++++++++--------- 4 files changed, 329 insertions(+), 311 deletions(-) diff --git a/autonomous.chor b/autonomous.chor index 50430b1..beb797d 100644 --- a/autonomous.chor +++ b/autonomous.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 37.04398850154597, "rotationalInertia": 3, - "motorMaxTorque": 0.18121546961325966, - "motorMaxVelocity": 4704, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 2822, "gearing": 8.14, "wheelbase": 0.6667496399551944, "trackWidth": 0.6667496399551944, @@ -22,11 +22,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 0, - "y": 1, + "y": 2, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -39,118 +39,127 @@ "x": 0, "y": 0, "heading": 0, - "angularVelocity": -7.600376309198775e-18, - "velocityX": 9.47848126641677e-34, - "velocityY": 2.3685455382964595, + "angularVelocity": -3.947033998189832e-17, + "velocityX": -3.612854008750732e-31, + "velocityY": 1.2005140973885657, "timestamp": 0 }, { - "x": 3.444002287789685e-36, - "y": 0.16666663815137714, - "heading": -2.2326890701092606e-22, - "angularVelocity": -3.2118068414505653e-21, - "velocityX": 4.9543256977745e-35, - "velocityY": 2.3975617978327155, - "timestamp": 0.0695150541279211 + "x": 1.1944785629958918e-34, + "y": 0.17191154064183406, + "heading": 8.946622442095673e-18, + "angularVelocity": 9.597955586605163e-17, + "velocityX": 1.2814391555427755e-33, + "velocityY": 1.8442706648059815, + "timestamp": 0.0932138345647434 }, { - "x": 1.80940340937385e-36, - "y": 0.31818178117808316, - "heading": -8.171022190596369e-22, - "angularVelocity": -8.542513749172842e-21, - "velocityX": -2.3514315537398912e-35, - "velocityY": 2.1796018851958148, - "timestamp": 0.1390301082558422 + "x": 3.0218756793156384e-34, + "y": 0.34382308128367217, + "heading": -1.6139197453297265e-18, + "angularVelocity": -1.132937212242932e-16, + "velocityX": 1.960435468763232e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.1864276691294868 }, { - "x": 5.9379803896552905e-37, - "y": 0.4545454170640189, - "heading": -1.0153633602375655e-21, - "angularVelocity": -2.8520602469919215e-21, - "velocityX": -1.7486936861866492e-35, - "velocityY": 1.9616417997028441, - "timestamp": 0.2085451623837633 + "x": 5.424652677084512e-34, + "y": 0.5157346219255102, + "heading": -1.5884909113524752e-17, + "angularVelocity": -1.5309947750602212e-16, + "velocityX": 2.577704286588845e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.2796415036942302 }, { - "x": 9.346108964575416e-37, - "y": 0.575757541803813, - "heading": -9.823771839587742e-22, - "angularVelocity": 4.745184644957e-22, - "velocityX": 4.902720218198206e-36, - "velocityY": 1.7436816565911075, - "timestamp": 0.2780602165116844 + "x": 8.747257947587387e-34, + "y": 0.6876461625673483, + "heading": 1.7441665329815146e-18, + "angularVelocity": 1.8912509853096619e-16, + "velocityX": 3.564497995151033e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.3728553382589736 }, { - "x": 2.094062831073701e-36, - "y": 0.6818181533947788, - "heading": -8.40480447256135e-22, - "angularVelocity": 2.0412375100710908e-21, - "velocityX": 1.6679149082686126e-35, - "velocityY": 1.5257214846699805, - "timestamp": 0.34757527063960547 + "x": 9.332526707530359e-34, + "y": 0.8595577032091865, + "heading": 4.4302423015963514e-18, + "angularVelocity": 2.881627798231134e-17, + "velocityX": 6.278775686801844e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.466069172823717 }, { - "x": 4.0842527693840064e-36, - "y": 0.7727272506353046, - "heading": -6.555813623569812e-22, - "angularVelocity": 2.6598423207322688e-21, - "velocityX": 2.8629625672203317e-35, - "velocityY": 1.3077612954632174, - "timestamp": 0.41709032476752655 + "x": 7.404560247182679e-34, + "y": 1.0314692438510245, + "heading": -1.9174121317431025e-17, + "angularVelocity": -2.532281150029562e-16, + "velocityX": -2.0683265264103736e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.5592830073884604 }, { - "x": -2.861998005146919e-37, - "y": 0.8484848327243157, - "heading": -4.732144278399494e-22, - "angularVelocity": 2.6234163690356904e-21, - "velocityX": -6.287059025950303e-35, - "velocityY": 1.0898010947326966, - "timestamp": 0.48660537889544764 + "x": 6.388428375393719e-34, + "y": 1.2033807844928626, + "heading": -1.926556919165743e-17, + "angularVelocity": -9.810547399257286e-19, + "velocityX": -1.0901084402040666e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.6524968419532038 }, { - "x": -2.03857738269056e-36, - "y": 0.9090908990896158, - "heading": -3.0214961018999903e-22, - "angularVelocity": 2.4608311899881687e-21, - "velocityX": -2.520860526747833e-35, - "velocityY": 0.87184088577092, - "timestamp": 0.5561204330233688 + "x": 3.0162184012138363e-34, + "y": 1.3752923251347007, + "heading": 4.527535991323288e-18, + "angularVelocity": 2.5525293851584663e-16, + "velocityX": -3.6177140401587575e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.7457106765179472 }, { - "x": 5.106160606722849e-38, - "y": 0.9545454493020581, - "heading": -1.6145441512322172e-22, - "angularVelocity": 2.0239528885973894e-21, - "velocityX": 3.006023625587289e-35, - "velocityY": 0.6538806706357015, - "timestamp": 0.6256354871512899 + "x": 3.354628939612959e-34, + "y": 1.5472038657765388, + "heading": 3.130040657204647e-17, + "angularVelocity": 2.872199250866309e-16, + "velocityX": 3.63047576810033e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.8389245110826906 }, { - "x": 1.7914848768309607e-36, - "y": 0.984848483027861, - "heading": -5.689927869393762e-23, - "angularVelocity": 1.5040646843361851e-21, - "velocityX": 2.5036637789133933e-35, - "velocityY": 0.43592045069891716, - "timestamp": 0.695150541279211 + "x": -2.3908944051347553e-34, + "y": 1.719115406418377, + "heading": 6.073122238228363e-17, + "angularVelocity": 3.157344180470925e-16, + "velocityX": -6.163809634413478e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.932138345647434 }, { - "x": 2.176452252156848e-48, - "y": 1, - "heading": 5.357751950103021e-41, - "angularVelocity": 8.185174103588329e-22, - "velocityX": -2.577117806968029e-35, - "velocityY": 0.21796022692088016, - "timestamp": 0.7646655954071322 + "x": -6.918462356301768e-36, + "y": 1.8910269470602104, + "heading": 4.515056239569573e-17, + "angularVelocity": -1.6714965175867783e-16, + "velocityX": 2.490735185578158e-33, + "velocityY": 1.8442706648059772, + "timestamp": 1.0253521802121774 }, { "x": 0, - "y": 1, - "heading": 1.7637632388066702e-41, - "angularVelocity": -5.169863684148192e-40, - "velocityX": -3.141686539318957e-47, - "velocityY": 5.499408435381289e-40, - "timestamp": 0.8341806495350533 + "y": 2, + "heading": 0, + "angularVelocity": -4.843761937970241e-16, + "velocityX": 7.422140407242795e-35, + "velocityY": 1.1690652299482462, + "timestamp": 1.1185660147769207 + }, + { + "x": 0, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.211779849341664 } ], "constraints": [ @@ -159,7 +168,7 @@ 1 ], "type": "StopPoint", - "uuid": "4e292adc-83c9-4a24-8725-0a82efddefef" + "uuid": "40f8b823-8306-4dc8-8a96-0fc4aeda883f" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/TestingPath.traj b/src/main/deploy/choreo/TestingPath.traj index 219a707..12599ab 100644 --- a/src/main/deploy/choreo/TestingPath.traj +++ b/src/main/deploy/choreo/TestingPath.traj @@ -4,118 +4,127 @@ "x": 0, "y": 0, "heading": 0, - "angularVelocity": -7.600376309198775e-18, - "velocityX": 9.47848126641677e-34, - "velocityY": 2.3685455382964595, + "angularVelocity": -3.947033998189832e-17, + "velocityX": -3.612854008750732e-31, + "velocityY": 1.2005140973885657, "timestamp": 0 }, { - "x": 3.444002287789685e-36, - "y": 0.16666663815137714, - "heading": -2.2326890701092606e-22, - "angularVelocity": -3.2118068414505653e-21, - "velocityX": 4.9543256977745e-35, - "velocityY": 2.3975617978327155, - "timestamp": 0.0695150541279211 + "x": 1.1944785629958918e-34, + "y": 0.17191154064183406, + "heading": 8.946622442095673e-18, + "angularVelocity": 9.597955586605163e-17, + "velocityX": 1.2814391555427755e-33, + "velocityY": 1.8442706648059815, + "timestamp": 0.0932138345647434 }, { - "x": 1.80940340937385e-36, - "y": 0.31818178117808316, - "heading": -8.171022190596369e-22, - "angularVelocity": -8.542513749172842e-21, - "velocityX": -2.3514315537398912e-35, - "velocityY": 2.1796018851958148, - "timestamp": 0.1390301082558422 + "x": 3.0218756793156384e-34, + "y": 0.34382308128367217, + "heading": -1.6139197453297265e-18, + "angularVelocity": -1.132937212242932e-16, + "velocityX": 1.960435468763232e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.1864276691294868 }, { - "x": 5.9379803896552905e-37, - "y": 0.4545454170640189, - "heading": -1.0153633602375655e-21, - "angularVelocity": -2.8520602469919215e-21, - "velocityX": -1.7486936861866492e-35, - "velocityY": 1.9616417997028441, - "timestamp": 0.2085451623837633 + "x": 5.424652677084512e-34, + "y": 0.5157346219255102, + "heading": -1.5884909113524752e-17, + "angularVelocity": -1.5309947750602212e-16, + "velocityX": 2.577704286588845e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.2796415036942302 }, { - "x": 9.346108964575416e-37, - "y": 0.575757541803813, - "heading": -9.823771839587742e-22, - "angularVelocity": 4.745184644957e-22, - "velocityX": 4.902720218198206e-36, - "velocityY": 1.7436816565911075, - "timestamp": 0.2780602165116844 + "x": 8.747257947587387e-34, + "y": 0.6876461625673483, + "heading": 1.7441665329815146e-18, + "angularVelocity": 1.8912509853096619e-16, + "velocityX": 3.564497995151033e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.3728553382589736 }, { - "x": 2.094062831073701e-36, - "y": 0.6818181533947788, - "heading": -8.40480447256135e-22, - "angularVelocity": 2.0412375100710908e-21, - "velocityX": 1.6679149082686126e-35, - "velocityY": 1.5257214846699805, - "timestamp": 0.34757527063960547 + "x": 9.332526707530359e-34, + "y": 0.8595577032091865, + "heading": 4.4302423015963514e-18, + "angularVelocity": 2.881627798231134e-17, + "velocityX": 6.278775686801844e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.466069172823717 }, { - "x": 4.0842527693840064e-36, - "y": 0.7727272506353046, - "heading": -6.555813623569812e-22, - "angularVelocity": 2.6598423207322688e-21, - "velocityX": 2.8629625672203317e-35, - "velocityY": 1.3077612954632174, - "timestamp": 0.41709032476752655 + "x": 7.404560247182679e-34, + "y": 1.0314692438510245, + "heading": -1.9174121317431025e-17, + "angularVelocity": -2.532281150029562e-16, + "velocityX": -2.0683265264103736e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.5592830073884604 }, { - "x": -2.861998005146919e-37, - "y": 0.8484848327243157, - "heading": -4.732144278399494e-22, - "angularVelocity": 2.6234163690356904e-21, - "velocityX": -6.287059025950303e-35, - "velocityY": 1.0898010947326966, - "timestamp": 0.48660537889544764 + "x": 6.388428375393719e-34, + "y": 1.2033807844928626, + "heading": -1.926556919165743e-17, + "angularVelocity": -9.810547399257286e-19, + "velocityX": -1.0901084402040666e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.6524968419532038 }, { - "x": -2.03857738269056e-36, - "y": 0.9090908990896158, - "heading": -3.0214961018999903e-22, - "angularVelocity": 2.4608311899881687e-21, - "velocityX": -2.520860526747833e-35, - "velocityY": 0.87184088577092, - "timestamp": 0.5561204330233688 + "x": 3.0162184012138363e-34, + "y": 1.3752923251347007, + "heading": 4.527535991323288e-18, + "angularVelocity": 2.5525293851584663e-16, + "velocityX": -3.6177140401587575e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.7457106765179472 }, { - "x": 5.106160606722849e-38, - "y": 0.9545454493020581, - "heading": -1.6145441512322172e-22, - "angularVelocity": 2.0239528885973894e-21, - "velocityX": 3.006023625587289e-35, - "velocityY": 0.6538806706357015, - "timestamp": 0.6256354871512899 + "x": 3.354628939612959e-34, + "y": 1.5472038657765388, + "heading": 3.130040657204647e-17, + "angularVelocity": 2.872199250866309e-16, + "velocityX": 3.63047576810033e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.8389245110826906 }, { - "x": 1.7914848768309607e-36, - "y": 0.984848483027861, - "heading": -5.689927869393762e-23, - "angularVelocity": 1.5040646843361851e-21, - "velocityX": 2.5036637789133933e-35, - "velocityY": 0.43592045069891716, - "timestamp": 0.695150541279211 + "x": -2.3908944051347553e-34, + "y": 1.719115406418377, + "heading": 6.073122238228363e-17, + "angularVelocity": 3.157344180470925e-16, + "velocityX": -6.163809634413478e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.932138345647434 }, { - "x": 2.176452252156848e-48, - "y": 1, - "heading": 5.357751950103021e-41, - "angularVelocity": 8.185174103588329e-22, - "velocityX": -2.577117806968029e-35, - "velocityY": 0.21796022692088016, - "timestamp": 0.7646655954071322 + "x": -6.918462356301768e-36, + "y": 1.8910269470602104, + "heading": 4.515056239569573e-17, + "angularVelocity": -1.6714965175867783e-16, + "velocityX": 2.490735185578158e-33, + "velocityY": 1.8442706648059772, + "timestamp": 1.0253521802121774 }, { "x": 0, - "y": 1, - "heading": 1.7637632388066702e-41, - "angularVelocity": -5.169863684148192e-40, - "velocityX": -3.141686539318957e-47, - "velocityY": 5.499408435381289e-40, - "timestamp": 0.8341806495350533 + "y": 2, + "heading": 0, + "angularVelocity": -4.843761937970241e-16, + "velocityX": 7.422140407242795e-35, + "velocityY": 1.1690652299482462, + "timestamp": 1.1185660147769207 + }, + { + "x": 0, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.211779849341664 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9e5ca51..93372df 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -174,8 +174,8 @@ public static final class Mod1 { /* Back Left Module - Module 2 */ public static final class Mod2 { - public static final int driveMotorID = 8; - public static final int angleMotorID = 7; + public static final int driveMotorID = 7; + public static final int angleMotorID = 8; //public static final int canCoderID = 3; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(470.5); //public static final Rotation2d angleOffset = Rotation2d.fromDegrees(204.7); @@ -201,9 +201,9 @@ public static final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = 2 * Math.PI; - public static final double kPXController = 0.3; - public static final double kPYController = 0.3; - public static final double kPThetaController = 0.3; + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; // Constraint for the motion profilied robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = diff --git a/src/main/java/frc/robot/subsystems/HookSubsystem.java b/src/main/java/frc/robot/subsystems/HookSubsystem.java index ac4c9c1..d9c6456 100644 --- a/src/main/java/frc/robot/subsystems/HookSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HookSubsystem.java @@ -1,129 +1,129 @@ -package frc.robot.subsystems; -import java.util.*; -import com.revrobotics.*; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class HookSubsystem extends SubsystemBase { - private CANSparkMax hookMotor; // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller - private RelativeEncoder hookEncoder; // Grab the attached encoder - private final SparkPIDController hookController; - - public HookSubsystem() { - hookMotor = new CANSparkMax(Constants.Hook.MOTOR_CAN_ID, MotorType.kBrushless); // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller - hookEncoder = hookMotor.getEncoder(); // Grab the attached encoder - hookController = hookMotor.getPIDController(); - hookMotor.restoreFactoryDefaults(); - hookMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); - hookMotor.setInverted(Constants.Shooter.driveInvert); - hookMotor.setIdleMode(Constants.Shooter.driveNeutralMode); - hookController.setP(1.0); - hookController.setI(0); - hookController.setD(0); - hookController.setFF(0); - hookMotor.burnFlash(); - hookEncoder.setPosition(0.0); - } - - - - - - double i = hookEncoder.getPosition(); - - //raises the hook motor - public void liftHook() { - //private CANSparkMax hookRaising = new CANSparkMax(Constants.MAXIMUMHOOK_HEIGHT, MotorType.kBrushless); - System.out.println("Raising Hook"); - hookController.setReference(Constants.Hook.LIFT_HOOK_HEIGHT, ControlType.kPosition); - } - - //lowers the hook motor - public void downHook() { - //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); - System.out.println("Down Hook"); - hookController.setReference(Constants.Hook.WITHDRAWN_HOOK_HEIGHT, ControlType.kPosition); - } - - //lowers the hook motor - public void extendHook() { - //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); - System.out.println("Down Hook"); - hookController.setReference(Constants.Hook.EXTENDED_HOOK_HEIGHT, ControlType.kPosition); - } - - //Shuffleboard - protected void execute() { - SmartDashboard.putNumber("Hook Lifting", hookEncoder.getPosition()); - SmartDashboard.putNumber("Hook Extension", hookEncoder.getPosition()); - SmartDashboard.putNumber("Hook Withdrawn", hookEncoder.getPosition()); - //SmartDashboard.putNumber("Shooter Angle", .getPosition()); - //SmartDashboard.putNumber("Speed", ); - //SmartDashboard.putNumber("Front Left Drive Encoder", 0); - //SmartDashboard.putNumber("Front Right Drive Encoder", 0); - //SmartDashboard.putNumber("Back Left Drive Encoder", 0); - //SmartDashboard.putNumber("Back Right Drive Encoder", 0); - //SmartDashboard.putNumber("Swerve Angle", 0); - } - Shuffleboard.getTab("Hook Lifting") - .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("min", 0, "max", 5)) - .withSize(330,270) - .withPosition(500,0) - .getEntry(); - Shuffleboard.getTab("Hook Extension") - .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("min", 0, "max", 5)) - .withSize(330,270) - .withPosition(500,275) - .getEntry(); - Shuffleboard.getTab("Hook Withdrawn") - .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) - .withWidget(BuiltInWidgets.kNumberSlider) - .withProperties(Map.of("min", 0, "max", -5)) - .withSize(330,270) - .withPosition(500,550) - .getEntry(); - Shuffleboard.getTab("Shooter Angle") - .addPersistent("Shooter Angle", SHOOTER_ANGLE) - .withSize(800,530) - .withPosition(850,0) - .getEntry(); - Shuffleboard.getTab("Speed") - .addPersistent("Speed", SPEED) - .withSize(330,270) - .withPosition(500,825) - .getEntry(); - Shuffleboard.getTab("Front Left Drive Encoder") - .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) - .withSize(495,269) - .withPosition(0,0) - .getEntry(); - Shuffleboard.getTab("Front Right Drive Encoder") - .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) - .withSize(495,269) - .withPosition(0,270) - .getEntry(); - Shuffleboard.getTab("Back Left Drive Encoder") - .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) - .withSize(495,269) - .withPosition(0,540) - .getEntry(); - Shuffleboard.getTab("Right Drive Encoder") - .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) - .withSize(495,269) - .withPosition(0,810) - .getEntry(); - Shuffleboard.getTab("Swerve Angle") - .addPersistent("Swerve Angle", SWERVE_ANGLE) - .withSize(800,530) - .withPosition(850,545) - .getEntry(); -} +//package frc.robot.subsystems; +//import java.util.*; +//import com.revrobotics.*; +//import com.revrobotics.CANSparkLowLevel.MotorType; +// +//import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +//import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +//import edu.wpi.first.wpilibj2.command.SubsystemBase; +//import frc.robot.Constants; +// +//public class HookSubsystem extends SubsystemBase { +// private CANSparkMax hookMotor; // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller +// private RelativeEncoder hookEncoder; // Grab the attached encoder +// private final SparkPIDController hookController; +// +// public HookSubsystem() { +// hookMotor = new CANSparkMax(Constants.Hook.MOTOR_CAN_ID, MotorType.kBrushless); // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller +// hookEncoder = hookMotor.getEncoder(); // Grab the attached encoder +// hookController = hookMotor.getPIDController(); +// hookMotor.restoreFactoryDefaults(); +// hookMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); +// hookMotor.setInverted(Constants.Shooter.driveInvert); +// hookMotor.setIdleMode(Constants.Shooter.driveNeutralMode); +// hookController.setP(1.0); +// hookController.setI(0); +// hookController.setD(0); +// hookController.setFF(0); +// hookMotor.burnFlash(); +// hookEncoder.setPosition(0.0); +// } +// +// +// +// +// +// double i = hookEncoder.getPosition(); +// +// //raises the hook motor +// public void liftHook() { +// //private CANSparkMax hookRaising = new CANSparkMax(Constants.MAXIMUMHOOK_HEIGHT, MotorType.kBrushless); +// System.out.println("Raising Hook"); +// hookController.setReference(Constants.Hook.LIFT_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //lowers the hook motor +// public void downHook() { +// //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); +// System.out.println("Down Hook"); +// hookController.setReference(Constants.Hook.WITHDRAWN_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //lowers the hook motor +// public void extendHook() { +// //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); +// System.out.println("Down Hook"); +// hookController.setReference(Constants.Hook.EXTENDED_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //Shuffleboard +// protected void execute() { +// SmartDashboard.putNumber("Hook Lifting", hookEncoder.getPosition()); +// SmartDashboard.putNumber("Hook Extension", hookEncoder.getPosition()); +// SmartDashboard.putNumber("Hook Withdrawn", hookEncoder.getPosition()); +// //SmartDashboard.putNumber("Shooter Angle", .getPosition()); +// //SmartDashboard.putNumber("Speed", ); +// //SmartDashboard.putNumber("Front Left Drive Encoder", 0); +// //SmartDashboard.putNumber("Front Right Drive Encoder", 0); +// //SmartDashboard.putNumber("Back Left Drive Encoder", 0); +// //SmartDashboard.putNumber("Back Right Drive Encoder", 0); +// //SmartDashboard.putNumber("Swerve Angle", 0); +// } +// Shuffleboard.getTab("Hook Lifting") +// .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", 5)) +// .withSize(330,270) +// .withPosition(500,0) +// .getEntry(); +// Shuffleboard.getTab("Hook Extension") +// .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", 5)) +// .withSize(330,270) +// .withPosition(500,275) +// .getEntry(); +// Shuffleboard.getTab("Hook Withdrawn") +// .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", -5)) +// .withSize(330,270) +// .withPosition(500,550) +// .getEntry(); +// Shuffleboard.getTab("Shooter Angle") +// .addPersistent("Shooter Angle", SHOOTER_ANGLE) +// .withSize(800,530) +// .withPosition(850,0) +// .getEntry(); +// Shuffleboard.getTab("Speed") +// .addPersistent("Speed", SPEED) +// .withSize(330,270) +// .withPosition(500,825) +// .getEntry(); +// Shuffleboard.getTab("Front Left Drive Encoder") +// .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,0) +// .getEntry(); +// Shuffleboard.getTab("Front Right Drive Encoder") +// .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,270) +// .getEntry(); +// Shuffleboard.getTab("Back Left Drive Encoder") +// .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,540) +// .getEntry(); +// Shuffleboard.getTab("Right Drive Encoder") +// .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,810) +// .getEntry(); +// Shuffleboard.getTab("Swerve Angle") +// .addPersistent("Swerve Angle", SWERVE_ANGLE) +// .withSize(800,530) +// .withPosition(850,545) +// .getEntry(); +//}