From cb35732f2b3ea03cab420fb15c12320c03a4ae2e Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 17 Feb 2024 10:16:20 -0700 Subject: [PATCH] Clean up robot container and a some of intake code --- .gitignore | 2 + .../java/frc4388/robot/RobotContainer.java | 79 +++---------------- .../Intake/RotateIntakeToPosition.java | 38 --------- .../java/frc4388/robot/subsystems/Intake.java | 25 +++--- 4 files changed, 24 insertions(+), 120 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java diff --git a/.gitignore b/.gitignore index 5528d4f..b5b18bb 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ +simgui.json +simgui-ds.json diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4528086..107e314 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,10 +7,7 @@ package frc4388.robot; -import java.time.Instant; - import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -23,7 +20,6 @@ import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Intake.ArmIntakeIn; -import frc4388.robot.commands.Intake.RotateIntakeToPosition; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Limelight; import frc4388.robot.subsystems.SwerveDrive; @@ -70,17 +66,6 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin()) ); - private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin()), - new InstantCommand(() -> m_robotIntake.handoff()) - ); - - private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), - new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) - ); - private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot ); @@ -101,22 +86,22 @@ public class RobotContainer { private Command startRightMoveLeft = new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - ejectToShoot, - taxi + ejectToShoot.asProxy(), + taxi.asProxy() ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( - startLeftMoveRight, - ejectToShoot, - taxi + startLeftMoveRight.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() ); private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( - startRightMoveLeft, - ejectToShoot, - taxi + startRightMoveLeft.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", taxi) + .addOption("Taxi Auto", taxi.asProxy()) .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft) .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight) @@ -141,10 +126,6 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); - - // SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity()); - - // m_robotIntake.resetPostion(); } /** @@ -185,13 +166,6 @@ public class RobotContainer { /* Operator Buttons */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter)); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) @@ -204,46 +178,17 @@ public class RobotContainer { // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(-53), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); - // Override Intake Position encoder: out + // Override Intake Position encoder: in new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.setPosition(0), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); - // //Pull arm in - // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); - - // //Pull arm out - // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake)); - - // //Intake Note - // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - - // //Outtake Note - // new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - //Spin Shooter Motors new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); - // //Intake Note and ramp up shooter to 40% - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(intakeToShoot); - - // //Ramps up shooter to 100% to Shooter - // new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - // .onTrue(outtakeToShootFull); - - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(ejectToShoot) .onFalse(turnOffShoot); diff --git a/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java b/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java deleted file mode 100644 index 2dd7a55..0000000 --- a/src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java +++ /dev/null @@ -1,38 +0,0 @@ -// 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 frc4388.robot.commands.Intake; - - -import com.revrobotics.CANSparkMax; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.SwerveDrive; -import edu.wpi.first.wpilibj.motorcontrol.Spark; - -import frc4388.robot.subsystems.Intake; -public class RotateIntakeToPosition extends PID { - - Intake intake; - double targetAngle; - - /** Creates a new PIDSparkMax. */ - public RotateIntakeToPosition(Intake intake, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.intake = intake; - this.targetAngle = targetAngle; - - addRequirements(intake); - } - - @Override - public double getError() { - return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360); - } - - @Override - public void runWithOutput(double output) { - intake.setVoltage(output / Math.abs(getError())); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 6cbb9e2..b10ff7d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -36,10 +36,7 @@ public class Intake extends SubsystemBase { private BooleanSupplier sup = () -> true; private BooleanSupplier dup = () -> false; - private double val; - - - + private double smartDashboardOuttakeValue; /** Creates a new Intake. */ public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { @@ -66,6 +63,8 @@ public class Intake extends SubsystemBase { m_spedController.setP(armGains.kP); m_spedController.setI(armGains.kI); m_spedController.setD(armGains.kD); + + SmartDashboard.putNumber("Intake Speed", 0.5); } //hanoff @@ -123,9 +122,9 @@ public class Intake extends SubsystemBase { public void handoff2() { if(intakeforwardLimit.isPressed()) { - intakeMotor.set(-val); + intakeMotor.set(-smartDashboardOuttakeValue); } else { - intakeMotor.set(-val); + intakeMotor.set(-smartDashboardOuttakeValue); } } @@ -161,17 +160,13 @@ public class Intake extends SubsystemBase { return pivot.getEncoder().getVelocity(); } - public void resetPostion() { - setPosition(0); - } - - public void setPosition(int val) { + public void setPivotEncoderPosition(int val) { pivot.getEncoder().setPosition(val); } - public void resetPosition1() { + public void resetPosition() { if(forwardLimit.isPressed()) { - resetPostion(); + setPivotEncoderPosition(0); } } @@ -206,9 +201,9 @@ public class Intake extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Vel Output", getVelocity()); SmartDashboard.putNumber("Position", getPos()); - resetPosition1(); + resetPosition(); changeIntakeNeutralState(); - val = SmartDashboard.getNumber("Intake Speed", 0.5); + smartDashboardOuttakeValue = SmartDashboard.getNumber("Intake Speed", 0.5); } }