From 7af6b4f242602b03be9108d371d477ad8b9d7add Mon Sep 17 00:00:00 2001 From: IAmRice33 Date: Sat, 17 Feb 2024 17:29:20 -0500 Subject: [PATCH 01/31] Intake 2024 - First Iteration Needs PID and better controls --- build.gradle | 2 +- .../java/frc/team7520/robot/Constants.java | 10 ++ .../frc/team7520/robot/RobotContainer.java | 28 ++++++ .../subsystems/Intake/IntakeRollers.java | 64 +++++++++++++ .../subsystems/Intake/IntakeTurning.java | 95 +++++++++++++++++++ 5 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java create mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java diff --git a/build.gradle b/build.gradle index d25e5bd..72793ee 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "idea" } diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 9880d61..a9af500 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -41,6 +41,7 @@ public static final class Drivebase { public static class OperatorConstants { // Joystick Ports public static final int DRIVER_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; // Joystick Deadband public static final double LEFT_X_DEADBAND = 0.01; @@ -59,4 +60,13 @@ public static class Telemetry { // change at comp to low public static final SwerveDriveTelemetry.TelemetryVerbosity SWERVE_VERBOSITY = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; } + + public static class IntakeConstants { + public static class turn { + public static final int CAN_ID = 23; + } + public static class rollers { + public static final int CAN_ID = 22; + } + } } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 90cff51..663b71f 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.TeleopDrive; +import frc.team7520.robot.subsystems.Intake.IntakeRollers; +import frc.team7520.robot.subsystems.Intake.IntakeTurning; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; @@ -39,6 +41,16 @@ public class RobotContainer // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); + private final XboxController operatorController = + new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); + + JoystickButton Shoot = new JoystickButton(operatorController, XboxController.Button.kY.value); + JoystickButton Floor = new JoystickButton(operatorController, XboxController.Button.kA.value); + + JoystickButton Stop = new JoystickButton(operatorController, XboxController.Button.kX.value); + JoystickButton Intake = new JoystickButton(operatorController, XboxController.Button.kB.value); + JoystickButton ShootHalf = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); + JoystickButton ShootFull = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -103,6 +115,22 @@ private void configureBindings() // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new InstantCommand(drivebase::lock)); + + IntakeRollers.getInstance().setDefaultCommand(IntakeRollers.getInstance().ControlledShooting( + () -> operatorController.getRawAxis(XboxController.Axis.kLeftTrigger.value) + )); + + Intake.whileTrue(IntakeRollers.getInstance().Intake()); + ShootFull.whileTrue(IntakeRollers.getInstance().ShootFull()); + ShootHalf.whileTrue(IntakeRollers.getInstance().ShootHalf()); + Stop.whileTrue(IntakeRollers.getInstance().Stop()); + + IntakeTurning.getInstance().setDefaultCommand(IntakeTurning.getInstance().Manual( + () -> operatorController.getRawAxis(XboxController.Axis.kLeftY.value) + )); + + Shoot.whileTrue(IntakeTurning.getInstance().Shoot()); + Floor.whileTrue(IntakeTurning.getInstance().Intake()); } diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java new file mode 100644 index 0000000..d8f0daf --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java @@ -0,0 +1,64 @@ +// 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.team7520.robot.subsystems.Intake; + +import java.util.function.DoubleSupplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; + +public class IntakeRollers extends SubsystemBase { + + public CANSparkMax rollers = new CANSparkMax(Constants.IntakeConstants.rollers.CAN_ID, MotorType.kBrushless); + + private final static IntakeRollers INSTANCE = new IntakeRollers(); + + @SuppressWarnings("WeakerAccess") + public static IntakeRollers getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + public IntakeRollers() {} + + public InstantCommand ShootFull() { + return new InstantCommand( + () -> { + rollers.set(-1); + }); + } + + public InstantCommand ShootHalf() { + return new InstantCommand( + () -> { + rollers.set(-0.5); + }); + } + + public InstantCommand Stop() { + return new InstantCommand( + () -> { + rollers.set(0); + }); + } + + public InstantCommand Intake() { + return new InstantCommand( + () -> { + rollers.set(0.35); + }); + } + + public InstantCommand ControlledShooting(DoubleSupplier BumpVal) { + return new InstantCommand( + () -> { + rollers.set(-0.9 * BumpVal.getAsDouble()); + }); + } +} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java new file mode 100644 index 0000000..9950735 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java @@ -0,0 +1,95 @@ +// 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.team7520.robot.subsystems.Intake; + +import java.util.function.DoubleSupplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; + +public class IntakeTurning extends SubsystemBase { + + public CANSparkMax turn = new CANSparkMax(Constants.IntakeConstants.turn.CAN_ID, MotorType.kBrushless); + private RelativeEncoder turnEncoder; + private final SparkPIDController turnPID; + + public double currentPosition = Position.SHOOT; + + class Position { + public static double FLOOR = 51.78583526611328; //not exact + public static double SHOOT = 0.238095209002495; //not exact + } + + private final static IntakeTurning INSTANCE = new IntakeTurning(); + + @SuppressWarnings("WeakerAccess") + public static IntakeTurning getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + public IntakeTurning() { + this.turnEncoder = turn.getEncoder(); + this.turnPID = turn.getPIDController(); + + turnPID.setP(0.15); + turnPID.setI(0); + turnPID.setD(0); + turnPID.setFF(0.1); + turnPID.setOutputRange(-1, 1); + + turn.setIdleMode(CANSparkMax.IdleMode.kBrake); + } + + public void setPosition(double position){ + currentPosition = position; + } + + public Command Shoot() { + return runOnce( + () -> { + setPosition(Position.SHOOT); + }).andThen(MoveIntake()); + } + + public Command Intake() { + return runOnce( + () -> { + setPosition(Position.FLOOR); + }).andThen(MoveIntake()); + } + + public Command Manual(DoubleSupplier turn) { + return runOnce( + () -> { + double turnVal = turn.getAsDouble(); + + if(Math.abs(turnVal) > 0.05) { + currentPosition = currentPosition + 1 * turnVal; + } + }).andThen(MoveIntake()); + } + + public Command MoveIntake() { + return runOnce( + () -> { + turnPID.setReference(currentPosition, ControlType.kPosition); + } + ); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("TurnEncoder", turnEncoder.getPosition()); + } +} \ No newline at end of file From 763b7b14ee10063da7aea2104309b5250dbe2eef Mon Sep 17 00:00:00 2001 From: IAmRice33 Date: Sat, 17 Feb 2024 17:47:53 -0500 Subject: [PATCH 02/31] Fixed issue with code (Not deploying) There is no default command for IntakeRollers --- src/main/java/frc/team7520/robot/RobotContainer.java | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 663b71f..ff63419 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -49,7 +49,7 @@ public class RobotContainer JoystickButton Stop = new JoystickButton(operatorController, XboxController.Button.kX.value); JoystickButton Intake = new JoystickButton(operatorController, XboxController.Button.kB.value); - JoystickButton ShootHalf = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); + JoystickButton ControlledShooting = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); JoystickButton ShootFull = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value); @@ -115,14 +115,11 @@ private void configureBindings() // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new InstantCommand(drivebase::lock)); - - IntakeRollers.getInstance().setDefaultCommand(IntakeRollers.getInstance().ControlledShooting( - () -> operatorController.getRawAxis(XboxController.Axis.kLeftTrigger.value) - )); Intake.whileTrue(IntakeRollers.getInstance().Intake()); ShootFull.whileTrue(IntakeRollers.getInstance().ShootFull()); - ShootHalf.whileTrue(IntakeRollers.getInstance().ShootHalf()); + ControlledShooting.whileTrue(IntakeRollers.getInstance().ControlledShooting( + () -> operatorController.getRawAxis(XboxController.Axis.kLeftTrigger.value))); Stop.whileTrue(IntakeRollers.getInstance().Stop()); IntakeTurning.getInstance().setDefaultCommand(IntakeTurning.getInstance().Manual( From da27ad377a492d549c0b80df11070030164d615c Mon Sep 17 00:00:00 2001 From: IAmRice33 Date: Sun, 18 Feb 2024 14:09:22 -0500 Subject: [PATCH 03/31] Added default command To-Do: Add Smartmotion for arm positions --- .../frc/team7520/robot/RobotContainer.java | 46 ++++++++++-------- .../team7520/robot/commands/IntakeStop.java | 48 +++++++++++++++++++ .../subsystems/Intake/IntakeRollers.java | 32 ++++++------- 3 files changed, 91 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/team7520/robot/commands/IntakeStop.java diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index ff63419..1b0274e 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.TeleopDrive; +import frc.team7520.robot.commands.IntakeStop; import frc.team7520.robot.subsystems.Intake.IntakeRollers; import frc.team7520.robot.subsystems.Intake.IntakeTurning; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; @@ -37,6 +38,8 @@ public class RobotContainer // Subsystems private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + private final IntakeRollers IntakeRollersSubsystem = IntakeRollers.getInstance(); + private final IntakeTurning IntakeTurningSubsystem = IntakeTurning.getInstance(); // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = @@ -44,13 +47,15 @@ public class RobotContainer private final XboxController operatorController = new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); - JoystickButton Shoot = new JoystickButton(operatorController, XboxController.Button.kY.value); - JoystickButton Floor = new JoystickButton(operatorController, XboxController.Button.kA.value); - - JoystickButton Stop = new JoystickButton(operatorController, XboxController.Button.kX.value); - JoystickButton Intake = new JoystickButton(operatorController, XboxController.Button.kB.value); - JoystickButton ControlledShooting = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); - JoystickButton ShootFull = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value); + JoystickButton Shoot = new JoystickButton(operatorController, XboxController.Button.kY.value); + JoystickButton Floor = new JoystickButton(operatorController, XboxController.Button.kA.value); + + JoystickButton Stop = new JoystickButton(operatorController, XboxController.Button.kX.value); + JoystickButton Intake = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); + JoystickButton ControlledShooting = new JoystickButton(operatorController, XboxController.Button.kB.value); + JoystickButton ShootHalf = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value); + + IntakeStop IntakeStop = new IntakeStop(IntakeRollersSubsystem); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -86,6 +91,7 @@ public RobotContainer() () -> driverController.getRawAxis(2), () -> true); drivebase.setDefaultCommand(closedAbsoluteDrive); + IntakeRollersSubsystem.setDefaultCommand(IntakeStop); } /** @@ -116,18 +122,20 @@ private void configureBindings() new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new InstantCommand(drivebase::lock)); - Intake.whileTrue(IntakeRollers.getInstance().Intake()); - ShootFull.whileTrue(IntakeRollers.getInstance().ShootFull()); - ControlledShooting.whileTrue(IntakeRollers.getInstance().ControlledShooting( - () -> operatorController.getRawAxis(XboxController.Axis.kLeftTrigger.value))); - Stop.whileTrue(IntakeRollers.getInstance().Stop()); - - IntakeTurning.getInstance().setDefaultCommand(IntakeTurning.getInstance().Manual( - () -> operatorController.getRawAxis(XboxController.Axis.kLeftY.value) - )); - - Shoot.whileTrue(IntakeTurning.getInstance().Shoot()); - Floor.whileTrue(IntakeTurning.getInstance().Intake()); + Intake.whileTrue(IntakeRollersSubsystem.Intake()); + ControlledShooting.whileTrue(IntakeRollersSubsystem.ControlledShooting( + () -> operatorController.getRawAxis(XboxController.Axis.kRightTrigger.value) + )); + + ShootHalf.whileTrue(IntakeRollersSubsystem.ShootHalf()); + Stop.whileTrue(IntakeRollersSubsystem.Stop()); + + IntakeTurningSubsystem.setDefaultCommand(IntakeTurningSubsystem.Manual( + () -> operatorController.getRawAxis(XboxController.Axis.kLeftY.value) + )); + + Shoot.whileTrue(IntakeTurningSubsystem.Shoot()); + Floor.whileTrue(IntakeTurningSubsystem.Intake()); } diff --git a/src/main/java/frc/team7520/robot/commands/IntakeStop.java b/src/main/java/frc/team7520/robot/commands/IntakeStop.java new file mode 100644 index 0000000..7b73626 --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/IntakeStop.java @@ -0,0 +1,48 @@ +// 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.team7520.robot.commands; + +import frc.team7520.robot.subsystems.Intake.IntakeRollers; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example command that uses an example subsystem. */ +public class IntakeStop extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final IntakeRollers IntakeRollersSubsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public IntakeStop(IntakeRollers IntakeRollersSubsystem) { + this.IntakeRollersSubsystem = IntakeRollersSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(IntakeRollersSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + IntakeRollersSubsystem.rollers.set(0); + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java index d8f0daf..10f04dd 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java @@ -9,7 +9,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants; @@ -27,36 +27,36 @@ public static IntakeRollers getInstance() { /** Creates a new ExampleSubsystem. */ public IntakeRollers() {} - public InstantCommand ShootFull() { - return new InstantCommand( + public Command Intake() { + return run( + () -> { + rollers.set(0.35); + }); + } + + public Command ShootFull() { + return run( () -> { rollers.set(-1); }); } - public InstantCommand ShootHalf() { - return new InstantCommand( + public Command ShootHalf() { + return run( () -> { rollers.set(-0.5); }); } - public InstantCommand Stop() { - return new InstantCommand( + public Command Stop() { + return run( () -> { rollers.set(0); }); } - public InstantCommand Intake() { - return new InstantCommand( - () -> { - rollers.set(0.35); - }); - } - - public InstantCommand ControlledShooting(DoubleSupplier BumpVal) { - return new InstantCommand( + public Command ControlledShooting(DoubleSupplier BumpVal) { + return run( () -> { rollers.set(-0.9 * BumpVal.getAsDouble()); }); From 552b26b06a913d1097b0dc73776bfca4c134b4ab Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Mon, 19 Feb 2024 20:40:59 -0500 Subject: [PATCH 04/31] Zero encoder --- src/main/deploy/swerve/neo/modules/backleft.json | 2 +- src/main/deploy/swerve/neo/modules/backright.json | 2 +- src/main/deploy/swerve/neo/modules/frontleft.json | 2 +- src/main/deploy/swerve/neo/modules/frontright.json | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index 2707b64..cd2e54a 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 271.142578125, + "absoluteEncoderOffset": 359.12109375, "location": { "front": -13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index 03efd3e..eaa3eda 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 316.845703125, + "absoluteEncoderOffset": 239.4140625, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index c895ee0..0113575 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 339.873046875, + "absoluteEncoderOffset": 328.0078125, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 2fee389..fde0aae 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": 33.837890625, + "absoluteEncoderOffset": 151.259765625, "location": { "front": 13, "left": -13 From c1a4f89710a823b4051d924088d92cdbe3230cb0 Mon Sep 17 00:00:00 2001 From: IAmRice33 Date: Tue, 20 Feb 2024 17:26:33 -0500 Subject: [PATCH 05/31] Added Angle setting for pivot + Default intake command --- .../java/frc/team7520/robot/Constants.java | 24 +++- .../frc/team7520/robot/RobotContainer.java | 39 +++--- .../commands/{IntakeStop.java => Intake.java} | 20 ++- .../robot/subsystems/Intake/IntakePivot.java | 115 ++++++++++++++++++ .../subsystems/Intake/IntakeRollers.java | 21 ++-- .../subsystems/Intake/IntakeTurning.java | 95 --------------- 6 files changed, 177 insertions(+), 137 deletions(-) rename src/main/java/frc/team7520/robot/commands/{IntakeStop.java => Intake.java} (68%) create mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java delete mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index a9af500..86eeb19 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -62,8 +62,30 @@ public static class Telemetry { } public static class IntakeConstants { - public static class turn { + public static class pivotConstants { public static final int CAN_ID = 23; + + public static final double GearRatio = 100; + public static final double GearAnglePerMotorRev = 360/GearRatio; + public static final double ConversionFactor = 1/GearRatio; + + public static final double Floor = 55.2 * GearAnglePerMotorRev; + public static final double Amp = 21.809415817260742 * GearAnglePerMotorRev; + public static final double Shoot = 0; + + public static final double kP = 0.00022; + public static final double kI = 0; + public static final double kD = 0; + public static final double kFF = 0.000156; + + public static final double OutputMax = 1; + public static final double OutputMin = -1; + + public static final double SmartMaxVel = 30000000; + public static final double SmartMinVel = 0; + public static final double SmartAccel = 20000; + public static final double SmartErr = 0.01; + public static final int SlotID = 0; } public static class rollers { public static final int CAN_ID = 22; diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 1b0274e..7f69bf0 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,9 +19,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.TeleopDrive; -import frc.team7520.robot.commands.IntakeStop; +import frc.team7520.robot.commands.Intake; import frc.team7520.robot.subsystems.Intake.IntakeRollers; -import frc.team7520.robot.subsystems.Intake.IntakeTurning; +import frc.team7520.robot.subsystems.Intake.IntakePivot; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; @@ -39,7 +39,7 @@ public class RobotContainer private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); private final IntakeRollers IntakeRollersSubsystem = IntakeRollers.getInstance(); - private final IntakeTurning IntakeTurningSubsystem = IntakeTurning.getInstance(); + private final IntakePivot IntakePivotSubsystem = IntakePivot.getInstance(); // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = @@ -48,14 +48,10 @@ public class RobotContainer new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); JoystickButton Shoot = new JoystickButton(operatorController, XboxController.Button.kY.value); + JoystickButton Amp = new JoystickButton(operatorController, XboxController.Button.kX.value); JoystickButton Floor = new JoystickButton(operatorController, XboxController.Button.kA.value); - - JoystickButton Stop = new JoystickButton(operatorController, XboxController.Button.kX.value); - JoystickButton Intake = new JoystickButton(operatorController, XboxController.Button.kLeftBumper.value); - JoystickButton ControlledShooting = new JoystickButton(operatorController, XboxController.Button.kB.value); - JoystickButton ShootHalf = new JoystickButton(operatorController, XboxController.Button.kRightBumper.value); - - IntakeStop IntakeStop = new IntakeStop(IntakeRollersSubsystem); + + Intake Intake = new Intake(IntakeRollersSubsystem, operatorController); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -91,7 +87,7 @@ public RobotContainer() () -> driverController.getRawAxis(2), () -> true); drivebase.setDefaultCommand(closedAbsoluteDrive); - IntakeRollersSubsystem.setDefaultCommand(IntakeStop); + IntakeRollersSubsystem.setDefaultCommand(Intake); } /** @@ -121,21 +117,14 @@ private void configureBindings() // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new InstantCommand(drivebase::lock)); - - Intake.whileTrue(IntakeRollersSubsystem.Intake()); - ControlledShooting.whileTrue(IntakeRollersSubsystem.ControlledShooting( - () -> operatorController.getRawAxis(XboxController.Axis.kRightTrigger.value) - )); - - ShootHalf.whileTrue(IntakeRollersSubsystem.ShootHalf()); - Stop.whileTrue(IntakeRollersSubsystem.Stop()); - - IntakeTurningSubsystem.setDefaultCommand(IntakeTurningSubsystem.Manual( + + IntakePivotSubsystem.setDefaultCommand(IntakePivotSubsystem.Manual( () -> operatorController.getRawAxis(XboxController.Axis.kLeftY.value) - )); - - Shoot.whileTrue(IntakeTurningSubsystem.Shoot()); - Floor.whileTrue(IntakeTurningSubsystem.Intake()); + )); + + Shoot.whileTrue(IntakePivotSubsystem.Shoot()); + Amp.whileTrue(IntakePivotSubsystem.Amp()); + Floor.whileTrue(IntakePivotSubsystem.Intake()); } diff --git a/src/main/java/frc/team7520/robot/commands/IntakeStop.java b/src/main/java/frc/team7520/robot/commands/Intake.java similarity index 68% rename from src/main/java/frc/team7520/robot/commands/IntakeStop.java rename to src/main/java/frc/team7520/robot/commands/Intake.java index 7b73626..1657e82 100644 --- a/src/main/java/frc/team7520/robot/commands/IntakeStop.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -5,20 +5,23 @@ package frc.team7520.robot.commands; import frc.team7520.robot.subsystems.Intake.IntakeRollers; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ -public class IntakeStop extends Command { +public class Intake extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeRollers IntakeRollersSubsystem; + private XboxController xbox; /** * Creates a new ExampleCommand. * * @param subsystem The subsystem used by this command. */ - public IntakeStop(IntakeRollers IntakeRollersSubsystem) { + public Intake(IntakeRollers IntakeRollersSubsystem, XboxController xbox) { this.IntakeRollersSubsystem = IntakeRollersSubsystem; + this.xbox = xbox; // Use addRequirements() here to declare subsystem dependencies. addRequirements(IntakeRollersSubsystem); } @@ -31,7 +34,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - IntakeRollersSubsystem.rollers.set(0); + if(xbox.getLeftBumper() == true) { + IntakeRollersSubsystem.Intake(); + } + else if (xbox.getRightBumper() == true) { + IntakeRollersSubsystem.ControlledShooting(() -> xbox.getRightTriggerAxis()); + } + else if (xbox.getBButton() == true) { + IntakeRollersSubsystem.Amp(); + } + else { + IntakeRollersSubsystem.Stop(); + } } diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java new file mode 100644 index 0000000..029e6dd --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java @@ -0,0 +1,115 @@ +// 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.team7520.robot.subsystems.Intake; + +import java.util.function.DoubleSupplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants.IntakeConstants.pivotConstants; + +public class IntakePivot extends SubsystemBase { + + public CANSparkMax pivot = new CANSparkMax(pivotConstants.CAN_ID, MotorType.kBrushless); + private RelativeEncoder pivotEncoder; + private final SparkPIDController pivotPID; + + public Rotation2d DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Shoot); + + private final static IntakePivot INSTANCE = new IntakePivot(); + + @SuppressWarnings("WeakerAccess") + public static IntakePivot getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + public IntakePivot() { + this.pivotEncoder = pivot.getEncoder(); + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(pivotConstants.ConversionFactor); + this.pivotPID = pivot.getPIDController(); + + pivotPID.setP(pivotConstants.kP); + pivotPID.setI(pivotConstants.kI); + pivotPID.setD(pivotConstants.kD); + pivotPID.setFF(pivotConstants.kFF); + pivotPID.setOutputRange(pivotConstants.OutputMin, pivotConstants.OutputMax); + + pivotPID.setSmartMotionMaxVelocity(pivotConstants.SmartMaxVel, pivotConstants.SlotID); + pivotPID.setSmartMotionMinOutputVelocity(pivotConstants.SmartMinVel, pivotConstants.SlotID); + pivotPID.setSmartMotionMaxAccel(pivotConstants.SmartAccel, pivotConstants.SlotID); + pivotPID.setSmartMotionAllowedClosedLoopError(pivotConstants.SmartErr, pivotConstants.SlotID); + + pivot.setIdleMode(CANSparkMax.IdleMode.kBrake); + } + + public void setRotation(Rotation2d position){ + DesiredPosition = position; + } + + public Command Shoot() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(pivotConstants.Shoot)); + }).andThen(MoveIntake()); + } + + public Command Amp() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(pivotConstants.Amp)); + }).andThen(MoveIntake()); + } + + public Command Intake() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(pivotConstants.Floor)); + }).andThen(MoveIntake()); + } + + public Command Manual(DoubleSupplier pivot) { + return runOnce( + () -> { + double pivotVal = pivot.getAsDouble(); + + if(Math.abs(pivotVal) > 0.05) { + DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); + } + }).andThen(MoveIntake()); + } + + public Command MoveIntake() { + return runOnce( + () -> { + if(DesiredPosition.getDegrees() > Rotation2d.fromDegrees(pivotConstants.Floor).getDegrees()) { + DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Floor); + } + + if(DesiredPosition.getDegrees() < Rotation2d.fromDegrees(pivotConstants.Shoot).getDegrees()) { + DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Shoot); + } + + pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); + } + ); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); + SmartDashboard.putNumber("DesiredDeg", DesiredPosition.getDegrees()); + SmartDashboard.putNumber("DesiredRot", DesiredPosition.getRotations()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java index 10f04dd..441c80b 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java @@ -25,26 +25,21 @@ public static IntakeRollers getInstance() { } /** Creates a new ExampleSubsystem. */ - public IntakeRollers() {} + public IntakeRollers() { + rollers.setInverted(true); + } - public Command Intake() { + public Command Intake() { return run( () -> { rollers.set(0.35); }); } - public Command ShootFull() { - return run( - () -> { - rollers.set(-1); - }); - } - - public Command ShootHalf() { + public Command Amp() { return run( () -> { - rollers.set(-0.5); + rollers.set(-0.525); }); } @@ -55,10 +50,10 @@ public Command Stop() { }); } - public Command ControlledShooting(DoubleSupplier BumpVal) { + public Command ControlledShooting(DoubleSupplier ShootVal) { return run( () -> { - rollers.set(-0.9 * BumpVal.getAsDouble()); + rollers.set(-0.9 * ShootVal.getAsDouble()); }); } } \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java deleted file mode 100644 index 9950735..0000000 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeTurning.java +++ /dev/null @@ -1,95 +0,0 @@ -// 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.team7520.robot.subsystems.Intake; - -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team7520.robot.Constants; - -public class IntakeTurning extends SubsystemBase { - - public CANSparkMax turn = new CANSparkMax(Constants.IntakeConstants.turn.CAN_ID, MotorType.kBrushless); - private RelativeEncoder turnEncoder; - private final SparkPIDController turnPID; - - public double currentPosition = Position.SHOOT; - - class Position { - public static double FLOOR = 51.78583526611328; //not exact - public static double SHOOT = 0.238095209002495; //not exact - } - - private final static IntakeTurning INSTANCE = new IntakeTurning(); - - @SuppressWarnings("WeakerAccess") - public static IntakeTurning getInstance() { - return INSTANCE; - } - - /** Creates a new ExampleSubsystem. */ - public IntakeTurning() { - this.turnEncoder = turn.getEncoder(); - this.turnPID = turn.getPIDController(); - - turnPID.setP(0.15); - turnPID.setI(0); - turnPID.setD(0); - turnPID.setFF(0.1); - turnPID.setOutputRange(-1, 1); - - turn.setIdleMode(CANSparkMax.IdleMode.kBrake); - } - - public void setPosition(double position){ - currentPosition = position; - } - - public Command Shoot() { - return runOnce( - () -> { - setPosition(Position.SHOOT); - }).andThen(MoveIntake()); - } - - public Command Intake() { - return runOnce( - () -> { - setPosition(Position.FLOOR); - }).andThen(MoveIntake()); - } - - public Command Manual(DoubleSupplier turn) { - return runOnce( - () -> { - double turnVal = turn.getAsDouble(); - - if(Math.abs(turnVal) > 0.05) { - currentPosition = currentPosition + 1 * turnVal; - } - }).andThen(MoveIntake()); - } - - public Command MoveIntake() { - return runOnce( - () -> { - turnPID.setReference(currentPosition, ControlType.kPosition); - } - ); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("TurnEncoder", turnEncoder.getPosition()); - } -} \ No newline at end of file From 08f6d3f5bed42c77a1ce92a30a6069d79719f018 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Fri, 23 Feb 2024 19:36:23 -0500 Subject: [PATCH 06/31] Intake and LEDS --- .../java/frc/team7520/robot/Constants.java | 59 ++++---- .../frc/team7520/robot/RobotContainer.java | 23 +-- .../frc/team7520/robot/commands/Intake.java | 94 ++++++------ .../robot/subsystems/Intake/IntakePivot.java | 115 -------------- .../subsystems/Intake/IntakeRollers.java | 4 +- .../subsystems/Intake/IntakeSubsystem.java | 143 ++++++++++++++++++ .../frc/team7520/robot/subsystems/LED.java | 48 ++++++ 7 files changed, 273 insertions(+), 213 deletions(-) delete mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java create mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java create mode 100644 src/main/java/frc/team7520/robot/subsystems/LED.java diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 86eeb19..ceb5529 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -62,33 +62,38 @@ public static class Telemetry { } public static class IntakeConstants { - public static class pivotConstants { - public static final int CAN_ID = 23; - - public static final double GearRatio = 100; - public static final double GearAnglePerMotorRev = 360/GearRatio; - public static final double ConversionFactor = 1/GearRatio; - - public static final double Floor = 55.2 * GearAnglePerMotorRev; - public static final double Amp = 21.809415817260742 * GearAnglePerMotorRev; - public static final double Shoot = 0; - - public static final double kP = 0.00022; - public static final double kI = 0; - public static final double kD = 0; - public static final double kFF = 0.000156; - - public static final double OutputMax = 1; - public static final double OutputMin = -1; - - public static final double SmartMaxVel = 30000000; - public static final double SmartMinVel = 0; - public static final double SmartAccel = 20000; - public static final double SmartErr = 0.01; - public static final int SlotID = 0; + public static class PivotConstants { + public static final int CAN_ID = 23; + + public static final double GearRatio = 100; + public static final double GearAnglePerMotorRev = 360/GearRatio; + public static final double ConversionFactor = 1/GearRatio; + + public static final double Floor = 55.2 * GearAnglePerMotorRev; + public static final double Amp = 21.809415817260742 * GearAnglePerMotorRev; + public static final double Shoot = 0; + + public static final double kP = 0.00022; + public static final double kI = 0; + public static final double kD = 0; + public static final double kFF = 0.000156; + + public static final double OutputMax = 1; + public static final double OutputMin = -1; + + public static final double SmartMaxVel = 30000000; + public static final double SmartMinVel = 0; + public static final double SmartAccel = 20000; + public static final double SmartErr = 0.01; + public static final int SlotID = 0; } - public static class rollers { - public static final int CAN_ID = 22; + public static class WheelConstants { + public static final int CAN_ID = 22; + + public static final double kP = 0.0020645; + public static final double kI = 0; + public static final double kD = 0; + public static final double kFF = 0; } - } + } } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index f5008ac..5d48b55 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -20,9 +20,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.TeleopDrive; -import frc.team7520.robot.commands.Intake; -import frc.team7520.robot.subsystems.Intake.IntakeRollers; -import frc.team7520.robot.subsystems.Intake.IntakePivot; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; @@ -39,20 +36,10 @@ public class RobotContainer // Subsystems private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); - private final IntakeRollers IntakeRollersSubsystem = IntakeRollers.getInstance(); - private final IntakePivot IntakePivotSubsystem = IntakePivot.getInstance(); // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); - private final XboxController operatorController = - new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); - - JoystickButton Shoot = new JoystickButton(operatorController, XboxController.Button.kY.value); - JoystickButton Amp = new JoystickButton(operatorController, XboxController.Button.kX.value); - JoystickButton Floor = new JoystickButton(operatorController, XboxController.Button.kA.value); - - Intake Intake = new Intake(IntakeRollersSubsystem, operatorController); /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -88,7 +75,6 @@ public RobotContainer() () -> driverController.getRawAxis(2), () -> true); drivebase.setDefaultCommand(closedAbsoluteDrive); - IntakeRollersSubsystem.setDefaultCommand(Intake); } /** @@ -117,14 +103,7 @@ private void configureBindings() .onTrue(new InstantCommand(drivebase::zeroGyro)); // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) - .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); - IntakePivotSubsystem.setDefaultCommand(IntakePivotSubsystem.Manual( - () -> operatorController.getRawAxis(XboxController.Axis.kLeftY.value) - )); - - Shoot.whileTrue(IntakePivotSubsystem.Shoot()); - Amp.whileTrue(IntakePivotSubsystem.Amp()); - Floor.whileTrue(IntakePivotSubsystem.Intake()); + .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); } diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index 1657e82..9ef931b 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -4,59 +4,59 @@ package frc.team7520.robot.commands; -import frc.team7520.robot.subsystems.Intake.IntakeRollers; +import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; /** An example command that uses an example subsystem. */ public class Intake extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final IntakeRollers IntakeRollersSubsystem; - private XboxController xbox; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public Intake(IntakeRollers IntakeRollersSubsystem, XboxController xbox) { - this.IntakeRollersSubsystem = IntakeRollersSubsystem; - this.xbox = xbox; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(IntakeRollersSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if(xbox.getLeftBumper() == true) { - IntakeRollersSubsystem.Intake(); + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final IntakeSubsystem intakeSubsystem; + + private final boolean intake; + private final boolean amp; + + /** + * Creates a new ExampleCommand. + * + * @param intakeSubsystem The subsystem used by this command. + */ + public Intake(IntakeSubsystem intakeSubsystem, boolean intake, boolean amp) { + this.intakeSubsystem = intakeSubsystem; + this.intake = intake; + this.amp = amp; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intakeSubsystem); } - else if (xbox.getRightBumper() == true) { - IntakeRollersSubsystem.ControlledShooting(() -> xbox.getRightTriggerAxis()); + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (intake) { + intakeSubsystem.setSpeed(0.35, false); + return; + } + if (amp) { + intakeSubsystem.setSpeed(-0.525, false); + return; + } + intakeSubsystem.setSpeed(0); } - else if (xbox.getBButton() == true) { - IntakeRollersSubsystem.Amp(); + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { } - else { - IntakeRollersSubsystem.Stop(); + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; } - } - - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file +} diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java deleted file mode 100644 index 029e6dd..0000000 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakePivot.java +++ /dev/null @@ -1,115 +0,0 @@ -// 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.team7520.robot.subsystems.Intake; - -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team7520.robot.Constants.IntakeConstants.pivotConstants; - -public class IntakePivot extends SubsystemBase { - - public CANSparkMax pivot = new CANSparkMax(pivotConstants.CAN_ID, MotorType.kBrushless); - private RelativeEncoder pivotEncoder; - private final SparkPIDController pivotPID; - - public Rotation2d DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Shoot); - - private final static IntakePivot INSTANCE = new IntakePivot(); - - @SuppressWarnings("WeakerAccess") - public static IntakePivot getInstance() { - return INSTANCE; - } - - /** Creates a new ExampleSubsystem. */ - public IntakePivot() { - this.pivotEncoder = pivot.getEncoder(); - pivotEncoder.setPosition(0); - pivotEncoder.setPositionConversionFactor(pivotConstants.ConversionFactor); - this.pivotPID = pivot.getPIDController(); - - pivotPID.setP(pivotConstants.kP); - pivotPID.setI(pivotConstants.kI); - pivotPID.setD(pivotConstants.kD); - pivotPID.setFF(pivotConstants.kFF); - pivotPID.setOutputRange(pivotConstants.OutputMin, pivotConstants.OutputMax); - - pivotPID.setSmartMotionMaxVelocity(pivotConstants.SmartMaxVel, pivotConstants.SlotID); - pivotPID.setSmartMotionMinOutputVelocity(pivotConstants.SmartMinVel, pivotConstants.SlotID); - pivotPID.setSmartMotionMaxAccel(pivotConstants.SmartAccel, pivotConstants.SlotID); - pivotPID.setSmartMotionAllowedClosedLoopError(pivotConstants.SmartErr, pivotConstants.SlotID); - - pivot.setIdleMode(CANSparkMax.IdleMode.kBrake); - } - - public void setRotation(Rotation2d position){ - DesiredPosition = position; - } - - public Command Shoot() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(pivotConstants.Shoot)); - }).andThen(MoveIntake()); - } - - public Command Amp() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(pivotConstants.Amp)); - }).andThen(MoveIntake()); - } - - public Command Intake() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(pivotConstants.Floor)); - }).andThen(MoveIntake()); - } - - public Command Manual(DoubleSupplier pivot) { - return runOnce( - () -> { - double pivotVal = pivot.getAsDouble(); - - if(Math.abs(pivotVal) > 0.05) { - DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); - } - }).andThen(MoveIntake()); - } - - public Command MoveIntake() { - return runOnce( - () -> { - if(DesiredPosition.getDegrees() > Rotation2d.fromDegrees(pivotConstants.Floor).getDegrees()) { - DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Floor); - } - - if(DesiredPosition.getDegrees() < Rotation2d.fromDegrees(pivotConstants.Shoot).getDegrees()) { - DesiredPosition = Rotation2d.fromDegrees(pivotConstants.Shoot); - } - - pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); - } - ); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); - SmartDashboard.putNumber("DesiredDeg", DesiredPosition.getDegrees()); - SmartDashboard.putNumber("DesiredRot", DesiredPosition.getRotations()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java index 441c80b..3278e74 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java @@ -15,7 +15,7 @@ public class IntakeRollers extends SubsystemBase { - public CANSparkMax rollers = new CANSparkMax(Constants.IntakeConstants.rollers.CAN_ID, MotorType.kBrushless); + public CANSparkMax rollers = new CANSparkMax(Constants.IntakeConstants.PivotConstants.CAN_ID, MotorType.kBrushless); private final static IntakeRollers INSTANCE = new IntakeRollers(); @@ -56,4 +56,4 @@ public Command ControlledShooting(DoubleSupplier ShootVal) { rollers.set(-0.9 * ShootVal.getAsDouble()); }); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java new file mode 100644 index 0000000..69c586c --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java @@ -0,0 +1,143 @@ +// 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.team7520.robot.subsystems.Intake; + +import java.util.function.DoubleSupplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants.IntakeConstants; + +public class IntakeSubsystem extends SubsystemBase { + + public CANSparkMax pivot = new CANSparkMax(IntakeConstants.PivotConstants.CAN_ID, MotorType.kBrushless); + public CANSparkMax wheels = new CANSparkMax(IntakeConstants.WheelConstants.CAN_ID, MotorType.kBrushless); + + private RelativeEncoder pivotEncoder; + private final SparkPIDController pivotPID = pivot.getPIDController(); + private final SparkPIDController wheelsPID = wheels.getPIDController(); + + public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); + + private final static IntakeSubsystem INSTANCE = new IntakeSubsystem(); + + @SuppressWarnings("WeakerAccess") + public static IntakeSubsystem getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + public IntakeSubsystem() { + this.pivotEncoder = pivot.getEncoder(); + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(IntakeConstants.PivotConstants.ConversionFactor); + + pivotPID.setP(IntakeConstants.PivotConstants.kP); + pivotPID.setI(IntakeConstants.PivotConstants.kI); + pivotPID.setD(IntakeConstants.PivotConstants.kD); + pivotPID.setFF(IntakeConstants.PivotConstants.kFF); + + pivotPID.setSmartMotionMaxVelocity(IntakeConstants.PivotConstants.SmartMaxVel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionMinOutputVelocity(IntakeConstants.PivotConstants.SmartMinVel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionMaxAccel(IntakeConstants.PivotConstants.SmartAccel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionAllowedClosedLoopError(IntakeConstants.PivotConstants.SmartErr, IntakeConstants.PivotConstants.SlotID); + + + pivot.setIdleMode(CANSparkMax.IdleMode.kBrake); + + // Wheels + wheels.setIdleMode(CANSparkMax.IdleMode.kBrake); + wheels.setInverted(true); + + wheelsPID.setP(IntakeConstants.WheelConstants.kP); + wheelsPID.setI(IntakeConstants.WheelConstants.kI); + wheelsPID.setD(IntakeConstants.WheelConstants.kD); + wheelsPID.setFF(IntakeConstants.WheelConstants.kFF); + } + + public void setRotation(Rotation2d position){ + DesiredPosition = position; + } + + public Command Shoot() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot)); + }).andThen(MoveIntake()); + } + + public Command Amp() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Amp)); + }).andThen(MoveIntake()); + } + + public Command Intake() { + return runOnce( + () -> { + setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Floor)); + }).andThen(MoveIntake()); + } + + public Command Manual(DoubleSupplier pivot) { + return runOnce( + () -> { + double pivotVal = pivot.getAsDouble(); + + if(Math.abs(pivotVal) > 0.05) { + DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); + } + }).andThen(MoveIntake()); + } + + public Command MoveIntake() { + return runOnce( + () -> { + pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); + } + ); + } + + public Command stop() { + return run( + () -> { + wheels.set(0); + }); + } + + public void setSpeed(double speed) { + setSpeed(speed, false); + } + + public void setSpeed(double speed, boolean closedLoop) { + if(closedLoop) { + if (speed == 0) { + wheels.set(0); + } else { + + + wheelsPID.setReference(speed, ControlType.kVelocity); + } + } else { + wheels.set(speed); + } + } + + @Override + public void periodic() { + SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); + SmartDashboard.putNumber("DesiredDeg", DesiredPosition.getDegrees()); + SmartDashboard.putNumber("DesiredRot", DesiredPosition.getRotations()); + } +} diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java new file mode 100644 index 0000000..2ff1cd9 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -0,0 +1,48 @@ +package frc.team7520.robot.subsystems.swerve; + + +import com.ctre.phoenix.led.*; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LED extends SubsystemBase { + + // With eager singleton initialization, any static variables/fields used in the + // constructor must appear before the "INSTANCE" variable so that they are initialized + // before the constructor is called when the "INSTANCE" variable initializes. + + /** + * The Singleton instance of this LED. Code should use + * the {@link #getInstance()} method to get the single instance (rather + * than trying to construct an instance of this class.) + */ + private final static LED INSTANCE = new LED(); + + public final static CANdle candle = new CANdle(17); + + + /** + * Returns the Singleton instance of this LED. This static method + * should be used, rather than the constructor, to get the single instance + * of this class. For example: {@code LED.getInstance();} + */ + @SuppressWarnings("WeakerAccess") + public static LED getInstance() { + return INSTANCE; + } + + /** + * Creates a new instance of this LED. This constructor + * is private since this class is a Singleton. Code should use + * the {@link #getInstance()} method to get the singleton instance. + */ + private LED() { + + } + + public void test(){ +// Animation animation = new RainbowAnimation(255, 0.75, 100); + Animation animation = new ColorFlowAnimation(255, 0, 0, 0, 0.75, 100, ColorFlowAnimation.Direction.Forward); + candle.animate(animation); + } +} + From abc5b650ddac6c1a568b79000acf22f6c294dbf9 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Fri, 23 Feb 2024 20:12:30 -0500 Subject: [PATCH 07/31] More intake stuff --- .../frc/team7520/robot/commands/Intake.java | 14 +++-- .../subsystems/Intake/IntakeRollers.java | 59 ------------------- .../subsystems/Intake/IntakeSubsystem.java | 18 +++--- 3 files changed, 19 insertions(+), 72 deletions(-) delete mode 100644 src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index 9ef931b..dadd826 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -8,20 +8,22 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; + /** An example command that uses an example subsystem. */ public class Intake extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; - private final boolean intake; - private final boolean amp; + private final BooleanSupplier intake; + private final BooleanSupplier amp; /** * Creates a new ExampleCommand. * * @param intakeSubsystem The subsystem used by this command. */ - public Intake(IntakeSubsystem intakeSubsystem, boolean intake, boolean amp) { + public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier intake, BooleanSupplier amp) { this.intakeSubsystem = intakeSubsystem; this.intake = intake; this.amp = amp; @@ -37,15 +39,15 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (intake) { + if (intake.getAsBoolean()) { intakeSubsystem.setSpeed(0.35, false); return; } - if (amp) { + if (amp.getAsBoolean()) { intakeSubsystem.setSpeed(-0.525, false); return; } - intakeSubsystem.setSpeed(0); + intakeSubsystem.stop(); } diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java deleted file mode 100644 index 3278e74..0000000 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeRollers.java +++ /dev/null @@ -1,59 +0,0 @@ -// 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.team7520.robot.subsystems.Intake; - -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team7520.robot.Constants; - -public class IntakeRollers extends SubsystemBase { - - public CANSparkMax rollers = new CANSparkMax(Constants.IntakeConstants.PivotConstants.CAN_ID, MotorType.kBrushless); - - private final static IntakeRollers INSTANCE = new IntakeRollers(); - - @SuppressWarnings("WeakerAccess") - public static IntakeRollers getInstance() { - return INSTANCE; - } - - /** Creates a new ExampleSubsystem. */ - public IntakeRollers() { - rollers.setInverted(true); - } - - public Command Intake() { - return run( - () -> { - rollers.set(0.35); - }); - } - - public Command Amp() { - return run( - () -> { - rollers.set(-0.525); - }); - } - - public Command Stop() { - return run( - () -> { - rollers.set(0); - }); - } - - public Command ControlledShooting(DoubleSupplier ShootVal) { - return run( - () -> { - rollers.set(-0.9 * ShootVal.getAsDouble()); - }); - } -} diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java index 69c586c..54b8e3f 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java @@ -12,6 +12,7 @@ import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -27,6 +28,8 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkPIDController pivotPID = pivot.getPIDController(); private final SparkPIDController wheelsPID = wheels.getPIDController(); + private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.1); + public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); private final static IntakeSubsystem INSTANCE = new IntakeSubsystem(); @@ -109,11 +112,10 @@ public Command MoveIntake() { ); } - public Command stop() { - return run( - () -> { - wheels.set(0); - }); + public void stop() { + + wheels.set(0); + } public void setSpeed(double speed) { @@ -121,12 +123,14 @@ public void setSpeed(double speed) { } public void setSpeed(double speed, boolean closedLoop) { + speed = slewRateLimiter.calculate(speed); + if(closedLoop) { + speed *= IntakeConstants.WheelConstants.MAX_RPM; + if (speed == 0) { wheels.set(0); } else { - - wheelsPID.setReference(speed, ControlType.kVelocity); } } else { From db0d5d29851c5bd657c6c987c963956a61a7471a Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Fri, 23 Feb 2024 20:13:10 -0500 Subject: [PATCH 08/31] Shooter --- .../java/frc/team7520/robot/Constants.java | 14 ++ .../frc/team7520/robot/commands/Shooter.java | 48 +++++++ .../subsystems/shooter/ShooterSubsystem.java | 125 ++++++++++++++++++ 3 files changed, 187 insertions(+) create mode 100644 src/main/java/frc/team7520/robot/commands/Shooter.java create mode 100644 src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index ceb5529..0b8eb84 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -94,6 +94,20 @@ public static class WheelConstants { public static final double kI = 0; public static final double kD = 0; public static final double kFF = 0; + + public static final double MAX_RPM = 5676; } } + + public static class ShooterConstants { + public static final int ShooterLeftID = 20; + public static final int ShooterRightID = 21; + + public static final double kP = 0.002; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kFF = 0.000156; + + public static final double MAX_RPM = 5676; + } } diff --git a/src/main/java/frc/team7520/robot/commands/Shooter.java b/src/main/java/frc/team7520/robot/commands/Shooter.java new file mode 100644 index 0000000..c0ec4da --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/Shooter.java @@ -0,0 +1,48 @@ +package frc.team7520.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + + +public class Shooter extends Command { + private final ShooterSubsystem shooterSubsystem; + private final DoubleSupplier throttleSup; + private final BooleanSupplier invertSup; + + public Shooter(ShooterSubsystem shooterSubsystem, DoubleSupplier throttleSup, BooleanSupplier invertSup) { + this.shooterSubsystem = shooterSubsystem; + this.throttleSup = throttleSup; + this.invertSup = invertSup; + + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.shooterSubsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + double throttle = throttleSup.getAsDouble() * (invertSup.getAsBoolean() ? -1 : 1); + + shooterSubsystem.setSpeed(throttle, false); + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 0000000..fa31485 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,125 @@ +package frc.team7520.robot.subsystems.shooter; + + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; + +public class ShooterSubsystem extends SubsystemBase { + + // With eager singleton initialization, any static variables/fields used in the + // constructor must appear before the "INSTANCE" variable so that they are initialized + // before the constructor is called when the "INSTANCE" variable initializes. + + private CANSparkMax leftShooterMotor; + private CANSparkMax rightShooterMotor; + + private SparkPIDController leftShooterPID; + private SparkPIDController rightShooterPID; + + private RelativeEncoder leftShooterEncoder; + private RelativeEncoder rightShooterEncoder; + + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1); + + /** + * The Singleton instance of this shooterSubsystem. Code should use + * the {@link #getInstance()} method to get the single instance (rather + * than trying to construct an instance of this class.) + */ + private final static ShooterSubsystem INSTANCE = new ShooterSubsystem(); + + /** + * Returns the Singleton instance of this shooterSubsystem. This static method + * should be used, rather than the constructor, to get the single instance + * of this class. For example: {@code shooterSubsystem.getInstance();} + */ + @SuppressWarnings("WeakerAccess") + public static ShooterSubsystem getInstance() { + return INSTANCE; + } + + /** + * Creates a new instance of this shooterSubsystem. This constructor + * is private since this class is a Singleton. Code should use + * the {@link #getInstance()} method to get the singleton instance. + */ + private ShooterSubsystem() { + /* leftShooterMotor = new CANSparkMax(Constants.kShooterLeftMotorId, MotorType.kBrushless); + rightShooterMotor = new CANSparkMax(Constants.kShooterRightMotorId, MotorType.kBrushless); + leftShooterMotor.restoreFactoryDefaults(); + rightShooterMotor.restoreFactoryDefaults(); + + leftShooterPID = leftShooterMotor.getPIDController(); + leftShooterPID.setP(Constants.kShooterP); + leftShooterPID.setI(Constants.kShooterI); + leftShooterPID.setD(Constants.kShooterD); + leftShooterPID.setOutputRange(Constants.kShooterMinOutput, Constants.kShooterMaxOutput); + + rightShooterPID = rightShooterMotor.getPIDController(); + rightShooterPID.setP(Constants.kShooterP); + rightShooterPID.setI(Constants.kShooterI); + rightShooterPID.setD(Constants.kShooterD); + rightShooterPID.setOutputRange(Constants.kShooterMinOutput, Constants.kShooterMaxOutput); + + leftShooterEncoder = leftShooterMotor.getEncoder(); + rightShooterEncoder = rightShooterMotor.getEncoder(); + + leftShooterMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + rightShooterMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + + leftShooterMotor.setInverted(false); + rightShooterMotor.setInverted(true);*/ + + leftShooterMotor = new CANSparkMax(Constants.ShooterConstants.ShooterLeftID, CANSparkMax.MotorType.kBrushless); + rightShooterMotor = new CANSparkMax(Constants.ShooterConstants.ShooterRightID, CANSparkMax.MotorType.kBrushless); + + leftShooterMotor.restoreFactoryDefaults(); + rightShooterMotor.restoreFactoryDefaults(); + + leftShooterPID = leftShooterMotor.getPIDController(); + leftShooterPID.setP(Constants.ShooterConstants.kP); + leftShooterPID.setI(Constants.ShooterConstants.kI); + leftShooterPID.setD(Constants.ShooterConstants.kD); + leftShooterPID.setFF(Constants.ShooterConstants.kFF); + + rightShooterPID = rightShooterMotor.getPIDController(); + rightShooterPID.setP(Constants.ShooterConstants.kP); + rightShooterPID.setI(Constants.ShooterConstants.kI); + rightShooterPID.setD(Constants.ShooterConstants.kD); + rightShooterPID.setFF(Constants.ShooterConstants.kFF); + + leftShooterEncoder = leftShooterMotor.getEncoder(); + rightShooterEncoder = rightShooterMotor.getEncoder(); + + leftShooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast); + rightShooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast); + + leftShooterMotor.setInverted(false); + rightShooterMotor.setInverted(true); + } + + public void setSpeed(double speed, boolean closedLoop) { + speed = mSpeedLimiter.calculate(speed); + + if (closedLoop) { + speed *= Constants.ShooterConstants.MAX_RPM; + + leftShooterPID.setReference(speed, CANSparkBase.ControlType.kVelocity); + rightShooterPID.setReference(speed, CANSparkBase.ControlType.kVelocity); + } else { + leftShooterMotor.set(speed); + rightShooterMotor.set(speed); + } + } + + public void stop() { + leftShooterMotor.stopMotor(); + rightShooterMotor.stopMotor(); + } +} + From 79850b8c1f4732d462a447ce8e453029d227647c Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sat, 24 Feb 2024 13:09:56 -0500 Subject: [PATCH 09/31] Working --- .../swerve/neo/modules/pidfproperties.json | 4 +- src/main/deploy/swerve/neo/swervedrive.json | 2 +- .../java/frc/team7520/robot/Constants.java | 4 +- .../frc/team7520/robot/RobotContainer.java | 25 ++++++ .../frc/team7520/robot/commands/Intake.java | 74 +++++++++++---- .../subsystems/Intake/IntakeSubsystem.java | 89 +++++++++---------- 6 files changed, 131 insertions(+), 67 deletions(-) diff --git a/src/main/deploy/swerve/neo/modules/pidfproperties.json b/src/main/deploy/swerve/neo/modules/pidfproperties.json index 09a5bc5..a3456b5 100644 --- a/src/main/deploy/swerve/neo/modules/pidfproperties.json +++ b/src/main/deploy/swerve/neo/modules/pidfproperties.json @@ -7,9 +7,9 @@ "iz": 0 }, "angle": { - "p": 0.0225, + "p": 0.01, "i": 0, - "d": 0.05, + "d": 0.0, "f": 0, "iz": 0 } diff --git a/src/main/deploy/swerve/neo/swervedrive.json b/src/main/deploy/swerve/neo/swervedrive.json index 5fb01c6..3f9d2c2 100644 --- a/src/main/deploy/swerve/neo/swervedrive.json +++ b/src/main/deploy/swerve/neo/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": false, + "invertedIMU": true, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 0b8eb84..044c19c 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -69,7 +69,7 @@ public static class PivotConstants { public static final double GearAnglePerMotorRev = 360/GearRatio; public static final double ConversionFactor = 1/GearRatio; - public static final double Floor = 55.2 * GearAnglePerMotorRev; + public static final double Intake = 55.2 * GearAnglePerMotorRev; public static final double Amp = 21.809415817260742 * GearAnglePerMotorRev; public static final double Shoot = 0; @@ -83,7 +83,7 @@ public static class PivotConstants { public static final double SmartMaxVel = 30000000; public static final double SmartMinVel = 0; - public static final double SmartAccel = 20000; + public static final double SmartAccel = 40000; public static final double SmartErr = 0.01; public static final int SlotID = 0; } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 5d48b55..4f3a9f7 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,7 +19,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; +import frc.team7520.robot.commands.Intake; +import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; +import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; @@ -37,10 +41,17 @@ public class RobotContainer private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + + private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); + private final XboxController operatorController = + new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() @@ -63,6 +74,18 @@ public RobotContainer() driverController::getLeftBumper ); + Shooter shooter = new Shooter(shooterSubsystem, + operatorController::getLeftTriggerAxis, + operatorController::getLeftBumper + ); + + Intake intake = new Intake(intakeSubsystem, + operatorController::getRightBumper, + operatorController::getYButton, + operatorController::getAButton, + operatorController::getBButton + ); + // Old drive method // like in video games // Easier to learn, harder to control @@ -75,6 +98,8 @@ public RobotContainer() () -> driverController.getRawAxis(2), () -> true); drivebase.setDefaultCommand(closedAbsoluteDrive); + shooterSubsystem.setDefaultCommand(shooter); + intakeSubsystem.setDefaultCommand(intake); } /** diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index dadd826..afcbef5 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -4,8 +4,9 @@ package frc.team7520.robot.commands; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.team7520.robot.Constants; import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.BooleanSupplier; @@ -14,23 +15,71 @@ public class Intake extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; + private final BooleanSupplier shootSup; + private final BooleanSupplier shootPosSup; + private final BooleanSupplier intakePosSup; + private final BooleanSupplier ampPosSup; + + public enum Position { + SHOOT, + INTAKE, + AMP + } + + public Position currPosition = Position.SHOOT; + - private final BooleanSupplier intake; - private final BooleanSupplier amp; /** * Creates a new ExampleCommand. * * @param intakeSubsystem The subsystem used by this command. */ - public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier intake, BooleanSupplier amp) { + public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, BooleanSupplier shootPosSup, BooleanSupplier intakePosSup, BooleanSupplier ampPosSup) { this.intakeSubsystem = intakeSubsystem; - this.intake = intake; - this.amp = amp; + this.shootSup = shootSup; + this.shootPosSup = shootPosSup; + this.intakePosSup = intakePosSup; + this.ampPosSup = ampPosSup; + // Use addRequirements() here to declare subsystem dependencies. addRequirements(intakeSubsystem); } + public void handleWheels() { + if (shootSup.getAsBoolean() && currPosition == Position.SHOOT) { + intakeSubsystem.setSpeed(0.35, false); + return; + } + if (shootSup.getAsBoolean() && currPosition == Position.AMP) { + intakeSubsystem.setSpeed(-0.525, false); + return; + } + if (shootSup.getAsBoolean() && currPosition == Position.INTAKE) { + intakeSubsystem.setSpeed(-0.35, false); + return; + } + intakeSubsystem.stop(); + } + + public void handlePosition() { + if (shootPosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Shoot)); + currPosition = Position.SHOOT; + return; + } + if (intakePosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Intake)); + currPosition = Position.INTAKE; + return; + } + if (ampPosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Amp)); + currPosition = Position.AMP; + return; + } + } + // Called when the command is initially scheduled. @Override public void initialize() { @@ -39,15 +88,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (intake.getAsBoolean()) { - intakeSubsystem.setSpeed(0.35, false); - return; - } - if (amp.getAsBoolean()) { - intakeSubsystem.setSpeed(-0.525, false); - return; - } - intakeSubsystem.stop(); + + handleWheels(); + handlePosition(); + } diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java index 54b8e3f..80203e4 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java @@ -4,8 +4,7 @@ package frc.team7520.robot.subsystems.Intake; -import java.util.function.DoubleSupplier; - +import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; @@ -15,7 +14,6 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants.IntakeConstants; @@ -28,7 +26,7 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkPIDController pivotPID = pivot.getPIDController(); private final SparkPIDController wheelsPID = wheels.getPIDController(); - private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.1); + private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.5); public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); @@ -40,7 +38,7 @@ public static IntakeSubsystem getInstance() { } /** Creates a new ExampleSubsystem. */ - public IntakeSubsystem() { + private IntakeSubsystem() { this.pivotEncoder = pivot.getEncoder(); pivotEncoder.setPosition(0); pivotEncoder.setPositionConversionFactor(IntakeConstants.PivotConstants.ConversionFactor); @@ -56,11 +54,11 @@ public IntakeSubsystem() { pivotPID.setSmartMotionAllowedClosedLoopError(IntakeConstants.PivotConstants.SmartErr, IntakeConstants.PivotConstants.SlotID); - pivot.setIdleMode(CANSparkMax.IdleMode.kBrake); + pivot.setIdleMode(CANSparkMax.IdleMode.kCoast); // Wheels wheels.setIdleMode(CANSparkMax.IdleMode.kBrake); - wheels.setInverted(true); +// wheels.setInverted(true); wheelsPID.setP(IntakeConstants.WheelConstants.kP); wheelsPID.setI(IntakeConstants.WheelConstants.kI); @@ -68,49 +66,46 @@ public IntakeSubsystem() { wheelsPID.setFF(IntakeConstants.WheelConstants.kFF); } - public void setRotation(Rotation2d position){ + public void setPosition(Rotation2d position){ DesiredPosition = position; + pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); } - public Command Shoot() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot)); - }).andThen(MoveIntake()); - } - - public Command Amp() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Amp)); - }).andThen(MoveIntake()); - } - - public Command Intake() { - return runOnce( - () -> { - setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Floor)); - }).andThen(MoveIntake()); - } - - public Command Manual(DoubleSupplier pivot) { - return runOnce( - () -> { - double pivotVal = pivot.getAsDouble(); - - if(Math.abs(pivotVal) > 0.05) { - DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); - } - }).andThen(MoveIntake()); - } - - public Command MoveIntake() { - return runOnce( - () -> { - pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); - } - ); - } +// public Command Shoot() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot)); +// }).andThen(MoveIntake()); +// } +// +// public Command Amp() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Amp)); +// }).andThen(MoveIntake()); +// } +// +// public Command Intake() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Floor)); +// }).andThen(MoveIntake()); +// } +// +// public Command Manual(DoubleSupplier pivot) { +// return runOnce( +// () -> { +// double pivotVal = pivot.getAsDouble(); +// +// if(Math.abs(pivotVal) > 0.05) { +// DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); +// } +// }).andThen(MoveIntake()); +// } + +// public void moveIntake() { +// pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); +// } public void stop() { From f924d8684fd9638c3790aadaefcb365167f1976f Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Fri, 1 Mar 2024 23:13:03 -0500 Subject: [PATCH 10/31] Working --- src/main/java/frc/team7520/robot/RobotContainer.java | 6 +++--- src/main/java/frc/team7520/robot/commands/Intake.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 4f3a9f7..730d2e1 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -64,11 +64,11 @@ public RobotContainer() // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive - () -> MathUtil.applyDeadband(-driverController.getLeftY(), + () -> MathUtil.applyDeadband(driverController.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(-driverController.getLeftX(), + () -> MathUtil.applyDeadband(driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> -driverController.getRightX(), + () -> driverController.getRightX(), () -> -driverController.getRightY(), driverController::getRightBumper, driverController::getLeftBumper diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index afcbef5..f0f5169 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -52,7 +52,7 @@ public void handleWheels() { return; } if (shootSup.getAsBoolean() && currPosition == Position.AMP) { - intakeSubsystem.setSpeed(-0.525, false); + intakeSubsystem.setSpeed(0.525, false); return; } if (shootSup.getAsBoolean() && currPosition == Position.INTAKE) { From eebc8c06a58755438668ee064926cccf90b9f8af Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sun, 3 Mar 2024 15:47:42 -0500 Subject: [PATCH 11/31] testing 2 --- src/main/deploy/swerve/neo/modules/backleft.json | 2 +- src/main/deploy/swerve/neo/modules/backright.json | 2 +- src/main/deploy/swerve/neo/modules/frontleft.json | 2 +- src/main/deploy/swerve/neo/modules/frontright.json | 2 +- src/main/deploy/swerve/neo/swervedrive.json | 2 +- src/main/java/frc/team7520/robot/Constants.java | 4 ++-- src/main/java/frc/team7520/robot/RobotContainer.java | 11 +++++++++-- .../frc/team7520/robot/commands/AbsoluteDrive.java | 4 ++-- src/main/java/frc/team7520/robot/commands/Intake.java | 10 +++++++++- .../java/frc/team7520/robot/commands/Shooter.java | 2 +- src/main/java/frc/team7520/robot/subsystems/LED.java | 6 +++--- .../robot/subsystems/shooter/ShooterSubsystem.java | 2 +- .../robot/subsystems/swerve/SwerveSubsystem.java | 2 ++ 13 files changed, 34 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index cd2e54a..75750b9 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -15,7 +15,7 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderOffset": 359.12109375, diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index eaa3eda..ce6d690 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -15,7 +15,7 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderOffset": 239.4140625, diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index 0113575..8794e6d 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -15,7 +15,7 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderOffset": 328.0078125, diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index fde0aae..442f343 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -15,7 +15,7 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderOffset": 151.259765625, diff --git a/src/main/deploy/swerve/neo/swervedrive.json b/src/main/deploy/swerve/neo/swervedrive.json index 3f9d2c2..5fb01c6 100644 --- a/src/main/deploy/swerve/neo/swervedrive.json +++ b/src/main/deploy/swerve/neo/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 044c19c..f89f189 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -69,8 +69,8 @@ public static class PivotConstants { public static final double GearAnglePerMotorRev = 360/GearRatio; public static final double ConversionFactor = 1/GearRatio; - public static final double Intake = 55.2 * GearAnglePerMotorRev; - public static final double Amp = 21.809415817260742 * GearAnglePerMotorRev; + public static final double Intake = 56 * GearAnglePerMotorRev; + public static final double Amp = 23.809415817260742 * GearAnglePerMotorRev; public static final double Shoot = 0; public static final double kP = 0.00022; diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 730d2e1..caa9f74 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; @@ -23,6 +24,7 @@ import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.LED; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; @@ -56,6 +58,10 @@ public class RobotContainer /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + LED.getInstance().test(); + + CameraServer.startAutomaticCapture(); + // Configure the trigger bindings configureBindings(); @@ -69,7 +75,7 @@ public RobotContainer() () -> MathUtil.applyDeadband(driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverController.getRightX(), - () -> -driverController.getRightY(), + () -> driverController.getRightY(), driverController::getRightBumper, driverController::getLeftBumper ); @@ -83,7 +89,8 @@ public RobotContainer() operatorController::getRightBumper, operatorController::getYButton, operatorController::getAButton, - operatorController::getBButton + operatorController::getBButton, + operatorController::getXButton ); // Old drive method diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index e97fa46..34bf9b2 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -88,9 +88,9 @@ public void execute() { ChassisSpeeds desiredSpeeds; if (CWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().minus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().minus(Rotation2d.fromDegrees(20))); } else if (CCWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),swerve.getHeading().plus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().plus(Rotation2d.fromDegrees(20))); } else { // Get the desired chassis speeds based on a 2 joystick module. desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index f0f5169..20ba108 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -19,6 +19,7 @@ public class Intake extends Command { private final BooleanSupplier shootPosSup; private final BooleanSupplier intakePosSup; private final BooleanSupplier ampPosSup; + private final BooleanSupplier reverseSup; public enum Position { SHOOT, @@ -35,12 +36,14 @@ public enum Position { * * @param intakeSubsystem The subsystem used by this command. */ - public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, BooleanSupplier shootPosSup, BooleanSupplier intakePosSup, BooleanSupplier ampPosSup) { + public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, BooleanSupplier shootPosSup, + BooleanSupplier intakePosSup, BooleanSupplier ampPosSup, BooleanSupplier reverseSup) { this.intakeSubsystem = intakeSubsystem; this.shootSup = shootSup; this.shootPosSup = shootPosSup; this.intakePosSup = intakePosSup; this.ampPosSup = ampPosSup; + this.reverseSup = reverseSup; // Use addRequirements() here to declare subsystem dependencies. addRequirements(intakeSubsystem); @@ -59,6 +62,10 @@ public void handleWheels() { intakeSubsystem.setSpeed(-0.35, false); return; } + if(reverseSup.getAsBoolean()) { + intakeSubsystem.setSpeed(-0.35); + return; + } intakeSubsystem.stop(); } @@ -78,6 +85,7 @@ public void handlePosition() { currPosition = Position.AMP; return; } + } // Called when the command is initially scheduled. diff --git a/src/main/java/frc/team7520/robot/commands/Shooter.java b/src/main/java/frc/team7520/robot/commands/Shooter.java index c0ec4da..157b1c9 100644 --- a/src/main/java/frc/team7520/robot/commands/Shooter.java +++ b/src/main/java/frc/team7520/robot/commands/Shooter.java @@ -30,7 +30,7 @@ public void initialize() { @Override public void execute() { - double throttle = throttleSup.getAsDouble() * (invertSup.getAsBoolean() ? -1 : 1); + double throttle = throttleSup.getAsDouble() * (invertSup.getAsBoolean() ? -1 : 1) * 1; shooterSubsystem.setSpeed(throttle, false); } diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java index 2ff1cd9..524caa1 100644 --- a/src/main/java/frc/team7520/robot/subsystems/LED.java +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -1,4 +1,4 @@ -package frc.team7520.robot.subsystems.swerve; +package frc.team7520.robot.subsystems; import com.ctre.phoenix.led.*; @@ -40,8 +40,8 @@ private LED() { } public void test(){ -// Animation animation = new RainbowAnimation(255, 0.75, 100); - Animation animation = new ColorFlowAnimation(255, 0, 0, 0, 0.75, 100, ColorFlowAnimation.Direction.Forward); + Animation animation = new RainbowAnimation(255, 0.75, 100); +// Animation animation = new ColorFlowAnimation(0, 255, 0, 0, 0.75, 100, ColorFlowAnimation.Direction.Forward); candle.animate(animation); } } diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java index fa31485..f1cb9b6 100644 --- a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -24,7 +24,7 @@ public class ShooterSubsystem extends SubsystemBase { private RelativeEncoder leftShooterEncoder; private RelativeEncoder rightShooterEncoder; - private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1); + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(0.5); /** * The Singleton instance of this shooterSubsystem. Code should use diff --git a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java index fe19188..0e88d75 100644 --- a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java @@ -322,6 +322,8 @@ public void setTargetHeading(Rotation2d angle){ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { xInput = Math.pow(xInput, 3); yInput = Math.pow(yInput, 3); + + swerveDrive.swerveController.lastAngleScalar = angle.getRadians(); return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), From 65e87d178a722d5d31e7be998fc797d09891cbe4 Mon Sep 17 00:00:00 2001 From: Cognoto Date: Wed, 6 Mar 2024 17:29:53 -0500 Subject: [PATCH 12/31] Climber Climber first version --- .../java/frc/team7520/robot/Constants.java | 11 ++ .../frc/team7520/robot/RobotContainer.java | 13 ++ .../frc/team7520/robot/commands/Climber.java | 131 ++++++++++++++++++ .../frc/team7520/robot/subsystems/LED.java | 2 +- .../subsystems/climber/ClimberSubsystem.java | 127 +++++++++++++++++ 5 files changed, 283 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/team7520/robot/commands/Climber.java create mode 100644 src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 044c19c..ee3f5c5 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -110,4 +110,15 @@ public static class ShooterConstants { public static final double MAX_RPM = 5676; } + + public static class ClimberConstants { + public static final int ClimberLeftID = 30; + public static final int ClimberRightID = 31; + + public static final double kP = 0.002; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kFF = 0.0006; + + } } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 730d2e1..27a7979 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,10 +19,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.commands.AbsoluteDrive; +import frc.team7520.robot.commands.Climber; import frc.team7520.robot.commands.Intake; import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; @@ -45,6 +47,8 @@ public class RobotContainer private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + private final ClimberSubsystem climberSubsystem = ClimberSubsystem.getInstance(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); @@ -86,6 +90,15 @@ public RobotContainer() operatorController::getBButton ); + Climber climber = new Climber(climberSubsystem, + operatorController::getLeftStickButton, + operatorController::getRightStickButton, + operatorController::getXButton, + operatorController::getRightY, + operatorController::getLeftY, + operatorController::getBackButton + ); + // Old drive method // like in video games // Easier to learn, harder to control diff --git a/src/main/java/frc/team7520/robot/commands/Climber.java b/src/main/java/frc/team7520/robot/commands/Climber.java new file mode 100644 index 0000000..535d43a --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/Climber.java @@ -0,0 +1,131 @@ +// 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.team7520.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import com.fasterxml.jackson.databind.ser.std.NumberSerializers.DoubleSerializer; +import com.revrobotics.CANSparkMax; + +/** An example command that uses an example subsystem. */ +public class Climber extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ClimberSubsystem subsystem; + private final BooleanSupplier bExtent; + private final BooleanSupplier bRetract; + private final BooleanSupplier bStop; + private final DoubleSupplier bRightManual; + private final DoubleSupplier bLeftManual; + private final BooleanSupplier bShift; + + private static final double maxPosition = 510; + private static final double motorSpeed = 0.9; + double leftMotorSpeed = 0; + double rightMotorSpeed = 0; + double pValue = 0.00001; + + enum State { + NOTHING, + EXTEND, + RETRACT + } + + State state = State.NOTHING; + + //public Position currPosition = Position.SHOOT; + + + + /** + * Creates a new ExampleCommand. + * + * @param climberSubsystem The subsystem used by this command. + */ + public Climber(ClimberSubsystem climberSubsystem, BooleanSupplier retract, BooleanSupplier extent, BooleanSupplier stop, DoubleSupplier rightManual, DoubleSupplier leftManual, BooleanSupplier shift) { + this.subsystem = climberSubsystem; + this.bRetract = retract; + this.bExtent = extent; + this.bStop = stop; + this.bRightManual = rightManual; + this.bLeftManual = leftManual; + this.bShift = shift; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(climberSubsystem); + } + + + + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + System.out.println("State: " + this.state); + double leftPosition = subsystem.getLeftPosition(); + double rightPosition = subsystem.getRightPosition(); + + /* + m_SparkMaxPIDController.setP(pValue); + m_SparkMaxPIDController.setI(0); + m_SparkMaxPIDController.setD(0); + m_SparkMaxPIDController.setOutputRange(-0.3, 0.3); + m_OtherSparkMaxPIDController.setP(pValue); + m_OtherSparkMaxPIDController.setI(0); + m_OtherSparkMaxPIDController.setD(0); + m_OtherSparkMaxPIDController.setOutputRange(-0.3, 0.3); + */ + + + + if (bExtent.getAsBoolean()) { + this.state = State.EXTEND; + subsystem.setRightArmReference(0); + subsystem.setLeftArmReference(0); + } else if (bRetract.getAsBoolean()) { + subsystem.setRightArmReference(maxPosition); + subsystem.setLeftArmReference(maxPosition); + this.state = State.RETRACT; + } else if (bStop.getAsBoolean()) { + if (bShift.getAsBoolean()) { + subsystem.setZeroPos(); + this.state = State.NOTHING; + } else { + subsystem.stop(); + this.state = State.NOTHING; + } + } + + if (this.state == State.NOTHING) { + subsystem.setRightSpeed(bRightManual.getAsDouble() / 2); + subsystem.setLeftSpeed(bLeftManual.getAsDouble() / 2); + } + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + //subsystem.setLeftSpeed(0); + //subsystem.setRightSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java index 2ff1cd9..d3589ae 100644 --- a/src/main/java/frc/team7520/robot/subsystems/LED.java +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -1,4 +1,4 @@ -package frc.team7520.robot.subsystems.swerve; +package frc.team7520.robot.subsystems; import com.ctre.phoenix.led.*; diff --git a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..927b498 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,127 @@ +// 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.team7520.robot.subsystems.climber; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; +import frc.team7520.robot.Constants.IntakeConstants; + + +public class ClimberSubsystem extends SubsystemBase { + + private CANSparkMax leftClimberMotor; + private CANSparkMax rightClimberMotor; + + private SparkPIDController leftClimberPID; + private SparkPIDController rightClimberPID; + + private RelativeEncoder leftClimberEncoder; + private RelativeEncoder rightClimberEncoder; + + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1); + + + private final static ClimberSubsystem INSTANCE = new ClimberSubsystem(); + + @SuppressWarnings("WeakerAccess") + public static ClimberSubsystem getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + private ClimberSubsystem() { + leftClimberMotor = new CANSparkMax(Constants.ClimberConstants.ClimberLeftID, CANSparkMax.MotorType.kBrushless); + rightClimberMotor = new CANSparkMax(Constants.ClimberConstants.ClimberRightID, CANSparkMax.MotorType.kBrushless); + + leftClimberMotor.restoreFactoryDefaults(); + rightClimberMotor.restoreFactoryDefaults(); + + leftClimberPID = leftClimberMotor.getPIDController(); + leftClimberPID.setP(Constants.ClimberConstants.kP); + leftClimberPID.setI(Constants.ClimberConstants.kI); + leftClimberPID.setD(Constants.ClimberConstants.kD); + //leftClimberPID.setFF(Constants.ClimberConstants.kFF); + + rightClimberPID = rightClimberMotor.getPIDController(); + rightClimberPID.setP(Constants.ClimberConstants.kP); + rightClimberPID.setI(Constants.ClimberConstants.kI); + rightClimberPID.setD(Constants.ClimberConstants.kD); + //rightClimberPID.setFF(Constants.ClimberConstants.kFF); + + leftClimberEncoder = leftClimberMotor.getEncoder(); + rightClimberEncoder = rightClimberMotor.getEncoder(); + + leftClimberMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + rightClimberMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + + leftClimberMotor.setInverted(false); + rightClimberMotor.setInverted(false); + } + + public void setPosition(Rotation2d position){ + //DesiredPosition = position; + //pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); + } + + public double getLeftPosition(){ + return leftClimberEncoder.getPosition(); + } + + public double getRightPosition(){ + return rightClimberEncoder.getPosition(); + } + + public void setLeftPosition(double pos){ + leftClimberEncoder.setPosition(pos); + } + + public void setRightPosition(double pos){ + rightClimberEncoder.setPosition(pos); + } + + public void setRightArmReference(double pos) { + rightClimberPID.setReference(pos, CANSparkMax.ControlType.kPosition); + } + + public void setLeftArmReference(double pos) { + leftClimberPID.setReference(pos, CANSparkMax.ControlType.kPosition); + } + + public void setLeftSpeed(double speed){ + leftClimberMotor.set(speed); + } + + public void setRightSpeed(double speed){ + rightClimberMotor.set(speed); + } + + public void setZeroPos() { + leftClimberEncoder.setPosition(0); + rightClimberEncoder.setPosition(0); + } + + + + public void stop() { + leftClimberMotor.set(0); + rightClimberMotor.set(0); + } + + @Override + public void periodic() { + System.out.println("Left Arm Position:" + leftClimberEncoder.getPosition()); + System.out.println("Right Arm Position:" + rightClimberEncoder.getPosition()); + } +} From 7002ac79415645e64a09848339e5dde70bba566a Mon Sep 17 00:00:00 2001 From: Cognoto Date: Wed, 6 Mar 2024 20:00:42 -0500 Subject: [PATCH 13/31] Climber (updated) Adds a couple forgotten lines of code --- src/main/java/frc/team7520/robot/RobotContainer.java | 1 + src/main/java/frc/team7520/robot/commands/Climber.java | 2 +- .../team7520/robot/subsystems/climber/ClimberSubsystem.java | 4 ++++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 27a7979..4d22919 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -113,6 +113,7 @@ public RobotContainer() drivebase.setDefaultCommand(closedAbsoluteDrive); shooterSubsystem.setDefaultCommand(shooter); intakeSubsystem.setDefaultCommand(intake); + climberSubsystem.setDefaultCommand(climber); } /** diff --git a/src/main/java/frc/team7520/robot/commands/Climber.java b/src/main/java/frc/team7520/robot/commands/Climber.java index 535d43a..88bc5b1 100644 --- a/src/main/java/frc/team7520/robot/commands/Climber.java +++ b/src/main/java/frc/team7520/robot/commands/Climber.java @@ -26,7 +26,7 @@ public class Climber extends Command { private final DoubleSupplier bLeftManual; private final BooleanSupplier bShift; - private static final double maxPosition = 510; + private static final double maxPosition = 530; private static final double motorSpeed = 0.9; double leftMotorSpeed = 0; double rightMotorSpeed = 0; diff --git a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java index 927b498..301a9ef 100644 --- a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java @@ -110,6 +110,8 @@ public void setRightSpeed(double speed){ public void setZeroPos() { leftClimberEncoder.setPosition(0); rightClimberEncoder.setPosition(0); + leftClimberPID.setReference(0, CANSparkMax.ControlType.kPosition); + rightClimberPID.setReference(0, CANSparkMax.ControlType.kPosition); } @@ -117,6 +119,8 @@ public void setZeroPos() { public void stop() { leftClimberMotor.set(0); rightClimberMotor.set(0); + leftClimberPID.setReference(leftClimberEncoder.getPosition(), CANSparkMax.ControlType.kPosition); + rightClimberPID.setReference(rightClimberEncoder.getPosition(), CANSparkMax.ControlType.kPosition); } @Override From f77cef8dcd9a4f9d6c40cd6c92a01c1f50cc684b Mon Sep 17 00:00:00 2001 From: Cognoto Date: Wed, 6 Mar 2024 20:11:21 -0500 Subject: [PATCH 14/31] merge changes from master --- src/main/java/frc/team7520/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 83c0a7b..02850c1 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -27,6 +27,7 @@ import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; import frc.team7520.robot.subsystems.climber.ClimberSubsystem; ======= +import frc.team7520.robot.subsystems.LED; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; From d2a6022dce9b9c5152c12870dabe41838959f8e3 Mon Sep 17 00:00:00 2001 From: Cognoto Date: Wed, 6 Mar 2024 20:13:28 -0500 Subject: [PATCH 15/31] Update RobotContainer.java --- src/main/java/frc/team7520/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 02850c1..0123288 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -26,7 +26,6 @@ import frc.team7520.robot.commands.TeleopDrive; import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; import frc.team7520.robot.subsystems.climber.ClimberSubsystem; -======= import frc.team7520.robot.subsystems.LED; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; From 134b7660d4f87066b19a3ef18d867ea07b6e67b8 Mon Sep 17 00:00:00 2001 From: Cognoto Date: Wed, 6 Mar 2024 20:14:29 -0500 Subject: [PATCH 16/31] change stop to start button --- src/main/java/frc/team7520/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 0123288..e72e512 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -100,7 +100,7 @@ public RobotContainer() Climber climber = new Climber(climberSubsystem, operatorController::getLeftStickButton, operatorController::getRightStickButton, - operatorController::getXButton, + operatorController::getStartButton, operatorController::getRightY, operatorController::getLeftY, operatorController::getBackButton From 7a9abc31deee86ef5cb19d1bead876cc7c62d2a2 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Wed, 6 Mar 2024 20:43:48 -0500 Subject: [PATCH 17/31] 3/5/2024 --- .../java/frc/team7520/robot/Constants.java | 14 +++---- .../subsystems/Intake/IntakeSubsystem.java | 29 ++++++++++++-- .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../frc/team7520/robot/util/DiffEncoder.java | 40 +++++++++++++++++++ 4 files changed, 74 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/team7520/robot/util/DiffEncoder.java diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index f89f189..520548d 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -66,11 +66,11 @@ public static class PivotConstants { public static final int CAN_ID = 23; public static final double GearRatio = 100; - public static final double GearAnglePerMotorRev = 360/GearRatio; - public static final double ConversionFactor = 1/GearRatio; + public static final double degreeConversionFactor = 360/GearRatio; + public static final double rotationConversionFactor = 1/GearRatio; - public static final double Intake = 56 * GearAnglePerMotorRev; - public static final double Amp = 23.809415817260742 * GearAnglePerMotorRev; + public static final double Intake = 211.374d; + public static final double Amp = 23.809415817260742 * degreeConversionFactor; public static final double Shoot = 0; public static final double kP = 0.00022; @@ -81,10 +81,10 @@ public static class PivotConstants { public static final double OutputMax = 1; public static final double OutputMin = -1; - public static final double SmartMaxVel = 30000000; + public static final double SmartMaxVel = 10000; public static final double SmartMinVel = 0; - public static final double SmartAccel = 40000; - public static final double SmartErr = 0.01; + public static final double SmartAccel = 1000; + public static final double SmartErr = 2; public static final int SlotID = 0; } public static class WheelConstants { diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java index 80203e4..c2f881d 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java @@ -4,7 +4,6 @@ package frc.team7520.robot.subsystems.Intake; -import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; @@ -13,9 +12,11 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants.IntakeConstants; +import frc.team7520.robot.util.DiffEncoder; public class IntakeSubsystem extends SubsystemBase { @@ -30,6 +31,13 @@ public class IntakeSubsystem extends SubsystemBase { public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); + + // rev through bore encoders + public DutyCycleEncoder wheelAbsEncoder = new DutyCycleEncoder(2); + public DutyCycleEncoder pivotAbsEncoder = new DutyCycleEncoder(1); + +// public DiffEncoder diffedEncoder = new DiffEncoder(pivotAbsEncoder, pivotAbsEncoder); + private final static IntakeSubsystem INSTANCE = new IntakeSubsystem(); @SuppressWarnings("WeakerAccess") @@ -41,7 +49,7 @@ public static IntakeSubsystem getInstance() { private IntakeSubsystem() { this.pivotEncoder = pivot.getEncoder(); pivotEncoder.setPosition(0); - pivotEncoder.setPositionConversionFactor(IntakeConstants.PivotConstants.ConversionFactor); + pivotEncoder.setPositionConversionFactor(IntakeConstants.PivotConstants.degreeConversionFactor); pivotPID.setP(IntakeConstants.PivotConstants.kP); pivotPID.setI(IntakeConstants.PivotConstants.kI); @@ -56,6 +64,9 @@ private IntakeSubsystem() { pivot.setIdleMode(CANSparkMax.IdleMode.kCoast); +// pivotAbsEncoder.setDistancePerRotation(360); + pivotAbsEncoder.setPositionOffset(0.159); + // Wheels wheels.setIdleMode(CANSparkMax.IdleMode.kBrake); // wheels.setInverted(true); @@ -64,11 +75,15 @@ private IntakeSubsystem() { wheelsPID.setI(IntakeConstants.WheelConstants.kI); wheelsPID.setD(IntakeConstants.WheelConstants.kD); wheelsPID.setFF(IntakeConstants.WheelConstants.kFF); + +// wheelAbsEncoder.setDistancePerRotation(360); + + } public void setPosition(Rotation2d position){ DesiredPosition = position; - pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); + pivotPID.setReference(DesiredPosition.getDegrees(), ControlType.kSmartMotion); } // public Command Shoot() { @@ -133,10 +148,18 @@ public void setSpeed(double speed, boolean closedLoop) { } } + + public double getDiffedEncoder(){ + return pivotAbsEncoder.get() - wheelAbsEncoder.get(); + } + @Override public void periodic() { SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); SmartDashboard.putNumber("DesiredDeg", DesiredPosition.getDegrees()); SmartDashboard.putNumber("DesiredRot", DesiredPosition.getRotations()); + SmartDashboard.putNumber("diffedEncoder", getDiffedEncoder()); + SmartDashboard.putNumber("PivotAbsEncoder", pivotAbsEncoder.get()); + SmartDashboard.putNumber("wheelsAbsEncoder", wheelAbsEncoder.get()); } } diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java index f1cb9b6..97b0701 100644 --- a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -24,7 +24,7 @@ public class ShooterSubsystem extends SubsystemBase { private RelativeEncoder leftShooterEncoder; private RelativeEncoder rightShooterEncoder; - private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(0.5); + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(5); /** * The Singleton instance of this shooterSubsystem. Code should use diff --git a/src/main/java/frc/team7520/robot/util/DiffEncoder.java b/src/main/java/frc/team7520/robot/util/DiffEncoder.java new file mode 100644 index 0000000..b83af41 --- /dev/null +++ b/src/main/java/frc/team7520/robot/util/DiffEncoder.java @@ -0,0 +1,40 @@ +package frc.team7520.robot.util; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class DiffEncoder{ + + private DutyCycleEncoder encoder1; + private DutyCycleEncoder encoder2; + + public DiffEncoder(DutyCycleEncoder encoder1, DutyCycleEncoder encoder2){ + this.encoder1 = encoder1; + this.encoder2 = encoder2; + } + + public double get(){ + return encoder1.get() - encoder2.get(); + } + + public double getRate(){ + return encoder1.getFrequency() - encoder2.getFrequency(); + } + + public void reset(){ + encoder1.reset(); + encoder2.reset(); + } + + public void setDistancePerRotation(double distance){ + encoder1.setDistancePerRotation(distance); + encoder2.setDistancePerRotation(distance); + } + + public double getDistancePerRotation(){ + return encoder1.getDistancePerRotation(); + } + + public double getDistance(){ + return encoder1.getDistance() - encoder2.getDistance(); + } +} From c7901e3c0e02e558ba3b18e90a05293bdae3ed60 Mon Sep 17 00:00:00 2001 From: IAmRice33 Date: Thu, 7 Mar 2024 16:43:25 -0500 Subject: [PATCH 18/31] LED --- .../frc/team7520/robot/RobotContainer.java | 43 +++++++++++++++---- .../frc/team7520/robot/commands/Intake.java | 11 ++++- .../subsystems/Intake/IntakeSubsystem.java | 7 ++- .../frc/team7520/robot/subsystems/LED.java | 41 ++++++++++++++++-- 4 files changed, 87 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index caa9f74..e8f5b07 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.team7520.robot.commands.Intake; import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; +import frc.team7520.robot.commands.Intake.Position; import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; import frc.team7520.robot.subsystems.LED; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; @@ -47,6 +48,8 @@ public class RobotContainer private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + private final LED LEDSubsystem = LED.getInstance(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); @@ -54,11 +57,19 @@ public class RobotContainer private final XboxController operatorController = new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); + private final Intake intake = new Intake(intakeSubsystem, + operatorController::getRightBumper, + operatorController::getYButton, + operatorController::getAButton, + operatorController::getBButton, + operatorController::getXButton, + intakeSubsystem::getSwitchVal + ); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - LED.getInstance().test(); CameraServer.startAutomaticCapture(); @@ -85,13 +96,14 @@ public RobotContainer() operatorController::getLeftBumper ); - Intake intake = new Intake(intakeSubsystem, - operatorController::getRightBumper, - operatorController::getYButton, - operatorController::getAButton, - operatorController::getBButton, - operatorController::getXButton - ); + // Intake intake = new Intake(intakeSubsystem, + // operatorController::getRightBumper, + // operatorController::getYButton, + // operatorController::getAButton, + // operatorController::getBButton, + // operatorController::getXButton, + // intakeSubsystem::getSwitchVal + // ); // Old drive method // like in video games @@ -107,6 +119,7 @@ public RobotContainer() drivebase.setDefaultCommand(closedAbsoluteDrive); shooterSubsystem.setDefaultCommand(shooter); intakeSubsystem.setDefaultCommand(intake); + LEDSubsystem.setDefaultCommand(LEDSubsystem.idle()); } /** @@ -136,6 +149,20 @@ private void configureBindings() // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); + + new Trigger(() -> intake.currPosition == Position.INTAKE) + .and(new JoystickButton(operatorController, XboxController.Button.kRightBumper.value)) + .onTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.clear()); + + new Trigger(() -> intake.currPosition == Position.INTAKE) + .and(new JoystickButton(operatorController, XboxController.Button.kX.value)) + .whileTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.clear()); + + new Trigger(intakeSubsystem::getSwitchVal) + .whileFalse(new RepeatCommand(LEDSubsystem.noteIn())) + .onTrue(LEDSubsystem.clear()); } diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index 20ba108..d4b5a78 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -20,6 +20,7 @@ public class Intake extends Command { private final BooleanSupplier intakePosSup; private final BooleanSupplier ampPosSup; private final BooleanSupplier reverseSup; + private final BooleanSupplier switchSup; public enum Position { SHOOT, @@ -37,13 +38,14 @@ public enum Position { * @param intakeSubsystem The subsystem used by this command. */ public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, BooleanSupplier shootPosSup, - BooleanSupplier intakePosSup, BooleanSupplier ampPosSup, BooleanSupplier reverseSup) { + BooleanSupplier intakePosSup, BooleanSupplier ampPosSup, BooleanSupplier reverseSup, BooleanSupplier switchSup) { this.intakeSubsystem = intakeSubsystem; this.shootSup = shootSup; this.shootPosSup = shootPosSup; this.intakePosSup = intakePosSup; this.ampPosSup = ampPosSup; this.reverseSup = reverseSup; + this.switchSup = switchSup; // Use addRequirements() here to declare subsystem dependencies. addRequirements(intakeSubsystem); @@ -59,7 +61,12 @@ public void handleWheels() { return; } if (shootSup.getAsBoolean() && currPosition == Position.INTAKE) { - intakeSubsystem.setSpeed(-0.35, false); + if (switchSup.getAsBoolean()) { + intakeSubsystem.setSpeed(-0.35, false); + } + else { + intakeSubsystem.setSpeed(0, false); + } return; } if(reverseSup.getAsBoolean()) { diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java index c2f881d..a2941b6 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,11 +28,12 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkPIDController pivotPID = pivot.getPIDController(); private final SparkPIDController wheelsPID = wheels.getPIDController(); + private final DigitalInput input = new DigitalInput(0); + private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.5); public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); - // rev through bore encoders public DutyCycleEncoder wheelAbsEncoder = new DutyCycleEncoder(2); public DutyCycleEncoder pivotAbsEncoder = new DutyCycleEncoder(1); @@ -148,6 +150,9 @@ public void setSpeed(double speed, boolean closedLoop) { } } + public boolean getSwitchVal() { + return input.get(); + } public double getDiffedEncoder(){ return pivotAbsEncoder.get() - wheelAbsEncoder.get(); diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java index 524caa1..c14e9a5 100644 --- a/src/main/java/frc/team7520/robot/subsystems/LED.java +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -2,6 +2,10 @@ import com.ctre.phoenix.led.*; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LED extends SubsystemBase { @@ -19,6 +23,9 @@ public class LED extends SubsystemBase { public final static CANdle candle = new CANdle(17); + private final Animation idleAnimation = new RainbowAnimation(255, 0.75, 100); + private final Animation intakingAnimation = new ColorFlowAnimation(255, 165, 0, 0, 0.75, 100, Direction.Forward); + private final Animation noteIn = new ColorFlowAnimation(0, 255, 0, 0, 0.75, 100, Direction.Forward); /** * Returns the Singleton instance of this LED. This static method @@ -39,10 +46,36 @@ private LED() { } - public void test(){ - Animation animation = new RainbowAnimation(255, 0.75, 100); -// Animation animation = new ColorFlowAnimation(0, 255, 0, 0, 0.75, 100, ColorFlowAnimation.Direction.Forward); - candle.animate(animation); + public Command idle() { + return run( + () -> { + candle.animate(idleAnimation); + } + ); + } + + public InstantCommand intaking() { + return new InstantCommand( + () -> { + candle.animate(intakingAnimation); + } + ); + } + + public InstantCommand noteIn() { + return new InstantCommand( + () -> { + candle.animate(noteIn); + } + ); + } + + public InstantCommand clear() { + return new InstantCommand( + () -> { + candle.clearAnimation(0); + } + ); } } From 634d925a563408f9764457c7a17b313a25140948 Mon Sep 17 00:00:00 2001 From: Cognoto Date: Thu, 7 Mar 2024 19:53:35 -0500 Subject: [PATCH 19/31] add climber init pos set --- .../java/frc/team7520/robot/Constants.java | 3 ++- .../frc/team7520/robot/RobotContainer.java | 2 +- .../frc/team7520/robot/commands/Climber.java | 24 +++++++++++++++---- .../subsystems/climber/ClimberSubsystem.java | 13 +++++----- 4 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 4f31792..ef37269 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -114,8 +114,9 @@ public static class ShooterConstants { public static class ClimberConstants { public static final int ClimberLeftID = 30; public static final int ClimberRightID = 31; + public static final int maxPosition = 520; - public static final double kP = 0.002; + public static final double kP = 0.004; public static final double kI = 0.0; public static final double kD = 0.0; public static final double kFF = 0.0006; diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index e72e512..0cd39ce 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -96,7 +96,7 @@ public RobotContainer() operatorController::getBButton, operatorController::getXButton ); - + Climber climber = new Climber(climberSubsystem, operatorController::getLeftStickButton, operatorController::getRightStickButton, diff --git a/src/main/java/frc/team7520/robot/commands/Climber.java b/src/main/java/frc/team7520/robot/commands/Climber.java index 88bc5b1..e05ff37 100644 --- a/src/main/java/frc/team7520/robot/commands/Climber.java +++ b/src/main/java/frc/team7520/robot/commands/Climber.java @@ -5,7 +5,10 @@ package frc.team7520.robot.commands; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.team7520.robot.Constants; +import frc.team7520.robot.Constants.ClimberConstants; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; import frc.team7520.robot.subsystems.climber.ClimberSubsystem; import edu.wpi.first.wpilibj2.command.Command; @@ -26,7 +29,6 @@ public class Climber extends Command { private final DoubleSupplier bLeftManual; private final BooleanSupplier bShift; - private static final double maxPosition = 530; private static final double motorSpeed = 0.9; double leftMotorSpeed = 0; double rightMotorSpeed = 0; @@ -57,6 +59,8 @@ public Climber(ClimberSubsystem climberSubsystem, BooleanSupplier retract, Boole this.bRightManual = rightManual; this.bLeftManual = leftManual; this.bShift = shift; + subsystem.setLeftPosition(ClimberConstants.maxPosition); + subsystem.setRightPosition(ClimberConstants.maxPosition); // Use addRequirements() here to declare subsystem dependencies. addRequirements(climberSubsystem); @@ -68,13 +72,25 @@ public Climber(ClimberSubsystem climberSubsystem, BooleanSupplier retract, Boole // Called when the command is initially scheduled. @Override public void initialize() { + + } + + public String convertState(State state) { + if (state == State.NOTHING) { + return "NOTHING"; + } else if (state == State.EXTEND) { + return "EXTEND"; + } else if (state == State.RETRACT) { + return "RETRACT"; + } + return ""; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.out.println("State: " + this.state); + SmartDashboard.putString("State", convertState(this.state)); double leftPosition = subsystem.getLeftPosition(); double rightPosition = subsystem.getRightPosition(); @@ -96,8 +112,8 @@ public void execute() { subsystem.setRightArmReference(0); subsystem.setLeftArmReference(0); } else if (bRetract.getAsBoolean()) { - subsystem.setRightArmReference(maxPosition); - subsystem.setLeftArmReference(maxPosition); + subsystem.setRightArmReference(ClimberConstants.maxPosition); + subsystem.setLeftArmReference(ClimberConstants.maxPosition); this.state = State.RETRACT; } else if (bStop.getAsBoolean()) { if (bShift.getAsBoolean()) { diff --git a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java index 301a9ef..bb86025 100644 --- a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants; import frc.team7520.robot.Constants.IntakeConstants; +import frc.team7520.robot.Constants.ClimberConstants; public class ClimberSubsystem extends SubsystemBase { @@ -108,10 +109,10 @@ public void setRightSpeed(double speed){ } public void setZeroPos() { - leftClimberEncoder.setPosition(0); - rightClimberEncoder.setPosition(0); - leftClimberPID.setReference(0, CANSparkMax.ControlType.kPosition); - rightClimberPID.setReference(0, CANSparkMax.ControlType.kPosition); + leftClimberEncoder.setPosition(ClimberConstants.maxPosition); + rightClimberEncoder.setPosition(ClimberConstants.maxPosition); + leftClimberPID.setReference(ClimberConstants.maxPosition, CANSparkMax.ControlType.kPosition); + rightClimberPID.setReference(ClimberConstants.maxPosition, CANSparkMax.ControlType.kPosition); } @@ -125,7 +126,7 @@ public void stop() { @Override public void periodic() { - System.out.println("Left Arm Position:" + leftClimberEncoder.getPosition()); - System.out.println("Right Arm Position:" + rightClimberEncoder.getPosition()); + SmartDashboard.putNumber("ClimberL", leftClimberEncoder.getPosition()); + SmartDashboard.putNumber("ClimberR", rightClimberEncoder.getPosition()); } } From 48826c3a714dc22ae58125aa779e2a1be5caa9a3 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sat, 9 Mar 2024 10:09:05 -0500 Subject: [PATCH 20/31] Comp working Auto should be working not tested yet --- .pathplanner/settings.json | 6 +- src/main/deploy/pathplanner/autos/Demo1.auto | 37 ------- src/main/deploy/pathplanner/autos/Demo2.auto | 37 ------- .../pathplanner/paths/Amp to mid note.path | 102 ------------------ .../deploy/pathplanner/paths/Mid to amp.path | 52 --------- .../paths/Source note to source side.path | 64 ----------- .../paths/Source side to source note.path | 64 ----------- .../deploy/swerve/neo/modules/backleft.json | 4 +- .../deploy/swerve/neo/modules/backright.json | 4 +- .../deploy/swerve/neo/modules/frontleft.json | 4 +- .../deploy/swerve/neo/modules/frontright.json | 4 +- .../java/frc/team7520/robot/Constants.java | 41 +++++-- .../frc/team7520/robot/RobotContainer.java | 42 ++++---- .../frc/team7520/robot/auto/AutoIntake.java | 46 ++++++++ .../frc/team7520/robot/auto/AutoShoot.java | 44 ++++++++ .../team7520/robot/auto/ShootSequence.java | 29 +++++ .../frc/team7520/robot/commands/Climber.java | 20 ++-- .../frc/team7520/robot/commands/Intake.java | 29 ++--- .../frc/team7520/robot/subsystems/LED.java | 2 +- .../subsystems/climber/ClimberSubsystem.java | 10 +- .../{Intake => intake}/IntakeSubsystem.java | 25 +++-- .../subsystems/shooter/ShooterSubsystem.java | 4 +- 22 files changed, 231 insertions(+), 439 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Demo1.auto delete mode 100644 src/main/deploy/pathplanner/autos/Demo2.auto delete mode 100644 src/main/deploy/pathplanner/paths/Amp to mid note.path delete mode 100644 src/main/deploy/pathplanner/paths/Mid to amp.path delete mode 100644 src/main/deploy/pathplanner/paths/Source note to source side.path delete mode 100644 src/main/deploy/pathplanner/paths/Source side to source note.path create mode 100644 src/main/java/frc/team7520/robot/auto/AutoIntake.java create mode 100644 src/main/java/frc/team7520/robot/auto/AutoShoot.java create mode 100644 src/main/java/frc/team7520/robot/auto/ShootSequence.java rename src/main/java/frc/team7520/robot/subsystems/{Intake => intake}/IntakeSubsystem.java (89%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index b3758f6..c43b5ce 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.7, - "robotLength": 0.7, + "robotWidth": 0.8, + "robotLength": 0.8, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -8,5 +8,5 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5 + "maxModuleSpeed": 4.6 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Demo1.auto b/src/main/deploy/pathplanner/autos/Demo1.auto deleted file mode 100644 index 09f1cf9..0000000 --- a/src/main/deploy/pathplanner/autos/Demo1.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3763132980811417, - "y": 5.569261320340823 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mid to amp" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Amp to mid note" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Demo2.auto b/src/main/deploy/pathplanner/autos/Demo2.auto deleted file mode 100644 index 7a80bde..0000000 --- a/src/main/deploy/pathplanner/autos/Demo2.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7792735585471171, - "y": 4.440483062784308 - }, - "rotation": 120.00492087082401 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Source side to source note" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "path", - "data": { - "pathName": "Source note to source side" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp to mid note.path b/src/main/deploy/pathplanner/paths/Amp to mid note.path deleted file mode 100644 index 6975456..0000000 --- a/src/main/deploy/pathplanner/paths/Amp to mid note.path +++ /dev/null @@ -1,102 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.8024918101118255, - "y": 7.895182457031582 - }, - "prevControl": null, - "nextControl": { - "x": 1.7651768263889298, - "y": 6.747746707611961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.9173805620338513, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 1.8167440858156032, - "y": 6.612941065997554 - }, - "nextControl": { - "x": 1.9267093079660895, - "y": 5.513288844762175 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.402475350405246, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 2.589050269009629, - "y": 5.625233795922139 - }, - "nextControl": { - "x": 2.215900431800863, - "y": 5.587918812201261 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2281684392428616, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 1.1582028447662178, - "y": 5.59724755813148 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.85, - "rotationDegrees": 180.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 1.1500000000000001, - "maxWaypointRelativePos": 1.75, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 90.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid to amp.path b/src/main/deploy/pathplanner/paths/Mid to amp.path deleted file mode 100644 index a9e23a7..0000000 --- a/src/main/deploy/pathplanner/paths/Mid to amp.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3763132980811417, - "y": 5.569261320340823 - }, - "prevControl": null, - "nextControl": { - "x": 1.9267093079640694, - "y": 6.306232248828135 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8054356108712217, - "y": 7.649571662779689 - }, - "prevControl": { - "x": 1.8427505945920981, - "y": 6.604752118595146 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source note to source side.path b/src/main/deploy/pathplanner/paths/Source note to source side.path deleted file mode 100644 index c5cd6a3..0000000 --- a/src/main/deploy/pathplanner/paths/Source note to source side.path +++ /dev/null @@ -1,64 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.551735285288752, - "y": 3.6568684046459006 - }, - "prevControl": null, - "nextControl": { - "x": 1.7774493730805638, - "y": 3.544923453483271 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.8818897637795275, - "y": 4.41249682499365 - }, - "prevControl": { - "x": 1.4229570277322372, - "y": 3.684854642436558 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 123.94358701975182, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -136.68468431789634, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source side to source note.path b/src/main/deploy/pathplanner/paths/Source side to source note.path deleted file mode 100644 index 48408b6..0000000 --- a/src/main/deploy/pathplanner/paths/Source side to source note.path +++ /dev/null @@ -1,64 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7792735585471171, - "y": 4.440483062784308 - }, - "prevControl": null, - "nextControl": { - "x": 1.779273558547119, - "y": 3.940483062784308 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.533077793428314, - "y": 3.694183388366777 - }, - "prevControl": { - "x": 1.7774493730805647, - "y": 3.0878149029025335 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.8, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -139.93921554212616, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 120.57922687248902, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index 75750b9..ceb0e38 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": true, + "drive": false, "angle": true }, - "absoluteEncoderOffset": 359.12109375, + "absoluteEncoderOffset": 179.12109375, "location": { "front": -13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index ce6d690..ea26059 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": true, + "drive": false, "angle": true }, - "absoluteEncoderOffset": 239.4140625, + "absoluteEncoderOffset": 59.4140625, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index 8794e6d..83a20b5 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": true, + "drive": false, "angle": true }, - "absoluteEncoderOffset": 328.0078125, + "absoluteEncoderOffset": 148.0078125, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 442f343..261ce9d 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": true, + "drive": false, "angle": true }, - "absoluteEncoderOffset": 151.259765625, + "absoluteEncoderOffset": 331.259765625, "location": { "front": 13, "left": -13 diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index eff413a..58f41b4 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -4,6 +4,7 @@ package frc.team7520.robot; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; @@ -62,6 +63,28 @@ public static class Telemetry { } public static class IntakeConstants { + public enum Position { + SHOOT(new Rotation2d(0), 1), + INTAKE(new Rotation2d(Units.degreesToRadians(211.374d)), 0.35), + AMP(new Rotation2d(Units.degreesToRadians(90)), 0.525); + + private final Rotation2d position; + private final double speed; + + Position(Rotation2d position, double speed) { + this.position = position; + this.speed = speed; + } + + public Rotation2d getPosition() { + return position; + } + + public double getSpeed() { + return speed; + } + } + public static class PivotConstants { public static final int CAN_ID = 23; @@ -78,12 +101,12 @@ public static class PivotConstants { public static final double kD = 0; public static final double kFF = 0.000156; - public static final double OutputMax = 1; - public static final double OutputMin = -1; + public static final double OUTPUT_MAX = 1; + public static final double OUTPUT_MIN = -1; - public static final double SmartMaxVel = 10000; + public static final double SmartMaxVel = 20000; public static final double SmartMinVel = 0; - public static final double SmartAccel = 1000; + public static final double SmartAccel = 2000; public static final double SmartErr = 2; public static final int SlotID = 0; } @@ -100,8 +123,8 @@ public static class WheelConstants { } public static class ShooterConstants { - public static final int ShooterLeftID = 20; - public static final int ShooterRightID = 21; + public static final int shooterLeftID = 20; + public static final int shooterRightID = 21; public static final double kP = 0.002; public static final double kI = 0.0; @@ -109,11 +132,13 @@ public static class ShooterConstants { public static final double kFF = 0.000156; public static final double MAX_RPM = 5676; + + } public static class ClimberConstants { - public static final int ClimberLeftID = 30; - public static final int ClimberRightID = 31; + public static final int climberLeftID = 30; + public static final int climberRightID = 31; public static final int maxPosition = 520; public static final double kP = 0.004; diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 81042a7..6a712c0 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -19,20 +19,22 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team7520.robot.auto.ShootSequence; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.Climber; import frc.team7520.robot.commands.Intake; import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; -import frc.team7520.robot.commands.Intake.Position; -import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; import frc.team7520.robot.subsystems.climber.ClimberSubsystem; import frc.team7520.robot.subsystems.LED; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; +import static frc.team7520.robot.subsystems.LED.candle; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -74,6 +76,8 @@ public class RobotContainer public RobotContainer() { + registerNamedCommands(); + CameraServer.startAutomaticCapture(); // Configure the trigger bindings @@ -88,8 +92,8 @@ public RobotContainer() OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> driverController.getRightX(), - () -> driverController.getRightY(), + () -> -driverController.getRightX(), + () -> -driverController.getRightY(), driverController::getRightBumper, driverController::getLeftBumper ); @@ -99,10 +103,10 @@ public RobotContainer() operatorController::getLeftBumper ); - + Climber climber = new Climber(climberSubsystem, - operatorController::getLeftStickButton, - operatorController::getRightStickButton, + () -> false, + () -> false, operatorController::getStartButton, operatorController::getRightY, operatorController::getLeftY, @@ -133,6 +137,8 @@ public RobotContainer() intakeSubsystem.setDefaultCommand(intake); climberSubsystem.setDefaultCommand(climber); LEDSubsystem.setDefaultCommand(LEDSubsystem.idle()); + + candle.animate(LEDSubsystem.idleAnimation); } /** @@ -142,7 +148,8 @@ public RobotContainer() private void registerNamedCommands() { // Example - NamedCommands.registerCommand("Shoot", new WaitCommand(1)); + NamedCommands.registerCommand("shoot", new ShootSequence()); + NamedCommands.registerCommand("log", new InstantCommand(() -> System.out.println("eeeeeeeeeeeeeeeeeeeeeeeee"))); } /** @@ -163,22 +170,21 @@ private void configureBindings() new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); - new Trigger(() -> intake.currPosition == Position.INTAKE) + new Trigger(() -> intake.currPosition == Constants.IntakeConstants.Position.INTAKE) .and(new JoystickButton(operatorController, XboxController.Button.kRightBumper.value)) - .onTrue(new RepeatCommand(LEDSubsystem.intaking())) - .onFalse(LEDSubsystem.clear()); - - new Trigger(() -> intake.currPosition == Position.INTAKE) + .onTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.idle()); + + new Trigger(() -> intake.currPosition == Constants.IntakeConstants.Position.INTAKE) .and(new JoystickButton(operatorController, XboxController.Button.kX.value)) - .whileTrue(new RepeatCommand(LEDSubsystem.intaking())) - .onFalse(LEDSubsystem.clear()); + .whileTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.idle()); new Trigger(intakeSubsystem::getSwitchVal) .whileFalse(new RepeatCommand(LEDSubsystem.noteIn())) - .onTrue(LEDSubsystem.clear()); + .onTrue(LEDSubsystem.idle()); } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -186,6 +192,6 @@ private void configureBindings() */ public Command getAutonomousCommand() { - return drivebase.getPPAutoCommand("Demo1", true); + return drivebase.getPPAutoCommand("test", true); } } diff --git a/src/main/java/frc/team7520/robot/auto/AutoIntake.java b/src/main/java/frc/team7520/robot/auto/AutoIntake.java new file mode 100644 index 0000000..e2965af --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/AutoIntake.java @@ -0,0 +1,46 @@ +package frc.team7520.robot.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; + + +public class AutoIntake extends Command { +// private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + + private final Constants.IntakeConstants.Position desiredPos; + + public AutoIntake(Constants.IntakeConstants.Position desiredPos) { + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements( +// this.shooterSubsystem, + this.intakeSubsystem + ); + + this.desiredPos = desiredPos; + } + + @Override + public void initialize() { + intakeSubsystem.setPosition(desiredPos); +// shooterSubsystem.setSpeed(1, false); + } + + @Override + public void execute() { + + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return intakeSubsystem.atPosition(); + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/auto/AutoShoot.java b/src/main/java/frc/team7520/robot/auto/AutoShoot.java new file mode 100644 index 0000000..0d0823b --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/AutoShoot.java @@ -0,0 +1,44 @@ +package frc.team7520.robot.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + + +public class AutoShoot extends Command { + private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + private final double power; + + private final boolean closedLoop; + public AutoShoot(double power, boolean closedLoop) { + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.shooterSubsystem); + + this.power = power; + this.closedLoop = closedLoop; + + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + shooterSubsystem.setSpeed(power, closedLoop); + + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/auto/ShootSequence.java b/src/main/java/frc/team7520/robot/auto/ShootSequence.java new file mode 100644 index 0000000..7d77a81 --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/ShootSequence.java @@ -0,0 +1,29 @@ +package frc.team7520.robot.auto; + + +import edu.wpi.first.wpilibj2.command.*; +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + +public class ShootSequence extends SequentialCommandGroup { + + public ShootSequence() { + + // TODO: Add your sequential commands in the super() call, e.g. + // super(new OpenClawCommand(), new MoveArmCommand()); + super( + new ParallelCommandGroup( + new AutoIntake(Constants.IntakeConstants.Position.SHOOT), + new ParallelRaceGroup( + new AutoShoot(1, false), + new WaitCommand(1.5) + ) + ), + new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(1)), + new WaitCommand(0.5), + new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(0)), + new InstantCommand(() -> ShooterSubsystem.getInstance().setSpeed(0, false)) + ); + } +} diff --git a/src/main/java/frc/team7520/robot/commands/Climber.java b/src/main/java/frc/team7520/robot/commands/Climber.java index e05ff37..7c7eca3 100644 --- a/src/main/java/frc/team7520/robot/commands/Climber.java +++ b/src/main/java/frc/team7520/robot/commands/Climber.java @@ -39,7 +39,7 @@ enum State { EXTEND, RETRACT } - + State state = State.NOTHING; //public Position currPosition = Position.SHOOT; @@ -61,13 +61,13 @@ public Climber(ClimberSubsystem climberSubsystem, BooleanSupplier retract, Boole this.bShift = shift; subsystem.setLeftPosition(ClimberConstants.maxPosition); subsystem.setRightPosition(ClimberConstants.maxPosition); - + // Use addRequirements() here to declare subsystem dependencies. addRequirements(climberSubsystem); } - - + + // Called when the command is initially scheduled. @Override @@ -94,7 +94,7 @@ public void execute() { double leftPosition = subsystem.getLeftPosition(); double rightPosition = subsystem.getRightPosition(); - /* + /* m_SparkMaxPIDController.setP(pValue); m_SparkMaxPIDController.setI(0); m_SparkMaxPIDController.setD(0); @@ -104,9 +104,9 @@ public void execute() { m_OtherSparkMaxPIDController.setD(0); m_OtherSparkMaxPIDController.setOutputRange(-0.3, 0.3); */ - - + + if (bExtent.getAsBoolean()) { this.state = State.EXTEND; subsystem.setRightArmReference(0); @@ -124,10 +124,10 @@ public void execute() { this.state = State.NOTHING; } } - + if (this.state == State.NOTHING) { - subsystem.setRightSpeed(bRightManual.getAsDouble() / 2); - subsystem.setLeftSpeed(bLeftManual.getAsDouble() / 2); + subsystem.setRightSpeed(bRightManual.getAsDouble()); + subsystem.setLeftSpeed(bLeftManual.getAsDouble()); } } diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java index d4b5a78..e79dadf 100644 --- a/src/main/java/frc/team7520/robot/commands/Intake.java +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -4,9 +4,8 @@ package frc.team7520.robot.commands; -import edu.wpi.first.math.geometry.Rotation2d; import frc.team7520.robot.Constants; -import frc.team7520.robot.subsystems.Intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.BooleanSupplier; @@ -22,13 +21,7 @@ public class Intake extends Command { private final BooleanSupplier reverseSup; private final BooleanSupplier switchSup; - public enum Position { - SHOOT, - INTAKE, - AMP - } - - public Position currPosition = Position.SHOOT; + public Constants.IntakeConstants.Position currPosition = Constants.IntakeConstants.Position.SHOOT; @@ -52,15 +45,15 @@ public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, Boolean } public void handleWheels() { - if (shootSup.getAsBoolean() && currPosition == Position.SHOOT) { + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.SHOOT) { intakeSubsystem.setSpeed(0.35, false); return; } - if (shootSup.getAsBoolean() && currPosition == Position.AMP) { + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.AMP) { intakeSubsystem.setSpeed(0.525, false); return; } - if (shootSup.getAsBoolean() && currPosition == Position.INTAKE) { + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.INTAKE) { if (switchSup.getAsBoolean()) { intakeSubsystem.setSpeed(-0.35, false); } @@ -78,18 +71,18 @@ public void handleWheels() { public void handlePosition() { if (shootPosSup.getAsBoolean()) { - intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Shoot)); - currPosition = Position.SHOOT; + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.SHOOT); + currPosition = Constants.IntakeConstants.Position.SHOOT; return; } if (intakePosSup.getAsBoolean()) { - intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Intake)); - currPosition = Position.INTAKE; + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.INTAKE); + currPosition = Constants.IntakeConstants.Position.INTAKE; return; } if (ampPosSup.getAsBoolean()) { - intakeSubsystem.setPosition(Rotation2d.fromDegrees(Constants.IntakeConstants.PivotConstants.Amp)); - currPosition = Position.AMP; + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.AMP); + currPosition = Constants.IntakeConstants.Position.AMP; return; } diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java index c14e9a5..8e4cb0b 100644 --- a/src/main/java/frc/team7520/robot/subsystems/LED.java +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -23,7 +23,7 @@ public class LED extends SubsystemBase { public final static CANdle candle = new CANdle(17); - private final Animation idleAnimation = new RainbowAnimation(255, 0.75, 100); + public final Animation idleAnimation = new RainbowAnimation(255, 0.75, 100); private final Animation intakingAnimation = new ColorFlowAnimation(255, 165, 0, 0, 0.75, 100, Direction.Forward); private final Animation noteIn = new ColorFlowAnimation(0, 255, 0, 0, 0.75, 100, Direction.Forward); diff --git a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java index bb86025..a4eb999 100644 --- a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java @@ -4,19 +4,15 @@ package frc.team7520.robot.subsystems.climber; -import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants; -import frc.team7520.robot.Constants.IntakeConstants; import frc.team7520.robot.Constants.ClimberConstants; @@ -43,8 +39,8 @@ public static ClimberSubsystem getInstance() { /** Creates a new ExampleSubsystem. */ private ClimberSubsystem() { - leftClimberMotor = new CANSparkMax(Constants.ClimberConstants.ClimberLeftID, CANSparkMax.MotorType.kBrushless); - rightClimberMotor = new CANSparkMax(Constants.ClimberConstants.ClimberRightID, CANSparkMax.MotorType.kBrushless); + leftClimberMotor = new CANSparkMax(Constants.ClimberConstants.climberLeftID, CANSparkMax.MotorType.kBrushless); + rightClimberMotor = new CANSparkMax(Constants.ClimberConstants.climberRightID, CANSparkMax.MotorType.kBrushless); leftClimberMotor.restoreFactoryDefaults(); rightClimberMotor.restoreFactoryDefaults(); @@ -115,7 +111,7 @@ public void setZeroPos() { rightClimberPID.setReference(ClimberConstants.maxPosition, CANSparkMax.ControlType.kPosition); } - + public void stop() { leftClimberMotor.set(0); diff --git a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java similarity index 89% rename from src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java rename to src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java index a2941b6..3a8a01e 100644 --- a/src/main/java/frc/team7520/robot/subsystems/Intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java @@ -2,7 +2,7 @@ // 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.team7520.robot.subsystems.Intake; +package frc.team7520.robot.subsystems.intake; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; @@ -16,8 +16,8 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; import frc.team7520.robot.Constants.IntakeConstants; -import frc.team7520.robot.util.DiffEncoder; public class IntakeSubsystem extends SubsystemBase { @@ -32,7 +32,7 @@ public class IntakeSubsystem extends SubsystemBase { private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.5); - public Rotation2d DesiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); + public Rotation2d desiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); // rev through bore encoders public DutyCycleEncoder wheelAbsEncoder = new DutyCycleEncoder(2); @@ -83,9 +83,17 @@ private IntakeSubsystem() { } - public void setPosition(Rotation2d position){ - DesiredPosition = position; - pivotPID.setReference(DesiredPosition.getDegrees(), ControlType.kSmartMotion); + public void setPosition(IntakeConstants.Position position){ + desiredPosition = position.getPosition(); + pivotPID.setReference(desiredPosition.getDegrees(), ControlType.kSmartMotion); + } + + public boolean atPosition() { + double currPos = pivotEncoder.getPosition(); + double targetPos = desiredPosition.getDegrees(); + double error = Math.abs(targetPos - currPos); + + return error < IntakeConstants.PivotConstants.SmartErr; } // public Command Shoot() { @@ -161,10 +169,11 @@ public double getDiffedEncoder(){ @Override public void periodic() { SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); - SmartDashboard.putNumber("DesiredDeg", DesiredPosition.getDegrees()); - SmartDashboard.putNumber("DesiredRot", DesiredPosition.getRotations()); + SmartDashboard.putNumber("DesiredDeg", desiredPosition.getDegrees()); + SmartDashboard.putNumber("DesiredRot", desiredPosition.getRotations()); SmartDashboard.putNumber("diffedEncoder", getDiffedEncoder()); SmartDashboard.putNumber("PivotAbsEncoder", pivotAbsEncoder.get()); SmartDashboard.putNumber("wheelsAbsEncoder", wheelAbsEncoder.get()); } + } diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java index 97b0701..3699258 100644 --- a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -75,8 +75,8 @@ private ShooterSubsystem() { leftShooterMotor.setInverted(false); rightShooterMotor.setInverted(true);*/ - leftShooterMotor = new CANSparkMax(Constants.ShooterConstants.ShooterLeftID, CANSparkMax.MotorType.kBrushless); - rightShooterMotor = new CANSparkMax(Constants.ShooterConstants.ShooterRightID, CANSparkMax.MotorType.kBrushless); + leftShooterMotor = new CANSparkMax(Constants.ShooterConstants.shooterLeftID, CANSparkMax.MotorType.kBrushless); + rightShooterMotor = new CANSparkMax(Constants.ShooterConstants.shooterRightID, CANSparkMax.MotorType.kBrushless); leftShooterMotor.restoreFactoryDefaults(); rightShooterMotor.restoreFactoryDefaults(); From 0c09c7431176ad0245a12f84b2acc6389688c1c4 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sat, 9 Mar 2024 10:09:27 -0500 Subject: [PATCH 21/31] Paths --- src/main/deploy/pathplanner/autos/test.auto | 37 ++++++++++ .../pathplanner/paths/Mid to center top.path | 68 +++++++++++++++++++ .../deploy/pathplanner/paths/New Path.path | 68 +++++++++++++++++++ src/main/deploy/pathplanner/paths/test.path | 52 ++++++++++++++ 4 files changed, 225 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/test.auto create mode 100644 src/main/deploy/pathplanner/paths/Mid to center top.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/test.path diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto new file mode 100644 index 0000000..d6391ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "test" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid to center top.path b/src/main/deploy/pathplanner/paths/Mid to center top.path new file mode 100644 index 0000000..13ace3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid to center top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.309243119380694, + "y": 5.492445188825977 + }, + "prevControl": null, + "nextControl": { + "x": 1.852613365334066, + "y": 6.35890044588676 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.909982492594682, + "y": 6.256100669625311 + }, + "prevControl": { + "x": 2.293183835025989, + "y": 6.226729304979181 + }, + "nextControl": { + "x": 4.294247534437606, + "y": 6.322018052570215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.844371753144222, + "y": 7.401583890824312 + }, + "prevControl": { + "x": 8.344371753144216, + "y": 7.401583890824312 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..f645916 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "prevControl": null, + "nextControl": { + "x": 1.8539869673657114, + "y": 4.508504473180682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1022699648261556, + "y": 1.4979395969525402 + }, + "prevControl": { + "x": 1.2459786562758262, + "y": 2.0144327671892466 + }, + "nextControl": { + "x": 3.0274679511791946, + "y": 0.939883668676104 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3010015071908505, + "y": 0.7783411631223989 + }, + "prevControl": { + "x": 7.801001507190851, + "y": 0.7783411631223989 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.2810957359708, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/test.path b/src/main/deploy/pathplanner/paths/test.path new file mode 100644 index 0000000..6b20dac --- /dev/null +++ b/src/main/deploy/pathplanner/paths/test.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "prevControl": null, + "nextControl": { + "x": 1.8379276830110018, + "y": 5.551187918118234 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7631256693640407, + "y": 5.513288844759509 + }, + "prevControl": { + "x": 2.395250483557632, + "y": 5.513288844759509 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From e4fe619b1fc713ab914c668c0983f81a5890323f Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sat, 9 Mar 2024 11:37:53 -0500 Subject: [PATCH 22/31] Paths --- src/main/deploy/pathplanner/autos/Amp.auto | 37 ++++++++++ .../pathplanner/autos/BotToCentBot.auto | 37 ++++++++++ .../pathplanner/autos/MidToCentTop.auto | 37 ++++++++++ .../pathplanner/autos/TopToCentTop.auto | 37 ++++++++++ src/main/deploy/pathplanner/autos/safe.auto | 37 ++++++++++ src/main/deploy/pathplanner/paths/Amp.path | 74 +++++++++++++++++++ .../{New Path.path => Bot to mid bot.path} | 8 +- .../pathplanner/paths/Mid to center top.path | 8 +- .../deploy/pathplanner/paths/Safe Auto.path | 68 +++++++++++++++++ .../pathplanner/paths/top to center top.path | 68 +++++++++++++++++ .../frc/team7520/robot/RobotContainer.java | 21 +++++- .../robot/commands/AbsoluteDrive.java | 20 +++-- 12 files changed, 437 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Amp.auto create mode 100644 src/main/deploy/pathplanner/autos/BotToCentBot.auto create mode 100644 src/main/deploy/pathplanner/autos/MidToCentTop.auto create mode 100644 src/main/deploy/pathplanner/autos/TopToCentTop.auto create mode 100644 src/main/deploy/pathplanner/autos/safe.auto create mode 100644 src/main/deploy/pathplanner/paths/Amp.path rename src/main/deploy/pathplanner/paths/{New Path.path => Bot to mid bot.path} (90%) create mode 100644 src/main/deploy/pathplanner/paths/Safe Auto.path create mode 100644 src/main/deploy/pathplanner/paths/top to center top.path diff --git a/src/main/deploy/pathplanner/autos/Amp.auto b/src/main/deploy/pathplanner/autos/Amp.auto new file mode 100644 index 0000000..5d6cd4d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Amp.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.82409310273166, + "y": 7.696215392430785 + }, + "rotation": -90.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Amp" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BotToCentBot.auto b/src/main/deploy/pathplanner/autos/BotToCentBot.auto new file mode 100644 index 0000000..ea12cb3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BotToCentBot.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "rotation": -58.751279204501785 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Bot to mid bot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidToCentTop.auto b/src/main/deploy/pathplanner/autos/MidToCentTop.auto new file mode 100644 index 0000000..6801329 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidToCentTop.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.309243119380694, + "y": 5.492445188825977 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid to center top" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopToCentTop.auto b/src/main/deploy/pathplanner/autos/TopToCentTop.auto new file mode 100644 index 0000000..931d7ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopToCentTop.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7606160666866788, + "y": 6.6047521185951465 + }, + "rotation": 59.381394591090626 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "top to center top" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/safe.auto b/src/main/deploy/pathplanner/autos/safe.auto new file mode 100644 index 0000000..3b6e6ef --- /dev/null +++ b/src/main/deploy/pathplanner/autos/safe.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "rotation": -59.34933204294715 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Safe Auto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path new file mode 100644 index 0000000..82eb88b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.82409310273166, + "y": 7.696215392430785 + }, + "prevControl": null, + "nextControl": { + "x": 2.8968988847068604, + "y": 7.546955457547279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9155563765672983, + "y": 7.5749416953379365 + }, + "prevControl": { + "x": 2.432429726634338, + "y": 7.638826211031551 + }, + "nextControl": { + "x": 4.044334634123814, + "y": 7.42568176045443 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572424781213199, + "y": 7.388366776733554 + }, + "prevControl": { + "x": 7.072424781213199, + "y": 7.388366776733554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Bot to mid bot.path similarity index 90% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/Bot to mid bot.path index f645916..cf2af2d 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/Bot to mid bot.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.3010015071908505, - "y": 0.7783411631223989 + "x": 6.497794813771446, + "y": 0.7462996744175311 }, "prevControl": { - "x": 7.801001507190851, - "y": 0.7783411631223989 + "x": 6.9977948137714465, + "y": 0.7462996744175311 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Mid to center top.path b/src/main/deploy/pathplanner/paths/Mid to center top.path index 13ace3f..39fbaea 100644 --- a/src/main/deploy/pathplanner/paths/Mid to center top.path +++ b/src/main/deploy/pathplanner/paths/Mid to center top.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.844371753144222, - "y": 7.401583890824312 + "x": 6.609739764934075, + "y": 7.360380538942897 }, "prevControl": { - "x": 8.344371753144216, - "y": 7.401583890824312 + "x": 7.109739764934075, + "y": 7.360380538942897 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Safe Auto.path b/src/main/deploy/pathplanner/paths/Safe Auto.path new file mode 100644 index 0000000..05448d8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Safe Auto.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "prevControl": null, + "nextControl": { + "x": 0.7979310504075553, + "y": 1.9776941372064571 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0013392754058237, + "y": 1.4179693813933087 + }, + "prevControl": { + "x": 1.1450479668554943, + "y": 1.9344625516300151 + }, + "nextControl": { + "x": 2.9265372617588623, + "y": 0.8599134531168725 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.2029233149375385, + "y": 1.1754219872076113 + }, + "prevControl": { + "x": 4.702923314937539, + "y": 1.1754219872076113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.2810957359708, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top to center top.path b/src/main/deploy/pathplanner/paths/top to center top.path new file mode 100644 index 0000000..fa44624 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top to center top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7606160666866788, + "y": 6.6047521185951465 + }, + "prevControl": null, + "nextControl": { + "x": 1.3039863126400504, + "y": 7.471207375655929 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8782413928464217, + "y": 6.259588519177039 + }, + "prevControl": { + "x": 2.261442735277729, + "y": 6.2302171545309095 + }, + "nextControl": { + "x": 4.262506434689346, + "y": 6.325505902121943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.441822338190131, + "y": 7.201791858129171 + }, + "prevControl": { + "x": 6.941822338190131, + "y": 7.201791858129171 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 59.23728046576105, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 6a712c0..50bbd50 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -55,6 +56,8 @@ public class RobotContainer private final ClimberSubsystem climberSubsystem = ClimberSubsystem.getInstance(); private final LED LEDSubsystem = LED.getInstance(); + public final SendableChooser autoChooser = new SendableChooser<>(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); @@ -72,6 +75,8 @@ public class RobotContainer ); + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -95,7 +100,8 @@ public RobotContainer() () -> -driverController.getRightX(), () -> -driverController.getRightY(), driverController::getRightBumper, - driverController::getLeftBumper + driverController::getLeftBumper, + () -> false ); Shooter shooter = new Shooter(shooterSubsystem, @@ -141,6 +147,17 @@ public RobotContainer() candle.animate(LEDSubsystem.idleAnimation); } + private void registerAutos(){ + + autoChooser.setDefaultOption("Safe auto", drivebase.getPPAutoCommand("safe", true)); + autoChooser.addOption("Amp", drivebase.getPPAutoCommand("Amp", true)); + autoChooser.addOption("BotToCentBot", drivebase.getPPAutoCommand("BotToCentBot", true)); + autoChooser.addOption("MidToCentTop", drivebase.getPPAutoCommand("MidToCentTop", true)); + autoChooser.addOption("TopToCentTop", drivebase.getPPAutoCommand("TopToCentTop", true)); + + + } + /** * Use this method to define named commands for use in {@link PathPlannerAuto} * @@ -152,6 +169,8 @@ private void registerNamedCommands() NamedCommands.registerCommand("log", new InstantCommand(() -> System.out.println("eeeeeeeeeeeeeeeeeeeeeeeee"))); } + + /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index 34bf9b2..b659111 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -26,7 +26,7 @@ public class AbsoluteDrive extends Command { private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingHorizontal, headingVertical; - private final BooleanSupplier CCWSpin, CWSpin; + private final BooleanSupplier CCWSpin, CWSpin, speedCutoffSup; private boolean initRotation = false; /** @@ -50,7 +50,7 @@ public class AbsoluteDrive extends Command { * with no deadband. Positive is away from the alliance wall. */ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical) { + DoubleSupplier headingVertical, BooleanSupplier speedCutoffSup) { this.swerve = swerve; this.vX = vX; this.vY = vY; @@ -58,12 +58,13 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v this.headingVertical = headingVertical; this.CCWSpin = () -> false; this.CWSpin = () -> false; + this.speedCutoffSup = speedCutoffSup; addRequirements(swerve); } public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical, BooleanSupplier CWSpin, BooleanSupplier CCWSpin) { + DoubleSupplier headingVertical, BooleanSupplier CWSpin, BooleanSupplier CCWSpin, BooleanSupplier speedCutoffSup) { this.swerve = swerve; this.vX = vX; this.vY = vY; @@ -71,6 +72,7 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v this.headingVertical = headingVertical; this.CCWSpin = CCWSpin; this.CWSpin = CWSpin; + this.speedCutoffSup = speedCutoffSup; addRequirements(swerve); } @@ -85,15 +87,20 @@ public void initialize() { @Override public void execute() { + Boolean speedCutoff = speedCutoffSup.getAsBoolean(); + + double vXspeed = vX.getAsDouble() * (speedCutoffSup.getAsBoolean() ? 1 : 1); + double vYspeed = vY.getAsDouble() * (speedCutoffSup.getAsBoolean() ? 1 : 1); + ChassisSpeeds desiredSpeeds; if (CWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().minus(Rotation2d.fromDegrees(20))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(20))); } else if (CCWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().plus(Rotation2d.fromDegrees(20))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(20))); } else { // Get the desired chassis speeds based on a 2 joystick module. - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, headingHorizontal.getAsDouble(), headingVertical.getAsDouble()); } @@ -122,6 +129,7 @@ public void execute() { // Make the robot move swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } // Called once the command ends or is interrupted. From 0ef2c3fde311232fcaea0c575ea1b3f638d3c71e Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Sat, 9 Mar 2024 20:06:51 -0500 Subject: [PATCH 23/31] Auto --- src/main/java/frc/team7520/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 50bbd50..4829e10 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -211,6 +211,6 @@ private void configureBindings() */ public Command getAutonomousCommand() { - return drivebase.getPPAutoCommand("test", true); + return new ShootSequence(); } } From 2b0a9091806c3b21e616262faf03db654fee1b04 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Mon, 11 Mar 2024 14:28:42 -0400 Subject: [PATCH 24/31] odometry reflects real life it was kinimatics comtrols are inverted tho --- src/main/deploy/swerve/neo/modules/backleft.json | 4 ++-- src/main/deploy/swerve/neo/modules/backright.json | 4 ++-- src/main/deploy/swerve/neo/modules/frontleft.json | 4 ++-- src/main/deploy/swerve/neo/modules/frontright.json | 4 ++-- src/main/deploy/swerve/neo/swervedrive.json | 2 +- src/main/java/frc/team7520/robot/Constants.java | 2 +- src/main/java/frc/team7520/robot/RobotContainer.java | 4 ++-- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index ceb0e38..efc25ce 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 179.12109375, + "absoluteEncoderOffset": 331.171875, "location": { "front": -13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index ea26059..d266fed 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 59.4140625, + "absoluteEncoderOffset": 146.513671875, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index 83a20b5..fe951de 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 148.0078125, + "absoluteEncoderOffset": 58.095703125, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 261ce9d..32736e8 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 331.259765625, + "absoluteEncoderOffset": 178.59375, "location": { "front": 13, "left": -13 diff --git a/src/main/deploy/swerve/neo/swervedrive.json b/src/main/deploy/swerve/neo/swervedrive.json index 5fb01c6..3f9d2c2 100644 --- a/src/main/deploy/swerve/neo/swervedrive.json +++ b/src/main/deploy/swerve/neo/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": false, + "invertedIMU": true, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 58f41b4..364aeec 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -21,7 +21,7 @@ */ public final class Constants { - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final double ROBOT_MASS = 49.8952; // Mass in kilos public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 4829e10..5f246c5 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -97,8 +97,8 @@ public RobotContainer() OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> -driverController.getRightX(), - () -> -driverController.getRightY(), + () -> driverController.getRightX(), + () -> driverController.getRightY(), driverController::getRightBumper, driverController::getLeftBumper, () -> false From 615f9f489d2a99f701f80a4831d91302b40387ff Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Tue, 12 Mar 2024 13:47:36 -0400 Subject: [PATCH 25/31] Auto working with a 2note --- src/main/deploy/pathplanner/autos/2Note.auto | 74 +++++++++++++++++++ src/main/deploy/pathplanner/paths/test.path | 14 ++-- src/main/deploy/pathplanner/paths/test2.path | 52 +++++++++++++ src/main/deploy/swerve/neo/swervedrive.json | 2 +- .../java/frc/team7520/robot/Constants.java | 6 +- .../frc/team7520/robot/RobotContainer.java | 26 +++++-- .../robot/commands/AbsoluteDrive.java | 4 +- 7 files changed, 159 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/2Note.auto create mode 100644 src/main/deploy/pathplanner/paths/test2.path diff --git a/src/main/deploy/pathplanner/autos/2Note.auto b/src/main/deploy/pathplanner/autos/2Note.auto new file mode 100644 index 0000000..b04d223 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2Note.auto @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "test" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "test2" + } + }, + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/test.path b/src/main/deploy/pathplanner/paths/test.path index 6b20dac..375443a 100644 --- a/src/main/deploy/pathplanner/paths/test.path +++ b/src/main/deploy/pathplanner/paths/test.path @@ -8,19 +8,19 @@ }, "prevControl": null, "nextControl": { - "x": 1.8379276830110018, - "y": 5.551187918118234 + "x": 1.843486565211486, + "y": 5.513288844759509 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.7631256693640407, + "x": 2.29, "y": 5.513288844759509 }, "prevControl": { - "x": 2.395250483557632, + "x": 1.9221248141935914, "y": 5.513288844759509 }, "nextControl": null, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -46,7 +46,7 @@ "folder": null, "previewStartingState": { "rotation": 0.0, - "velocity": 0 + "velocity": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/test2.path new file mode 100644 index 0000000..a976ddd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/test2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.327845382963493, + "y": 5.541275082550166 + }, + "prevControl": null, + "nextControl": { + "x": 1.3278453829634929, + "y": 5.541275082550166 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2363821091278546, + "y": 5.541275082550166 + }, + "prevControl": { + "x": 2.178585448079987, + "y": 5.550603828480384 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/swervedrive.json b/src/main/deploy/swerve/neo/swervedrive.json index 3f9d2c2..5fb01c6 100644 --- a/src/main/deploy/swerve/neo/swervedrive.json +++ b/src/main/deploy/swerve/neo/swervedrive.json @@ -4,7 +4,7 @@ "id": 0, "canbus": null }, - "invertedIMU": true, + "invertedIMU": false, "modules": [ "frontleft.json", "frontright.json", diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 364aeec..7149ba5 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -65,7 +65,7 @@ public static class Telemetry { public static class IntakeConstants { public enum Position { SHOOT(new Rotation2d(0), 1), - INTAKE(new Rotation2d(Units.degreesToRadians(211.374d)), 0.35), + INTAKE(new Rotation2d(Units.degreesToRadians(212.152317734808d)), -0.35), AMP(new Rotation2d(Units.degreesToRadians(90)), 0.525); private final Rotation2d position; @@ -104,9 +104,9 @@ public static class PivotConstants { public static final double OUTPUT_MAX = 1; public static final double OUTPUT_MIN = -1; - public static final double SmartMaxVel = 20000; + public static final double SmartMaxVel = 30000; public static final double SmartMinVel = 0; - public static final double SmartAccel = 2000; + public static final double SmartAccel = 3000; public static final double SmartErr = 2; public static final int SlotID = 0; } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 5f246c5..3f93d8b 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -12,14 +12,18 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.team7520.robot.Constants.IntakeConstants; +import frc.team7520.robot.Constants.IntakeConstants.Position; import frc.team7520.robot.Constants.OperatorConstants; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team7520.robot.auto.AutoIntake; import frc.team7520.robot.auto.ShootSequence; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.Climber; @@ -81,7 +85,7 @@ public class RobotContainer public RobotContainer() { - registerNamedCommands(); + registerAutos(); CameraServer.startAutomaticCapture(); @@ -93,9 +97,9 @@ public RobotContainer() // Applies deadbands and inverts controls because joysticks // are back-right positive while robot // controls are front-left positive - () -> MathUtil.applyDeadband(driverController.getLeftY(), + () -> MathUtil.applyDeadband(-driverController.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), - () -> MathUtil.applyDeadband(driverController.getLeftX(), + () -> MathUtil.applyDeadband(-driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverController.getRightX(), () -> driverController.getRightY(), @@ -149,13 +153,18 @@ public RobotContainer() private void registerAutos(){ + registerNamedCommands(); + autoChooser.setDefaultOption("Safe auto", drivebase.getPPAutoCommand("safe", true)); autoChooser.addOption("Amp", drivebase.getPPAutoCommand("Amp", true)); + autoChooser.addOption("Test", drivebase.getPPAutoCommand("test", true)); autoChooser.addOption("BotToCentBot", drivebase.getPPAutoCommand("BotToCentBot", true)); autoChooser.addOption("MidToCentTop", drivebase.getPPAutoCommand("MidToCentTop", true)); autoChooser.addOption("TopToCentTop", drivebase.getPPAutoCommand("TopToCentTop", true)); + autoChooser.addOption("2Note", drivebase.getPPAutoCommand("2Note", true)); + SmartDashboard.putData(autoChooser); } /** @@ -167,6 +176,11 @@ private void registerNamedCommands() // Example NamedCommands.registerCommand("shoot", new ShootSequence()); NamedCommands.registerCommand("log", new InstantCommand(() -> System.out.println("eeeeeeeeeeeeeeeeeeeeeeeee"))); + NamedCommands.registerCommand("intakeOut", new AutoIntake(Position.INTAKE)); + NamedCommands.registerCommand("intake", new InstantCommand(() -> intakeSubsystem.setSpeed(Position.INTAKE.getSpeed()))); + NamedCommands.registerCommand("stopIntaking", new InstantCommand(() -> intakeSubsystem.setSpeed(0))); + NamedCommands.registerCommand("intakeIn", new AutoIntake(Position.SHOOT)); + } @@ -189,12 +203,12 @@ private void configureBindings() new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); - new Trigger(() -> intake.currPosition == Constants.IntakeConstants.Position.INTAKE) + new Trigger(() -> intake.currPosition == Position.INTAKE) .and(new JoystickButton(operatorController, XboxController.Button.kRightBumper.value)) .onTrue(new RepeatCommand(LEDSubsystem.intaking())) .onFalse(LEDSubsystem.idle()); - new Trigger(() -> intake.currPosition == Constants.IntakeConstants.Position.INTAKE) + new Trigger(() -> intake.currPosition == Position.INTAKE) .and(new JoystickButton(operatorController, XboxController.Button.kX.value)) .whileTrue(new RepeatCommand(LEDSubsystem.intaking())) .onFalse(LEDSubsystem.idle()); @@ -211,6 +225,6 @@ private void configureBindings() */ public Command getAutonomousCommand() { - return new ShootSequence(); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index b659111..7b6b9ea 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -95,9 +95,9 @@ public void execute() { ChassisSpeeds desiredSpeeds; if (CWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(20))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(90))); } else if (CCWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(20))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(90))); } else { // Get the desired chassis speeds based on a 2 joystick module. desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, From c7336ea1dbea95163d11bdd401219584bc0289cc Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Wed, 13 Mar 2024 13:42:53 -0400 Subject: [PATCH 26/31] working --- .pathplanner/settings.json | 4 +- .../autos/{2Note.auto => 2NoteMid.auto} | 4 +- .../deploy/pathplanner/autos/3NoteMid.auto | 123 ++++++++++++++++++ src/main/deploy/pathplanner/autos/test.auto | 2 +- src/main/deploy/pathplanner/paths/Amp.path | 4 +- .../pathplanner/paths/Bot to mid bot.path | 4 +- .../{test2.path => Close Mid to Mid.path} | 22 ++-- .../pathplanner/paths/Close Top To Mid.path | 68 ++++++++++ .../{test.path => Mid To Close Mid.path} | 22 ++-- .../pathplanner/paths/Mid To Close Top.path | 68 ++++++++++ .../pathplanner/paths/Mid to center top.path | 4 +- .../deploy/pathplanner/paths/Safe Auto.path | 4 +- .../pathplanner/paths/top to center top.path | 4 +- .../java/frc/team7520/robot/Constants.java | 4 +- .../frc/team7520/robot/RobotContainer.java | 21 ++- .../team7520/robot/auto/ShootSequence.java | 2 +- .../robot/commands/AbsoluteDrive.java | 3 + .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../subsystems/swerve/SwerveSubsystem.java | 9 +- 19 files changed, 325 insertions(+), 49 deletions(-) rename src/main/deploy/pathplanner/autos/{2Note.auto => 2NoteMid.auto} (93%) create mode 100644 src/main/deploy/pathplanner/autos/3NoteMid.auto rename src/main/deploy/pathplanner/paths/{test2.path => Close Mid to Mid.path} (67%) create mode 100644 src/main/deploy/pathplanner/paths/Close Top To Mid.path rename src/main/deploy/pathplanner/paths/{test.path => Mid To Close Mid.path} (68%) create mode 100644 src/main/deploy/pathplanner/paths/Mid To Close Top.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index c43b5ce..406c9a6 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.6, + "defaultMaxAccel": 4.6, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.6 diff --git a/src/main/deploy/pathplanner/autos/2Note.auto b/src/main/deploy/pathplanner/autos/2NoteMid.auto similarity index 93% rename from src/main/deploy/pathplanner/autos/2Note.auto rename to src/main/deploy/pathplanner/autos/2NoteMid.auto index b04d223..63c3f4f 100644 --- a/src/main/deploy/pathplanner/autos/2Note.auto +++ b/src/main/deploy/pathplanner/autos/2NoteMid.auto @@ -30,7 +30,7 @@ { "type": "path", "data": { - "pathName": "test" + "pathName": "Mid To Close Mid" } }, { @@ -45,7 +45,7 @@ { "type": "path", "data": { - "pathName": "test2" + "pathName": "Close Mid to Mid" } }, { diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.auto b/src/main/deploy/pathplanner/autos/3NoteMid.auto new file mode 100644 index 0000000..8e3703e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteMid.auto @@ -0,0 +1,123 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Top" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Close Top To Mid" + } + }, + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto index d6391ea..c59ff46 100644 --- a/src/main/deploy/pathplanner/autos/test.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -26,7 +26,7 @@ { "type": "path", "data": { - "pathName": "test" + "pathName": "Mid To Close Mid" } } ] diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path index 82eb88b..42f216c 100644 --- a/src/main/deploy/pathplanner/paths/Amp.path +++ b/src/main/deploy/pathplanner/paths/Amp.path @@ -54,8 +54,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Bot to mid bot.path b/src/main/deploy/pathplanner/paths/Bot to mid bot.path index cf2af2d..5597b8a 100644 --- a/src/main/deploy/pathplanner/paths/Bot to mid bot.path +++ b/src/main/deploy/pathplanner/paths/Bot to mid bot.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/test2.path b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path similarity index 67% rename from src/main/deploy/pathplanner/paths/test2.path rename to src/main/deploy/pathplanner/paths/Close Mid to Mid.path index a976ddd..cc32a88 100644 --- a/src/main/deploy/pathplanner/paths/test2.path +++ b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.327845382963493, - "y": 5.541275082550166 + "x": 2.66, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.3278453829634929, - "y": 5.541275082550166 + "x": 1.6600000000000001, + "y": 5.55 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2363821091278546, - "y": 5.541275082550166 + "x": 1.329669568430046, + "y": 5.55 }, "prevControl": { - "x": 2.178585448079987, - "y": 5.550603828480384 + "x": 2.2719190881609803, + "y": 5.55 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Close Top To Mid.path b/src/main/deploy/pathplanner/paths/Close Top To Mid.path new file mode 100644 index 0000000..6c4579f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Close Top To Mid.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4864340637772187, + "y": 6.991800676923469 + }, + "prevControl": null, + "nextControl": { + "x": 2.3864340637772186, + "y": 6.991800676923469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.123258272103077, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 2.303258272103077, + "y": 6.991800676923469 + }, + "nextControl": { + "x": 1.1232582721030768, + "y": 6.991800676923469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2270533631976355, + "y": 5.569261320340823 + }, + "prevControl": { + "x": 2.2270533631976352, + "y": 5.569261320340823 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/test.path b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path similarity index 68% rename from src/main/deploy/pathplanner/paths/test.path rename to src/main/deploy/pathplanner/paths/Mid To Close Mid.path index 375443a..e0f6f13 100644 --- a/src/main/deploy/pathplanner/paths/test.path +++ b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3110120765696076, - "y": 5.513288844759509 + "x": 1.28, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.843486565211486, - "y": 5.513288844759509 + "x": 1.8124744886418784, + "y": 5.55 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.29, - "y": 5.513288844759509 + "x": 2.5868974814872714, + "y": 5.55 }, "prevControl": { - "x": 1.9221248141935914, - "y": 5.513288844759509 + "x": 2.2190222956808627, + "y": 5.55 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "rotation": 0.0, "velocity": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid To Close Top.path b/src/main/deploy/pathplanner/paths/Mid To Close Top.path new file mode 100644 index 0000000..924478d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid To Close Top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.289171939085591, + "y": 5.546051033026494 + }, + "prevControl": null, + "nextControl": { + "x": 2.598379014939848, + "y": 5.690535017433672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.82409310273166, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 1.205710842450273, + "y": 6.80503451422615 + }, + "nextControl": { + "x": 1.8707368323827558, + "y": 7.0058881935945685 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5124985608445702, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 2.41249856084457, + "y": 6.991800676923469 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 4.6, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid to center top.path b/src/main/deploy/pathplanner/paths/Mid to center top.path index 39fbaea..fd806d9 100644 --- a/src/main/deploy/pathplanner/paths/Mid to center top.path +++ b/src/main/deploy/pathplanner/paths/Mid to center top.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Safe Auto.path b/src/main/deploy/pathplanner/paths/Safe Auto.path index 05448d8..3d7a72c 100644 --- a/src/main/deploy/pathplanner/paths/Safe Auto.path +++ b/src/main/deploy/pathplanner/paths/Safe Auto.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/top to center top.path b/src/main/deploy/pathplanner/paths/top to center top.path index fa44624..e18e9df 100644 --- a/src/main/deploy/pathplanner/paths/top to center top.path +++ b/src/main/deploy/pathplanner/paths/top to center top.path @@ -48,8 +48,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 4.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 7149ba5..43eebc8 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -104,9 +104,9 @@ public static class PivotConstants { public static final double OUTPUT_MAX = 1; public static final double OUTPUT_MIN = -1; - public static final double SmartMaxVel = 30000; + public static final double SmartMaxVel = 600000; public static final double SmartMinVel = 0; - public static final double SmartAccel = 3000; + public static final double SmartAccel = 10000; public static final double SmartErr = 2; public static final int SlotID = 0; } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 3f93d8b..68e6bc1 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -13,17 +13,15 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team7520.robot.Constants.IntakeConstants; import frc.team7520.robot.Constants.IntakeConstants.Position; import frc.team7520.robot.Constants.OperatorConstants; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.team7520.robot.auto.AutoIntake; +import frc.team7520.robot.auto.AutoShoot; import frc.team7520.robot.auto.ShootSequence; import frc.team7520.robot.commands.AbsoluteDrive; import frc.team7520.robot.commands.Climber; @@ -78,6 +76,8 @@ public class RobotContainer intakeSubsystem::getSwitchVal ); + private Shooter shooter; + @@ -108,7 +108,7 @@ public RobotContainer() () -> false ); - Shooter shooter = new Shooter(shooterSubsystem, + shooter = new Shooter(shooterSubsystem, operatorController::getLeftTriggerAxis, operatorController::getLeftBumper ); @@ -161,7 +161,8 @@ private void registerAutos(){ autoChooser.addOption("BotToCentBot", drivebase.getPPAutoCommand("BotToCentBot", true)); autoChooser.addOption("MidToCentTop", drivebase.getPPAutoCommand("MidToCentTop", true)); autoChooser.addOption("TopToCentTop", drivebase.getPPAutoCommand("TopToCentTop", true)); - autoChooser.addOption("2Note", drivebase.getPPAutoCommand("2Note", true)); + autoChooser.addOption("2Note", drivebase.getPPAutoCommand("2NoteMid", true)); + autoChooser.addOption("3NoteMid", drivebase.getPPAutoCommand("3NoteMid", true)); SmartDashboard.putData(autoChooser); @@ -225,6 +226,12 @@ private void configureBindings() */ public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return new SequentialCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> shooterSubsystem.setDefaultCommand(new AutoShoot(1, false))), + autoChooser.getSelected() + ), + new InstantCommand(() -> shooterSubsystem.setDefaultCommand(shooter)) + ); } } diff --git a/src/main/java/frc/team7520/robot/auto/ShootSequence.java b/src/main/java/frc/team7520/robot/auto/ShootSequence.java index 7d77a81..61ec821 100644 --- a/src/main/java/frc/team7520/robot/auto/ShootSequence.java +++ b/src/main/java/frc/team7520/robot/auto/ShootSequence.java @@ -17,7 +17,7 @@ public ShootSequence() { new AutoIntake(Constants.IntakeConstants.Position.SHOOT), new ParallelRaceGroup( new AutoShoot(1, false), - new WaitCommand(1.5) + new WaitCommand(1) ) ), new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(1)), diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index 7b6b9ea..7cd8998 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -81,6 +81,7 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v @Override public void initialize() { initRotation = true; + SmartDashboard.putBoolean("initRotation", initRotation); } // Called every time the scheduler runs while the command is scheduled. @@ -113,6 +114,8 @@ public void execute() { // Set the Current Heading to the desired Heading desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + + SmartDashboard.putBoolean("initRotation", initRotation); } //Dont Init Rotation Again initRotation = false; diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java index 3699258..73b2127 100644 --- a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -24,7 +24,7 @@ public class ShooterSubsystem extends SubsystemBase { private RelativeEncoder leftShooterEncoder; private RelativeEncoder rightShooterEncoder; - private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(5); + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(100); /** * The Singleton instance of this shooterSubsystem. Code should use diff --git a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java index 0e88d75..f120c91 100644 --- a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java @@ -12,6 +12,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -19,6 +20,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.team7520.robot.Constants; @@ -142,7 +144,8 @@ public Command getPathCommand(String pathName, boolean setOdomToStart) { */ public Command getPPAutoCommand(String autoName, boolean setOdomToStart) { if (setOdomToStart) { - resetOdometry(PathPlannerAuto.getStaringPoseFromAutoFile(autoName)); + SmartDashboard.putNumber("HeadingFromFile", -1); +// resetOdometry(PathPlannerAuto.getStaringPoseFromAutoFile(autoName)); } return new PathPlannerAuto(autoName); } @@ -223,6 +226,10 @@ public SwerveDriveKinematics getKinematics() { * @param initialHolonomicPose The pose to set the odometry to */ public void resetOdometry(Pose2d initialHolonomicPose) { +// SmartDashboard.putNumber("ResetHeading", initialHolonomicPose.getRotation().getDegrees()); + + swerveDrive.setGyro(new Rotation3d(0, 0, initialHolonomicPose.getRotation().getRadians())); + swerveDrive.resetOdometry(initialHolonomicPose); } From e5123263315c532b6f35094faa91b384d1b5b224 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Wed, 13 Mar 2024 16:05:23 -0400 Subject: [PATCH 27/31] 3Note auto consistent --- .../deploy/pathplanner/autos/3NoteMid.auto | 22 ++++++++++++------- .../frc/team7520/robot/RobotContainer.java | 2 +- .../team7520/robot/auto/ShootSequence.java | 6 ++--- .../subsystems/intake/IntakeSubsystem.java | 2 +- 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.auto b/src/main/deploy/pathplanner/autos/3NoteMid.auto index 8e3703e..1fdaed4 100644 --- a/src/main/deploy/pathplanner/autos/3NoteMid.auto +++ b/src/main/deploy/pathplanner/autos/3NoteMid.auto @@ -43,15 +43,15 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Close Mid to Mid" + "name": "intakeIn" } }, { - "type": "named", + "type": "path", "data": { - "name": "intakeIn" + "pathName": "Close Mid to Mid" } }, { @@ -92,15 +92,15 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Close Top To Mid" + "name": "intakeIn" } }, { - "type": "named", + "type": "path", "data": { - "name": "intakeIn" + "pathName": "Close Top To Mid" } }, { @@ -114,6 +114,12 @@ "data": { "name": "shoot" } + }, + { + "type": "parallel", + "data": { + "commands": [] + } } ] } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 68e6bc1..ad34c37 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -228,7 +228,7 @@ public Command getAutonomousCommand() { return new SequentialCommandGroup( new ParallelCommandGroup( - new InstantCommand(() -> shooterSubsystem.setDefaultCommand(new AutoShoot(1, false))), + new InstantCommand(() -> shooterSubsystem.setDefaultCommand(new AutoShoot(0.7, false))), autoChooser.getSelected() ), new InstantCommand(() -> shooterSubsystem.setDefaultCommand(shooter)) diff --git a/src/main/java/frc/team7520/robot/auto/ShootSequence.java b/src/main/java/frc/team7520/robot/auto/ShootSequence.java index 61ec821..d3be198 100644 --- a/src/main/java/frc/team7520/robot/auto/ShootSequence.java +++ b/src/main/java/frc/team7520/robot/auto/ShootSequence.java @@ -17,13 +17,13 @@ public ShootSequence() { new AutoIntake(Constants.IntakeConstants.Position.SHOOT), new ParallelRaceGroup( new AutoShoot(1, false), - new WaitCommand(1) + new WaitCommand(0.1) ) ), new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(1)), new WaitCommand(0.5), - new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(0)), - new InstantCommand(() -> ShooterSubsystem.getInstance().setSpeed(0, false)) + new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(0)) +// new InstantCommand(() -> ShooterSubsystem.getInstance().setSpeed(0, false)) ); } } diff --git a/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java index 3a8a01e..01b4131 100644 --- a/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java @@ -30,7 +30,7 @@ public class IntakeSubsystem extends SubsystemBase { private final DigitalInput input = new DigitalInput(0); - private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(0.5); + private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(5); public Rotation2d desiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); From 54dff956138931c0cb3dcaa3e3629252a280663a Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:00:35 -0400 Subject: [PATCH 28/31] 3Note bath sides --- .pathplanner/settings.json | 2 +- .../{3NoteMid.auto => 3NoteMid.Note1.auto} | 102 +++++--- .../pathplanner/autos/3NoteMid.Note3.auto | 163 +++++++++++++ src/main/deploy/pathplanner/autos/4Note.auto | 226 ++++++++++++++++++ src/main/deploy/pathplanner/paths/Amp.path | 2 +- .../pathplanner/paths/Bot to mid bot.path | 2 +- .../pathplanner/paths/Close Mid to Mid.path | 8 +- .../pathplanner/paths/Close Top To Mid.path | 20 +- .../pathplanner/paths/Mid To Close Mid.path | 2 +- .../pathplanner/paths/Mid To Close Top.path | 8 +- .../pathplanner/paths/Mid to center top.path | 2 +- .../pathplanner/paths/Note3.SpeakerC.path | 68 ++++++ .../deploy/pathplanner/paths/Safe Auto.path | 2 +- .../pathplanner/paths/SpeakerC.Note3.path | 68 ++++++ .../pathplanner/paths/top to center top.path | 2 +- .../frc/team7520/robot/RobotContainer.java | 6 +- .../team7520/robot/auto/ShootSequence.java | 4 +- 17 files changed, 625 insertions(+), 62 deletions(-) rename src/main/deploy/pathplanner/autos/{3NoteMid.auto => 3NoteMid.Note1.auto} (50%) create mode 100644 src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto create mode 100644 src/main/deploy/pathplanner/autos/4Note.auto create mode 100644 src/main/deploy/pathplanner/paths/Note3.SpeakerC.path create mode 100644 src/main/deploy/pathplanner/paths/SpeakerC.Note3.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 406c9a6..8862b85 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,7 +5,7 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 4.6, - "defaultMaxAccel": 4.6, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.6 diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.auto b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto similarity index 50% rename from src/main/deploy/pathplanner/autos/3NoteMid.auto rename to src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto index 1fdaed4..79aab34 100644 --- a/src/main/deploy/pathplanner/autos/3NoteMid.auto +++ b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto @@ -43,21 +43,41 @@ } }, { - "type": "named", - "data": { - "name": "intakeIn" - } - }, - { - "type": "path", - "data": { - "pathName": "Close Mid to Mid" - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "stopIntaking" + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] } }, { @@ -92,21 +112,41 @@ } }, { - "type": "named", - "data": { - "name": "intakeIn" - } - }, - { - "type": "path", - "data": { - "pathName": "Close Top To Mid" - } - }, - { - "type": "named", + "type": "parallel", "data": { - "name": "stopIntaking" + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Top To Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] } }, { @@ -114,12 +154,6 @@ "data": { "name": "shoot" } - }, - { - "type": "parallel", - "data": { - "commands": [] - } } ] } diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto b/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto new file mode 100644 index 0000000..36f626b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto @@ -0,0 +1,163 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerC.Note3" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Note3.SpeakerC" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4Note.auto b/src/main/deploy/pathplanner/autos/4Note.auto new file mode 100644 index 0000000..1aa5dd0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4Note.auto @@ -0,0 +1,226 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Top" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Top To Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerC.Note3" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Note3.SpeakerC" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path index 42f216c..0359dbd 100644 --- a/src/main/deploy/pathplanner/paths/Amp.path +++ b/src/main/deploy/pathplanner/paths/Amp.path @@ -55,7 +55,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Bot to mid bot.path b/src/main/deploy/pathplanner/paths/Bot to mid bot.path index 5597b8a..c5bd8f4 100644 --- a/src/main/deploy/pathplanner/paths/Bot to mid bot.path +++ b/src/main/deploy/pathplanner/paths/Bot to mid bot.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Close Mid to Mid.path b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path index cc32a88..fb736aa 100644 --- a/src/main/deploy/pathplanner/paths/Close Mid to Mid.path +++ b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.329669568430046, + "x": 1.3533001663498863, "y": 5.55 }, "prevControl": { - "x": 2.2719190881609803, + "x": 2.295549686080821, "y": 5.55 }, "nextControl": null, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Close Top To Mid.path b/src/main/deploy/pathplanner/paths/Close Top To Mid.path index 6c4579f..d3d6f7a 100644 --- a/src/main/deploy/pathplanner/paths/Close Top To Mid.path +++ b/src/main/deploy/pathplanner/paths/Close Top To Mid.path @@ -20,24 +20,24 @@ "y": 6.991800676923469 }, "prevControl": { - "x": 2.303258272103077, - "y": 6.991800676923469 + "x": 2.287495365622818, + "y": 7.065463266561544 }, "nextControl": { - "x": 1.1232582721030768, - "y": 6.991800676923469 + "x": 1.6616994951342325, + "y": 6.784785233255618 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2270533631976355, - "y": 5.569261320340823 + "x": 1.323928801703758, + "y": 5.551187918118234 }, "prevControl": { - "x": 2.2270533631976352, - "y": 5.569261320340823 + "x": 2.323928801703758, + "y": 5.551187918118234 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -64,5 +64,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid To Close Mid.path b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path index e0f6f13..c9aef80 100644 --- a/src/main/deploy/pathplanner/paths/Mid To Close Mid.path +++ b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Mid To Close Top.path b/src/main/deploy/pathplanner/paths/Mid To Close Top.path index 924478d..86d6580 100644 --- a/src/main/deploy/pathplanner/paths/Mid To Close Top.path +++ b/src/main/deploy/pathplanner/paths/Mid To Close Top.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 1.82409310273166, + "x": 2.0728986001800274, "y": 6.991800676923469 }, "prevControl": { - "x": 1.205710842450273, + "x": 1.4545163398986403, "y": 6.80503451422615 }, "nextControl": { - "x": 1.8707368323827558, + "x": 2.1195423298311233, "y": 7.0058881935945685 }, "isLocked": false, @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Mid to center top.path b/src/main/deploy/pathplanner/paths/Mid to center top.path index fd806d9..6a3276c 100644 --- a/src/main/deploy/pathplanner/paths/Mid to center top.path +++ b/src/main/deploy/pathplanner/paths/Mid to center top.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Note3.SpeakerC.path b/src/main/deploy/pathplanner/paths/Note3.SpeakerC.path new file mode 100644 index 0000000..a35c8a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note3.SpeakerC.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.11 + }, + "prevControl": null, + "nextControl": { + "x": 2.3384574944462946, + "y": 3.8603434005079107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0288415532108353, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 2.0367375362376348, + "y": 3.9729436354627934 + }, + "nextControl": { + "x": 1.9700988239185793, + "y": 5.0225033544879265 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.34, + "y": 5.55 + }, + "prevControl": { + "x": 2.0744556721993943, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Safe Auto.path b/src/main/deploy/pathplanner/paths/Safe Auto.path index 3d7a72c..77df419 100644 --- a/src/main/deploy/pathplanner/paths/Safe Auto.path +++ b/src/main/deploy/pathplanner/paths/Safe Auto.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path b/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path new file mode 100644 index 0000000..fb7bf26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.309243119380694, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.654351490521163, + "y": 5.503960098829289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9360380538942894, + "y": 3.9833745122035706 + }, + "prevControl": { + "x": 1.6561756759877155, + "y": 4.285871337197221 + }, + "nextControl": { + "x": 1.9616383756039566, + "y": 3.95570371166969 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533077793428314, + "y": 3.9833745122035706 + }, + "prevControl": { + "x": 2.194222362098976, + "y": 3.9780227214999897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top to center top.path b/src/main/deploy/pathplanner/paths/top to center top.path index e18e9df..556d1aa 100644 --- a/src/main/deploy/pathplanner/paths/top to center top.path +++ b/src/main/deploy/pathplanner/paths/top to center top.path @@ -49,7 +49,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.6, - "maxAcceleration": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index ad34c37..6bb3c00 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -162,7 +162,10 @@ private void registerAutos(){ autoChooser.addOption("MidToCentTop", drivebase.getPPAutoCommand("MidToCentTop", true)); autoChooser.addOption("TopToCentTop", drivebase.getPPAutoCommand("TopToCentTop", true)); autoChooser.addOption("2Note", drivebase.getPPAutoCommand("2NoteMid", true)); - autoChooser.addOption("3NoteMid", drivebase.getPPAutoCommand("3NoteMid", true)); + autoChooser.addOption("3NoteMid.Note1", drivebase.getPPAutoCommand("3NoteMid.Note1", true)); + autoChooser.addOption("3NoteMid.Note3", drivebase.getPPAutoCommand("3NoteMid.Note3", true)); + autoChooser.addOption("4Note", drivebase.getPPAutoCommand("4Note", true)); + SmartDashboard.putData(autoChooser); @@ -182,6 +185,7 @@ private void registerNamedCommands() NamedCommands.registerCommand("stopIntaking", new InstantCommand(() -> intakeSubsystem.setSpeed(0))); NamedCommands.registerCommand("intakeIn", new AutoIntake(Position.SHOOT)); + } diff --git a/src/main/java/frc/team7520/robot/auto/ShootSequence.java b/src/main/java/frc/team7520/robot/auto/ShootSequence.java index d3be198..a09e5a3 100644 --- a/src/main/java/frc/team7520/robot/auto/ShootSequence.java +++ b/src/main/java/frc/team7520/robot/auto/ShootSequence.java @@ -17,11 +17,11 @@ public ShootSequence() { new AutoIntake(Constants.IntakeConstants.Position.SHOOT), new ParallelRaceGroup( new AutoShoot(1, false), - new WaitCommand(0.1) + new WaitCommand(0.25) ) ), new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(1)), - new WaitCommand(0.5), + new WaitCommand(0.3), new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(0)) // new InstantCommand(() -> ShooterSubsystem.getInstance().setSpeed(0, false)) ); From b1377f0adf666cd9558636d6ff3ababc887ad302 Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:42:19 -0400 Subject: [PATCH 29/31] PID now more chill --- src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto | 4 ++-- src/main/deploy/swerve/neo/controllerproperties.json | 2 +- src/main/java/frc/team7520/robot/Constants.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto index 79aab34..08e2d85 100644 --- a/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto +++ b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto @@ -65,7 +65,7 @@ { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.6 } }, { @@ -134,7 +134,7 @@ { "type": "wait", "data": { - "waitTime": 0.75 + "waitTime": 0.6 } }, { diff --git a/src/main/deploy/swerve/neo/controllerproperties.json b/src/main/deploy/swerve/neo/controllerproperties.json index c5ab644..972cbd7 100644 --- a/src/main/deploy/swerve/neo/controllerproperties.json +++ b/src/main/deploy/swerve/neo/controllerproperties.json @@ -1,7 +1,7 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.4, + "p": 0.3, "i": 0, "d": 0.01 } diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 43eebc8..3bb9642 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -65,7 +65,7 @@ public static class Telemetry { public static class IntakeConstants { public enum Position { SHOOT(new Rotation2d(0), 1), - INTAKE(new Rotation2d(Units.degreesToRadians(212.152317734808d)), -0.35), + INTAKE(new Rotation2d(Units.degreesToRadians(212.152317734808d)), -0.3), AMP(new Rotation2d(Units.degreesToRadians(90)), 0.525); private final Rotation2d position; From 25f5a3b7a0272cb2e0a36691e4290f1cd12a0a5f Mon Sep 17 00:00:00 2001 From: RunTheBot <58890327+RunTheBot@users.noreply.github.com> Date: Thu, 14 Mar 2024 19:35:16 -0400 Subject: [PATCH 30/31] PID now more chill --- src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index 7cd8998..3483d96 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -96,9 +96,9 @@ public void execute() { ChassisSpeeds desiredSpeeds; if (CWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(20))); } else if (CCWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(20))); } else { // Get the desired chassis speeds based on a 2 joystick module. desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, From 8f136e438a94884ed7efc0bb7aee38628beed5d0 Mon Sep 17 00:00:00 2001 From: kitzoyan <110635359+kitzoyan@users.noreply.github.com> Date: Fri, 15 Mar 2024 22:00:05 -0400 Subject: [PATCH 31/31] Additional Auto: Speaker Source Side 1Note Shot to Note8 Parking No Intake, For Blue & Red --- .../pathplanner/autos/SpeakerS.Note8.auto | 50 ++++++++++++ .../pathplanner/paths/SpeakerS.Note8.path | 79 +++++++++++++++++++ .../frc/team7520/robot/RobotContainer.java | 6 ++ 3 files changed, 135 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto create mode 100644 src/main/deploy/pathplanner/paths/SpeakerS.Note8.path diff --git a/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto b/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto new file mode 100644 index 0000000..c61b9c3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6419522005022541, + "y": 4.377819358446314 + }, + "rotation": -59.37446728130534 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "stopShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerS.Note8" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path b/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path new file mode 100644 index 0000000..1d735f2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6419522005022541, + "y": 4.377819358446314 + }, + "prevControl": null, + "nextControl": { + "x": 1.6047408109658412, + "y": 2.318976008690591 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.630250978090726, + "y": 1.3524488816112226 + }, + "prevControl": { + "x": 2.9206051269403472, + "y": 1.5130233538187157 + }, + "nextControl": { + "x": 5.866350427994003, + "y": 1.2363511432225178 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.986352139508573, + "y": 1.3524488816112226 + }, + "prevControl": { + "x": 5.886838264180244, + "y": 1.2295218644937702 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake Down", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 50.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.58163552094371, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.01348815657413, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 6bb3c00..9587b06 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -166,6 +166,9 @@ private void registerAutos(){ autoChooser.addOption("3NoteMid.Note3", drivebase.getPPAutoCommand("3NoteMid.Note3", true)); autoChooser.addOption("4Note", drivebase.getPPAutoCommand("4Note", true)); + // 1note shoot and Speaker Source side to note8 parking + autoChooser.addOption("SpeakerS.Note8", drivebase.getPPAutoCommand("SpeakerS.Note8", true)); + SmartDashboard.putData(autoChooser); @@ -184,6 +187,9 @@ private void registerNamedCommands() NamedCommands.registerCommand("intake", new InstantCommand(() -> intakeSubsystem.setSpeed(Position.INTAKE.getSpeed()))); NamedCommands.registerCommand("stopIntaking", new InstantCommand(() -> intakeSubsystem.setSpeed(0))); NamedCommands.registerCommand("intakeIn", new AutoIntake(Position.SHOOT)); + + // Overriding default command to always shoot: used to stop shooter in race group + NamedCommands.registerCommand("stopShoot", new AutoShoot(0, false)); }