From 01d04bc3f0e8ec20307e7a5116b2b5a9d36aa252 Mon Sep 17 00:00:00 2001 From: Lori Brown <34607662+Lori2018@users.noreply.github.com> Date: Sat, 20 Mar 2021 16:54:33 -0400 Subject: [PATCH 1/2] 3.20.21 --- .../main/java/frc/robot/RobotContainer.java | 71 +++++++------------ .../frc/robot/commands/ControlConveyor.java | 4 +- .../java/frc/robot/commands/DefaultDrive.java | 1 - .../java/frc/robot/commands/MoveHood.java | 1 + .../java/frc/robot/commands/SpinTurret.java | 8 ++- .../frc/robot/subsystems/SwerveDrive.java | 1 + .../java/frc/robot/subsystems/Turret.java | 25 +++---- 7 files changed, 48 insertions(+), 63 deletions(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java index 0e2bb90..c01985a 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/RobotContainer.java @@ -94,7 +94,8 @@ public RobotContainer() { swerveDrive.setDefaultCommand(new DefaultDrive(swerveDrive, m_driverController, 1)); conveyor.setDefaultCommand(new AutoIndexConveyor(conveyor)); intake.setDefaultCommand(new RunIntake(intake, m_operatorController)); - turret.setDefaultCommand(new SpinTurret(turret, false, 0)); + turret.setDefaultCommand(new SpinTurret(turret, vision, 2, 0)); + shooter.setDefaultCommand(new StopShooter(shooter)); // vision.setDefaultCommand(new RunCommand(vision::runVision, vision)); @@ -115,10 +116,18 @@ public RobotContainer() { * calling passing it to a {@link JoystickButton}. */ private void configureButtonBindings() { - //starts with 1 - JoystickButton butA = new JoystickButton(m_operatorController, 1); - JoystickButton butB = new JoystickButton(m_operatorController, 2); - JoystickButton butY = new JoystickButton(m_operatorController, 3); + + JoystickButton butA = new JoystickButton(m_operatorController, 1); + JoystickButton butB = new JoystickButton(m_operatorController, 2); + JoystickButton butX = new JoystickButton(m_operatorController, 3); + JoystickButton butY = new JoystickButton(m_operatorController, 4); + JoystickButton lBump = new JoystickButton(m_operatorController, 5); + JoystickButton rBump = new JoystickButton(m_operatorController, 6); + JoystickButton lWing = new JoystickButton(m_operatorController, 7); + JoystickButton rWing = new JoystickButton(m_operatorController, 8); + JoystickButton lAnal = new JoystickButton(m_operatorController, 9); + JoystickButton rAnal = new JoystickButton(m_operatorController, 10); + JoystickButton butXd = new JoystickButton(m_driverController, 3); JoystickButton butAd = new JoystickButton(m_driverController, 1); @@ -126,10 +135,18 @@ private void configureButtonBindings() { JoystickButton gyro = new JoystickButton(m_driverController, 8); - JoystickButton rBump = new JoystickButton(m_operatorController, 6); - JoystickButton lBump = new JoystickButton(m_operatorController, 5); - JoystickButton lAnal = new JoystickButton(m_operatorController, 9); - JoystickButton rAnal = new JoystickButton(m_operatorController, 10); + butA.whenPressed(new ExtendIntake(intake, m_operatorController)); + butB.whileHeld(new SpinTurret(turret, vision, 1, 0.25)); + butX.whileHeld(new SpinTurret(turret, vision, 1, -0.25)); + butY.whileHeld(new RunShooter(shooter)); + lBump.whileHeld(new ControlConveyor(conveyor, 1)); + rBump.whileHeld(new ControlConveyor(conveyor, -1)); + + // rWing for vision + lAnal.whileHeld(new MoveHood(shooter, 1)); + rAnal.whileHeld(new MoveHood(shooter, -1)); + + // B JoystickButton turbo = new JoystickButton(m_driverController, 2); @@ -154,37 +171,6 @@ private void configureButtonBindings() { SmartDashboard.putData("Slalom Path", Slolam); - // A button - butA.whileHeld(new ExtendIntake(intake, m_operatorController)); - butA.whenReleased(new RetractIntake(intake)); - - // right bumper - rBump.whileHeld(new RunShooter(shooter)); - rBump.whenReleased(new StopShooter(shooter)); - - // left analog center - lAnal.whileHeld(new MoveHood(shooter, -1)); - - - // right analog center - rAnal.whileHeld(new MoveHood(shooter, 1)); - - // right bumper - rBump.whileHeld(new FeedShooter(conveyor, shooter)); - rBump.whenReleased(new AutoIndexConveyor(conveyor)); - - // left bumper - lBump.whileHeld(new ControlConveyor(conveyor)); - lBump.whenReleased(new AutoIndexConveyor(conveyor)); - - // B button - butB.whileHeld(new SpinTurret(turret, true, 0.25)); - butB.whenReleased(new SpinTurret(turret, true, 0)); - - // Y button - butY.whileHeld(new SpinTurret(turret, true, -0.25)); - butY.whenReleased(new SpinTurret(turret, true, 0)); - // driver X button - slow butXd.whileHeld(new DefaultDrive(swerveDrive, m_driverController, 0.35)); butAd.whenPressed(new InstantCommand(swerveDrive::zeroWheels)); @@ -195,11 +181,6 @@ private void configureButtonBindings() { gyro.whenPressed(new InstantCommand(swerveDrive::zeroHeading)); - //new JoystickButton(m_operatorController, 4).whenPressed(new RunCommand(() -> conveyor.manualControl(-), conveyor)) - // .whenReleased(new RunCommand(conveyor::autoIndex, conveyor)); - // should be start button for camera to find target idk what number is so fix it - // new JoystickButton(m_operatorController, 7).whenHeld(new InstantCommand(turret::visionTurret, turret)); - } public static String getCoords() { diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java index de17209..6a0593a 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/ControlConveyor.java @@ -16,10 +16,10 @@ public class ControlConveyor extends CommandGroup { /** * Creates a new ControlConveyor. */ - public ControlConveyor(Conveyor subsystem) { + public ControlConveyor(Conveyor subsystem, double speed) { requires(subsystem); m_conveyor = subsystem; - m_speed = 1; + m_speed = speed; // Use addRequirements() here to declare subsystem dependencies. } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java index 718bc72..4e5bdea 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/DefaultDrive.java @@ -112,7 +112,6 @@ public void execute() { // fill with correct button if (m_joystick.getRawButtonPressed(6)) { // to zero all wheels - SmartDashboard.putNumber("BUTTON PRESSED", 5); } // sidestep diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java index a951194..5419e6d 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/MoveHood.java @@ -20,6 +20,7 @@ public class MoveHood extends CommandGroup { public MoveHood(Shooter subsystem, double pos) { requires(subsystem); m_shooter = subsystem; + m_pos = pos; // Use addRequirements() here to declare subsystem dependencies. } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java index 7f274e1..608aa72 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/commands/SpinTurret.java @@ -9,14 +9,16 @@ import edu.wpi.first.wpilibj.command.InstantCommand; import frc.robot.subsystems.Turret; +import frc.robot.subsystems.Vision; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class SpinTurret extends InstantCommand { - public SpinTurret(Turret subsystem, boolean manual, double speed) { - super(subsystem, () -> {subsystem.spin(manual, speed);}); - requires(subsystem); + public SpinTurret(Turret subsystem1, Vision subsystem2, int mode, double speed) { + super(subsystem1, () -> {subsystem1.spin(subsystem2, mode, speed);}); + requires(subsystem1); + requires(subsystem2); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java index 42617bf..9cef458 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -178,6 +178,7 @@ public void resetEncoders() { */ public void zeroHeading() { navX.reset(); + resetEncoders(); gyroReset = true; } diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java index d78f721..e933638 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -30,7 +30,7 @@ public class Turret extends Subsystem { // transfer to robot container private static final int motor = 10; private final CANSparkMax turretMotor; - private final CANPIDController m_turretPIDController; + private final PIDController m_turretPIDController; private CANEncoder turretEncoder; public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, setPoint, rotations; private CANDigitalInput m_reverseLimit; @@ -63,7 +63,7 @@ public Turret(SwerveDrive swerve) { turretEncoder = turretMotor.getEncoder(); turretEncoder.setPosition(0); - m_turretPIDController = turretMotor.getPIDController(); + m_turretPIDController = new PIDController(0, 0, 0); // PID coefficients kP = 0.1; @@ -78,9 +78,9 @@ public Turret(SwerveDrive swerve) { m_turretPIDController.setP(kP); m_turretPIDController.setI(kI); m_turretPIDController.setD(kD); - m_turretPIDController.setIZone(kIz); - m_turretPIDController.setFF(kFF); - m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput); + // m_turretPIDController.setIZone(kIz); + // m_turretPIDController.setFF(kFF); + // m_turretPIDController.setOutputRange(kMinOutput, kMaxOutput); // m_reverseLimit = turretMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); // m_reverseLimit.enableLimitSwitch(true); @@ -95,7 +95,7 @@ public void periodic() { } public void setPosition(double setpoint) { - m_turretPIDController.setReference(setpoint, ControlType.kPosition); + turretMotor.set(m_turretPIDController.calculate(turretEncoder.getPosition(), 0)); SmartDashboard.putNumber("SetPoint", setpoint); SmartDashboard.putNumber("ProcessVariable", turretEncoder.getPosition()); } @@ -104,7 +104,7 @@ public void stop() { turretMotor.set(0); } - public void spin(boolean manual, double speed) { + public void spin(Vision vision, int mode, double speed) { double robotHeading = swerve.getHeading(); double targetPosition = 0; @@ -115,12 +115,13 @@ public void spin(boolean manual, double speed) { targetPosition = 360 - robotHeading; } - if (manual) + if (mode == 1) turretMotor.set(speed); - else - // turretMotor.set(0); - setPosition(targetPosition); - // m_turretPIDController.setReference(targetPosition, ControlType.kPosition); + else if (mode == 2) { + setPosition(0); + } else if (mode == 3) { + // vision stuff + } } @Override From 2acfca3b1f4be3335f2f6b72ae52b6df73868e88 Mon Sep 17 00:00:00 2001 From: Lori Brown <34607662+Lori2018@users.noreply.github.com> Date: Thu, 25 Mar 2021 18:59:47 -0400 Subject: [PATCH 2/2] m --- .../src/main/java/frc/robot/subsystems/Turret.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java index e933638..7379b41 100644 --- a/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java +++ b/RobotCode2021-Imported/src/main/java/frc/robot/subsystems/Turret.java @@ -118,7 +118,8 @@ public void spin(Vision vision, int mode, double speed) { if (mode == 1) turretMotor.set(speed); else if (mode == 2) { - setPosition(0); + + setPosition(targetPosition); } else if (mode == 3) { // vision stuff }