From 5997c73f55ca30fc342ebd0e5bab1aef06853003 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 13 Feb 2024 20:01:12 -0700 Subject: [PATCH] it not shooting properly (im tweakin) --- src/main/java/frc4388/robot/Constants.java | 8 ++- src/main/java/frc4388/robot/Robot.java | 9 ++- .../java/frc4388/robot/RobotContainer.java | 67 +++++++++++------- .../robot/commands/Intake/ArmIntakeIn.java | 49 +++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 28 ++++++-- .../java/frc4388/robot/subsystems/LED.java | 69 +++++++++++++------ .../frc4388/robot/subsystems/Shooter.java | 12 +++- 7 files changed, 184 insertions(+), 58 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 18ac32b..abb9af2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -155,15 +155,17 @@ public final class Constants { public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final - public static final double SHOOTER_SPEED = 1; // final + public static final double SHOOTER_SPEED = 1.0; // final + public static final double SHOOTER_IDLE = 0.4; // final + } public static final class IntakeConstants { public static final int INTAKE_MOTOR_ID = 18; //TODO: public static final int PIVOT_MOTOR_ID = 17; //TODO: public static final double INTAKE_SPEED = 0.75; //TODO: - public static final double INTAKE_OUT_SPEED = 0.5; - public static final double HANDOFF_SPEED = 0.2; //TODO: + public static final double INTAKE_OUT_SPEED = 0.7; + public static final double HANDOFF_SPEED = 0.4; //TODO: public static final double PIVOT_SPEED = 0.2; //TODO: public static final class ArmPID { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1a29285..44970b5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -8,12 +8,13 @@ package frc4388.robot; import edu.wpi.first.cameraserver.CameraServer; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; - +//import frc4388.robot.subsystems.LED; /** * The VM is configured to automatically run this class, and to call the * functions corresponding to each mode, as described in the TimedRobot @@ -23,16 +24,17 @@ import frc4388.utility.RobotTime; */ public class Robot extends TimedRobot { Command m_autonomousCommand; - + private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + //private LED mled = new LED(); /** * This function is run when the robot is first started up and should be * used for any initialization code. */ @Override public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -50,6 +52,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 29c86f8..1fe331f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc4388.robot.Constants.OIConstants; 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.SwerveDrive; @@ -42,7 +43,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final LED m_robotLED = new LED(m_robotMap.LEDController); + private final LED m_robotLED = new LED(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, @@ -61,12 +62,13 @@ public class RobotContainer { private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); + private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.pidIn()), - new InstantCommand(() -> m_robotShooter.spin(0.4)) + new InstantCommand(() -> m_robotShooter.spin()) ); private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( @@ -75,12 +77,24 @@ public class RobotContainer { ); private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.rotateArmOut2()), - new RunCommand(() -> m_robotIntake.limitNote()).until(m_robotIntake.getArmFowardLimitState()), - new InstantCommand(() -> m_robotShooter.spin(0.4)) + new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), + new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), + new InstantCommand(() -> m_robotShooter.idle(), m_robotShooter) ); + private SequentialCommandGroup i = new SequentialCommandGroup( + intakeToShootStuff, intakeToShoot + ); + private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake) + ); + + private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake) + ); @@ -136,9 +150,9 @@ public class RobotContainer { // "IntenseTaxi.txt")) // .onFalse(new InstantCommand()); - //new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "IntenseTaxi.txt")) - // .onFalse(new InstantCommand()); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) + .onFalse(new InstantCommand()); /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final @@ -159,13 +173,13 @@ public class RobotContainer { // .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.rotateArmIn())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); - // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotIntake.rotateArmOut())) - // .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) + .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); // //Pull arm in // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) @@ -187,10 +201,10 @@ public class RobotContainer { // .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)) // .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)); - // //Spin Shooter Motors - // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) - // .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); + //Spin Shooter Motors + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_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) @@ -202,8 +216,13 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .whileTrue(intakeInToOut); - + .onTrue(ejectToShoot) + .onFalse(turnOffShoot); + + + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(i) + .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); } @@ -213,10 +232,10 @@ public class RobotContainer { * * @return the command to run in autonomous */ - // public Command getAutonomousCommand() { - // no auto - // return playbackChooser.getCommand(); - // } + public Command getAutonomousCommand() { + //no auto + return playbackChooser.getCommand(); + } /** * Add your docs here. diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java new file mode 100644 index 0000000..1952c2d --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -0,0 +1,49 @@ +// 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 frc4388.robot.commands.Intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Intake; +import frc4388.robot.subsystems.Shooter; + +public class ArmIntakeIn extends Command { + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; + + public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; + + addRequirements(this.robotIntake, this.robotShooter); + } + + // 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() { + robotIntake.pidOut(); + robotIntake.spinIntakeMotor(); + } + + // 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() { + if(robotIntake.getIntakeLimitSwtichState() == true) { + return true; + } else { + return false; + } + + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index aa3c43c..6ec6a16 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -32,6 +32,10 @@ public class Intake extends SubsystemBase { private Shooter shooter; + private BooleanSupplier sup = () -> true; + private BooleanSupplier dup = () -> false; + + /** Creates a new Intake. */ @@ -111,14 +115,11 @@ public class Intake extends SubsystemBase { pidIn(); } } - public void handoff() { intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED); } - - public void stopIntakeMotors() { intakeMotor.set(0); } @@ -130,6 +131,19 @@ public class Intake extends SubsystemBase { public RelativeEncoder getEncoder() { return pivot.getEncoder(); } + + public boolean getForwardLimitSwitchState() { + return forwardLimit.isPressed(); + } + + public boolean getReverseLimitSwitchState() { + return reverseLimit.isPressed(); + } + + public boolean getIntakeLimitSwtichState() { + return intakeforwardLimit.isPressed(); + } + public void setVoltage(double voltage) { pivot.setVoltage(voltage); } @@ -143,7 +157,7 @@ public class Intake extends SubsystemBase { } public void resetPosition1() { - if(forwardLimit.isPressed() == true) { + if(forwardLimit.isPressed()) { resetPostion(); } } @@ -157,7 +171,11 @@ public class Intake extends SubsystemBase { } public BooleanSupplier getArmFowardLimitState() { - return forwardLimit::isPressed; + if(forwardLimit.isPressed()) { + return sup; + } else { + return dup; + } } @Override diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 0d4af80..85919b5 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,6 +7,8 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - private LEDPatterns m_currentPattern; - private Spark m_LEDController; - + static AddressableLED m_led; + static AddressableLEDBuffer m_ledBuffer; + static LED m_self; /** * Add your docs here. */ - public LED(Spark LEDController){ - m_LEDController = LEDController; - setPattern(LEDConstants.DEFAULT_PATTERN); - updateLED(); + + public LED(){ + if(m_self != null) + return; + m_led = new AddressableLED(9); + m_ledBuffer = new AddressableLEDBuffer(130); + m_led.setLength(m_ledBuffer.getLength()); + m_led.setData(m_ledBuffer); + m_led.start(); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); } - + public static LED getInstance() { + if(m_self == null) + m_self = new LED(); + return m_self; + } @Override public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + //gamermode(); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + return; + } + static int firstcolor = 0; + static void gamermode() { + for(int i = 0; i < m_ledBuffer.getLength(); i++) { + final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; + setLEDHSV(i, hue, 255, 128); + } + firstcolor +=3; + firstcolor %= 180; + } + /** + * Add your docs here. + */ + public static void updateLED (){ + gamermode(); + // m_LEDController.set(m_currentPattern.getValue()); } /** * Add your docs here. */ - public void updateLED(){ - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDRGB(int lednum, int r, int g, int b){ + m_ledBuffer.setRGB(lednum, r, g, b); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - - /** - * Add your docs here. - */ - public void setPattern(LEDPatterns pattern){ - m_currentPattern = pattern; - m_LEDController.set(m_currentPattern.getValue()); + public static void setLEDHSV(int lednum, int hue, int sat, int val){ + m_ledBuffer.setRGB(lednum, hue, sat, val); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); } - /** * Add your docs here. * @return */ - public LEDPatterns getPattern() { - return m_currentPattern; + public AddressableLEDBuffer getBuffer() { + return m_ledBuffer; } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index e414aaf..004a70c 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -23,8 +25,8 @@ public class Shooter extends SubsystemBase { leftShooter = leftTalonFX; rightShooter = rightTalonFX; - leftShooter.setNeutralMode(NeutralModeValue.Coast); - rightShooter.setNeutralMode(NeutralModeValue.Coast); + leftShooter.setNeutralMode(NeutralModeValue.Brake); + rightShooter.setNeutralMode(NeutralModeValue.Brake); } public void spin() { @@ -40,9 +42,15 @@ public class Shooter extends SubsystemBase { spin(0.d); } + public void idle() { + spin(ShooterConstants.SHOOTER_IDLE); + } + @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); } }