diff --git a/smartdashboard-config.xml b/smartdashboard-config.xml index 48dc83c..db39e22 100644 --- a/smartdashboard-config.xml +++ b/smartdashboard-config.xml @@ -1,5 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + 473 @@ -9,9 +33,6 @@ - - - @@ -73,7 +94,7 @@ - + @@ -385,136 +406,261 @@ - + + + + - + - - + + - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 150 - 250 + + + 0 + 0 - - - 19 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 - - - 149 - 250 + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 - - - 149 + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 0 + 0 + + + + 20 250 - - - 149 + + + 150 250 262 - 2409 - - - + 192 \ No newline at end of file diff --git a/src/main/deploy/PathWeaver/Paths/2BallAuto1(2).path b/src/main/deploy/PathWeaver/Paths/2BallAuto1(2).path index 59a699d..5a640b5 100644 --- a/src/main/deploy/PathWeaver/Paths/2BallAuto1(2).path +++ b/src/main/deploy/PathWeaver/Paths/2BallAuto1(2).path @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name 7.701,-7.6316,0.004,0.01,true,false, -7.935,-5.021599999999999,0.004,0.01,true,false, +8.076330879361914,-5.086270229312063,0.004,0.01,true,false, diff --git a/src/main/deploy/PathWeaver/Paths/2BallAuto2(2).path b/src/main/deploy/PathWeaver/Paths/2BallAuto2(2).path index 22a6399..46858a5 100644 --- a/src/main/deploy/PathWeaver/Paths/2BallAuto2(2).path +++ b/src/main/deploy/PathWeaver/Paths/2BallAuto2(2).path @@ -1,3 +1,3 @@ X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name 5.399,-2.0795999999999992,0.007,-0.003,true,false, -7.353,-3.7485999999999997,0.007,-0.003,true,false, +7.621118309072781,-3.3971920079760713,0.007,-0.003,true,false, diff --git a/src/main/deploy/fenderRPMs.csv b/src/main/deploy/fenderRPMs.csv index 06e593a..1f05973 100644 --- a/src/main/deploy/fenderRPMs.csv +++ b/src/main/deploy/fenderRPMs.csv @@ -3,7 +3,7 @@ PSI,RPM 2.5,2500 2.75,2500 3,2500 -3.5,2500 -3.75,2500 -4,2500 +3.5,2600 +3.75,2600 +4,2600 5,2400 \ No newline at end of file diff --git a/src/main/java/org/team199/robot2022/AutoPath.java b/src/main/java/org/team199/robot2022/AutoPath.java index bd55a18..009e748 100644 --- a/src/main/java/org/team199/robot2022/AutoPath.java +++ b/src/main/java/org/team199/robot2022/AutoPath.java @@ -11,17 +11,19 @@ public class AutoPath { public final List path; public final boolean shootAtEnd; public final boolean runIntake; + public final boolean useLimelight; public final ShotPosition startShotPosition, endShotPosition; - public AutoPath(boolean shootAtStart, List path, boolean shootAtEnd, boolean runIntake) { - this(shootAtStart, path, shootAtEnd, runIntake, ShotPosition.FENDER, ShotPosition.FENDER); + public AutoPath(boolean shootAtStart, List path, boolean shootAtEnd, boolean runIntake, boolean useLimelight) { + this(shootAtStart, path, shootAtEnd, runIntake, useLimelight, ShotPosition.FENDER, ShotPosition.FENDER); } - public AutoPath(boolean shootAtStart, List path, boolean shootAtEnd, boolean runIntake, ShotPosition startShotPosition, ShotPosition endShotPosition) { + public AutoPath(boolean shootAtStart, List path, boolean shootAtEnd, boolean runIntake, boolean useLimelight, ShotPosition startShotPosition, ShotPosition endShotPosition) { this.shootAtStart = shootAtStart; this.path = path; this.shootAtEnd = shootAtEnd; this.runIntake = runIntake; + this.useLimelight = useLimelight; this.startShotPosition = startShotPosition; this.endShotPosition = endShotPosition; } diff --git a/src/main/java/org/team199/robot2022/Constants.java b/src/main/java/org/team199/robot2022/Constants.java index a489420..b42bc8e 100644 --- a/src/main/java/org/team199/robot2022/Constants.java +++ b/src/main/java/org/team199/robot2022/Constants.java @@ -149,6 +149,7 @@ public static enum StickDirection {X, Y}; public static final class LeftJoy { public static final int port = 0; + public static final int slowDriveButton = 1; public static final int manualAddPort = 3; public static final int manualSubtractPort = 2; public static final int shootSoftOnePort = 6; @@ -162,7 +163,7 @@ public static final class LeftJoy { public static final class RightJoy { public static final int port = 1; - + public static final int autoIntake = 1; public static final int shootPort = 2; public static final int slowExtendLeftClimberPort = 5; public static final int slowRetractLeftClimberPort = 3; diff --git a/src/main/java/org/team199/robot2022/Robot.java b/src/main/java/org/team199/robot2022/Robot.java index bc20bbf..48d79d0 100644 --- a/src/main/java/org/team199/robot2022/Robot.java +++ b/src/main/java/org/team199/robot2022/Robot.java @@ -75,6 +75,7 @@ public void autonomousInit() { robotContainer.dt.resetOdometry(); robotContainer.getAutonomousCommand().schedule(); robotContainer.initShooterConfig(); + robotContainer.setLimelightColor(); } /** This function is called periodically during autonomous. */ @@ -90,6 +91,7 @@ public void teleopInit() { CommandScheduler.getInstance().cancelAll(); robotContainer.dt.brake(); robotContainer.initShooterConfig(); + robotContainer.setLimelightColor(); } /** This function is called periodically during operator control. */ @@ -104,6 +106,8 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + robotContainer.setLimelightColor(); + } } diff --git a/src/main/java/org/team199/robot2022/RobotContainer.java b/src/main/java/org/team199/robot2022/RobotContainer.java index 48ffd14..7ab5b72 100644 --- a/src/main/java/org/team199/robot2022/RobotContainer.java +++ b/src/main/java/org/team199/robot2022/RobotContainer.java @@ -5,7 +5,13 @@ package org.team199.robot2022; import java.io.IOException; +import java.util.Arrays; +import java.util.List; +import java.util.stream.Collectors; +import org.carlmontrobotics.lib199.Limelight; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.path.RobotPath; import org.team199.robot2022.commands.Autonomous; import org.team199.robot2022.commands.ExtendClimber; import org.team199.robot2022.commands.PassiveAutomaticIntake; @@ -34,16 +40,11 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PerpetualCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; -import org.carlmontrobotics.lib199.MotorControllerFactory; -import org.carlmontrobotics.lib199.path.RobotPath; -import java.util.Arrays; -import java.util.List; -import java.util.stream.Collectors; /** * This class is where the bulk of the robot should be declared. Since @@ -65,12 +66,14 @@ public class RobotContainer { public final Drivetrain dt = new Drivetrain(); public final PowerDistribution pdp = new PowerDistribution(); public final Shooter shooter = new Shooter(); + private final Limelight lime = new Limelight("limelight-intake", 1/90d); public final IntakeFeeder intakeFeeder; public final DigitalInput[] autoSelectors; public final AutoPath[] autoPaths; private final boolean inCompetition = true; + private boolean robotWasFieldAligned = true; private final SendableChooser autoSelector = new SendableChooser<>(); @@ -80,17 +83,21 @@ public class RobotContainer { public RobotContainer(Robot robot) { MotorControllerFactory.configureCamera(); + lime.config.steeringFactor = .4; + + boolean useLimelightInAuto = true; // CHANGE THIS TO FALSE IF AUTO LIMELIGHT DOESN'T WORK + intakeFeeder = new IntakeFeeder(robot); autoPaths = new AutoPath[] { null, - new AutoPath(true, loadPaths("ShootAndTaxi1"), false, false), - new AutoPath(true, loadPaths("ShootAndTaxi2"), false, false), - new AutoPath(true, loadPaths("TarmacShootAndTaxi1"), false, false, ShotPosition.TARMAC, ShotPosition.FENDER), - new AutoPath(true, loadPaths("TarmacShootAndTaxi2"), false, false, ShotPosition.TARMAC, ShotPosition.FENDER), - new AutoPath(true,loadPaths("2BallAuto1(1)", "2BallAuto1(2)"), true, true, ShotPosition.FENDER, ShotPosition.FENDER), - new AutoPath(true, loadPaths("2BallAuto2(1)", "2BallAuto2(2)"), true, true, ShotPosition.FENDER, ShotPosition.FENDER), - new AutoPath(true, loadPaths("3BallAuto(1)", "3BallAuto(2)", "3BallAuto(3)", "3BallAuto(4)", "3BallAuto(5)"), false, true, ShotPosition.FENDER, ShotPosition.FENDER), - new AutoPath(false, loadPaths("RotateInPlace"), false, false, ShotPosition.FENDER, ShotPosition.FENDER) + new AutoPath(true, loadPaths("ShootAndTaxi1"), false, false, false), + new AutoPath(true, loadPaths("ShootAndTaxi2"), false, false, false), + new AutoPath(true, loadPaths("TarmacShootAndTaxi1", "2BallAuto2(2)"), true, false, false, ShotPosition.TARMAC, ShotPosition.FENDER), + new AutoPath(true, loadPaths("TarmacShootAndTaxi2", "2BallAuto1(2)"), true, false, false, ShotPosition.TARMAC, ShotPosition.FENDER), + new AutoPath(true, loadPaths("2BallAuto1(1)", "2BallAuto1(2)"), true, true, useLimelightInAuto, ShotPosition.FENDER, ShotPosition.FENDER), + new AutoPath(true, loadPaths("2BallAuto2(1)", "2BallAuto2(2)"), true, true, useLimelightInAuto, ShotPosition.FENDER, ShotPosition.FENDER), + new AutoPath(true, loadPaths("3BallAuto(1)", "3BallAuto(2)", "3BallAuto(3)", "3BallAuto(4)", "3BallAuto(5)"), false, true, false, ShotPosition.FENDER, ShotPosition.FENDER), + new AutoPath(false, loadPaths("RotateInPlace"), false, false, false, ShotPosition.FENDER, ShotPosition.FENDER) }; autoSelectors = new DigitalInput[Math.min(autoPaths.length, 26)]; @@ -105,8 +112,6 @@ public RobotContainer(Robot robot) { SmartDashboard.putNumber("Field Offset from North (degrees)", getAutoPath() == null ? 180 : getAutoPath().path.get(0).getRotation2d(0).getDegrees() + 180); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(3); - if (DriverStation.isJoystickConnected(Constants.OI.LeftJoy.port) || inCompetition) { configureButtonBindingsLeftJoy(); } else { @@ -128,7 +133,9 @@ public RobotContainer(Robot robot) { dt.setDefaultCommand(new TeleopDrive(dt, () -> inputProcessing(getStickValue(Constants.OI.StickType.LEFT, Constants.OI.StickDirection.Y)), () -> inputProcessing(getStickValue(Constants.OI.StickType.LEFT, Constants.OI.StickDirection.X)), - () -> inputProcessing(getStickValue(Constants.OI.StickType.RIGHT, Constants.OI.StickDirection.X)), () -> leftJoy.getRawButton(1) || rightJoy.getRawButton(1))); + () -> inputProcessing(getStickValue(Constants.OI.StickType.RIGHT, Constants.OI.StickDirection.X)), + () -> leftJoy.getRawButton(Constants.OI.LeftJoy.slowDriveButton), + () -> rightJoy.getRawButton(Constants.OI.RightJoy.autoIntake), lime)); intakeFeeder.setDefaultCommand( new PerpetualCommand( @@ -165,6 +172,12 @@ private void configureButtonBindingsRightJoy() { new JoystickButton(rightJoy, Constants.OI.RightJoy.slowRetractRightClimberPort).whileHeld(new InstantCommand(climber::slowRetractRight)).whenReleased(new InstantCommand(climber::stopRight)); new JoystickButton(rightJoy, Constants.OI.RightJoy.toggleShooterModePort).whenPressed(new InstantCommand(shooter::toggleDutyCycleMode)); new JoystickButton(rightJoy, Constants.OI.RightJoy.overridePort).whenPressed(new InstantCommand(intakeFeeder::override)); + new POVButton(rightJoy, 90).whenPressed(new InstantCommand(() -> lime.setIdleTurnDirection(Limelight.TurnDirection.CW))); + new POVButton(rightJoy, 270).whenPressed(new InstantCommand(() -> lime.setIdleTurnDirection(Limelight.TurnDirection.CCW))); + new JoystickButton(rightJoy, Constants.OI.RightJoy.autoIntake).whenPressed(() -> { + RobotContainer.this.robotWasFieldAligned = SmartDashboard.getBoolean("Field Oriented", true); + SmartDashboard.putBoolean("Field Oriented", false); + }).whenReleased(() -> SmartDashboard.putBoolean("Field Oriented", RobotContainer.this.robotWasFieldAligned)); } private void configureButtonBindingsController() { @@ -175,10 +188,10 @@ private void configureButtonBindingsController() { new JoystickButton(controller, Constants.OI.Controller.toggleIntakePort).whenPressed(new InstantCommand(intakeFeeder::toggleIntake, intakeFeeder)); new JoystickButton(controller, Constants.OI.Controller.extendClimberPort).whenPressed(new ExtendClimber(climber)); new JoystickButton(controller, Constants.OI.Controller.retractClimberPort).whenPressed(new RetractClimber(climber)); - new POVButton(controller, 0).whenPressed(new InstantCommand( () ->{shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() + 0.1);})); - new POVButton(controller, 180).whenPressed(new InstantCommand( () ->{shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() - 0.1);})); - new POVButton(controller, 90).whenPressed(new InstantCommand(() -> {shooter.setMainSpeed(shooter.getTargetSpeed() + 100);})); - new POVButton(controller, 270).whenPressed(new InstantCommand(() -> {shooter.setMainSpeed(shooter.getTargetSpeed() - 100);})); + new POVButton(controller, 0).whenPressed(new InstantCommand(() -> shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() + 0.1))); + new POVButton(controller, 180).whenPressed(new InstantCommand( () -> shooter.setLinearActuatorPos(shooter.getLinearActuatorPos() - 0.1))); + new POVButton(controller, 90).whenPressed(new InstantCommand(() -> shooter.setMainSpeed(shooter.getTargetSpeed() + 100))); + new POVButton(controller, 270).whenPressed(new InstantCommand(() -> shooter.setMainSpeed(shooter.getTargetSpeed() - 100))); } private List loadPaths(String... pathNames) { @@ -196,7 +209,7 @@ public void initShooterConfig() { */ public Command getAutonomousCommand() { AutoPath path = getAutoPath(); - return path == null ? new InstantCommand() : new Autonomous(path, path.shootAtStart, path.shootAtEnd, dt, shooter, intakeFeeder); + return path == null ? new InstantCommand() : new Autonomous(path, path.shootAtStart, path.shootAtEnd, dt, shooter, intakeFeeder, lime); } private double getStickValue(Constants.OI.StickType stick, Constants.OI.StickDirection dir) { @@ -274,4 +287,8 @@ public RobotPath loadPath(String pathName) { return null; } } + + public void setLimelightColor() { + NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("pipeline").setNumber(DriverStation.getAlliance().ordinal()); + } } diff --git a/src/main/java/org/team199/robot2022/commands/Autonomous.java b/src/main/java/org/team199/robot2022/commands/Autonomous.java index 2e0d24a..1a853ef 100644 --- a/src/main/java/org/team199/robot2022/commands/Autonomous.java +++ b/src/main/java/org/team199/robot2022/commands/Autonomous.java @@ -8,6 +8,8 @@ import org.team199.robot2022.subsystems.Shooter; import org.team199.robot2022.subsystems.IntakeFeeder.Motor; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -15,24 +17,43 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import org.carlmontrobotics.lib199.Limelight; import org.carlmontrobotics.lib199.path.RobotPath; public class Autonomous extends SequentialCommandGroup { - public Autonomous(AutoPath path, boolean shootAtStart, boolean shootAtEnd, Drivetrain drivetrain, Shooter shooter, IntakeFeeder intakeFeeder) { + public Autonomous(AutoPath path, boolean shootAtStart, boolean shootAtEnd, Drivetrain drivetrain, Shooter shooter, IntakeFeeder intakeFeeder, Limelight lime) { addRequirements(drivetrain, shooter, intakeFeeder); - addCommands(new PassiveAutomaticIntake(intakeFeeder)); + addCommands(new PassiveManualIntake(intakeFeeder)); if(path.shootAtStart) addCommands(new InstantCommand(() -> shooter.setShotPosition(path.startShotPosition))); addCommands( new InstantCommand(path.path.get(0)::initializeDrivetrainPosition), + shootAtStart ? new SequentialCommandGroup( new WaitUntilCommand(shooter::isAtTargetSpeed), new WaitCommand(1.5), new Shoot(intakeFeeder, shooter)) : new InstantCommand(), shootAtStart ? new SequentialCommandGroup( new WaitUntilCommand(shooter::isAtTargetSpeed), new WaitCommand(1.5), new Shoot(intakeFeeder, shooter)) : new InstantCommand() ); //addCommands(new WaitCommand(4)); for (int i = 0; i < path.path.size(); i++){ - addCommands(new ParallelRaceGroup(path.path.get(i).getPathCommand(false, false), new PassiveAutomaticIntake(intakeFeeder).perpetually())); + addCommands(new ParallelRaceGroup(path.path.get(i).getPathCommand(false, false), new PassiveManualIntake(intakeFeeder).perpetually())); if (i < path.path.size()-1){ - addCommands(new InstantCommand(path.path.get(i+1)::initializeDrivetrainPosition)); + if(path.useLimelight && false) + addCommands( + new ParallelRaceGroup( + new TeleopDrive(drivetrain, () -> 0D, () -> 0D, () -> 0D, () -> false, () -> true, lime), // Auto-pickup is sketch + new WaitUntilCommand(() -> intakeFeeder.getNumBalls() == 1), // Wait until we pick something up + new SequentialCommandGroup( + new WaitCommand(5), // If we haven't found anything after 5 seconds, abort + new ConditionalCommand( + new WaitUntilCommand(() -> intakeFeeder.getNumBalls() == 1), // We're chasing a ball! Wait to pick it up + new InstantCommand(), // We can't see the ball! Just abort + () -> NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("tv").getDouble(0) == 1 + ) + ) + ) + ); + else + addCommands(new InstantCommand(path.path.get(i+1)::initializeDrivetrainPosition)); } } addCommands(new InstantCommand(() -> {drivetrain.stop();})); diff --git a/src/main/java/org/team199/robot2022/commands/PassiveManualIntake.java b/src/main/java/org/team199/robot2022/commands/PassiveManualIntake.java index f6163fe..c5006c3 100644 --- a/src/main/java/org/team199/robot2022/commands/PassiveManualIntake.java +++ b/src/main/java/org/team199/robot2022/commands/PassiveManualIntake.java @@ -23,7 +23,7 @@ public void execute() { SmartDashboard.putString("IntakeFeeder Default Type", "Manual"); // Reset the balls in the cargo as color sensor no longer works and we cannot // accurately record the cargo - intakeFeeder.clearCargo(); + // intakeFeeder.clearCargo(); // Manually intake balls if(lastFeed != intakeFeeder.getNumBalls()) isSpinningUp = true; lastFeed = intakeFeeder.getNumBalls(); diff --git a/src/main/java/org/team199/robot2022/commands/Shoot.java b/src/main/java/org/team199/robot2022/commands/Shoot.java index 787c418..39af299 100644 --- a/src/main/java/org/team199/robot2022/commands/Shoot.java +++ b/src/main/java/org/team199/robot2022/commands/Shoot.java @@ -30,7 +30,10 @@ public Shoot(IntakeFeeder intakeFeeder, Shooter shooter) { new WaitUntilCommand(shooter::isAtTargetSpeed), new FunctionalCommand( shooter::disableShooter, - () -> {intakeFeeder.invertAndRun(Motor.TOP, false, true);}, + () -> { + intakeFeeder.invertAndRun(Motor.TOP, false, true); + intakeFeeder.invertAndRun(Motor.MIDDLE, false, true); + }, interrupted -> { if(interrupted) return; intakeFeeder.stopRunningFeeder(); diff --git a/src/main/java/org/team199/robot2022/commands/TeleopDrive.java b/src/main/java/org/team199/robot2022/commands/TeleopDrive.java index 84f1e94..a7105d8 100644 --- a/src/main/java/org/team199/robot2022/commands/TeleopDrive.java +++ b/src/main/java/org/team199/robot2022/commands/TeleopDrive.java @@ -9,33 +9,40 @@ import java.util.function.Supplier; +import org.carlmontrobotics.lib199.Limelight; import org.team199.robot2022.Constants; import org.team199.robot2022.subsystems.Drivetrain; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.drive.Vector2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; public class TeleopDrive extends CommandBase { private static final double kSlowDriveSpeed = 0.25; private static final double kSlowDriveRotation = 0.30; + private static final double kAlignMultiplier = 1D/3D; + private static final double kAlignForward = 0.6; private final Drivetrain drivetrain; private Supplier fwd; private Supplier str; private Supplier rcw; private Supplier slow; + private Supplier align; + private Limelight lime; double currentForward = 0; double currentStrafe = 0; /** * Creates a new TeleopDrive. */ - public TeleopDrive(Drivetrain drivetrain, Supplier fwd, Supplier str, Supplier rcw, Supplier slow) { + public TeleopDrive(Drivetrain drivetrain, Supplier fwd, Supplier str, Supplier rcw, Supplier slow, Supplier align, Limelight lime) { addRequirements(this.drivetrain = drivetrain); this.fwd = fwd; this.str = str; this.rcw = rcw; this.slow = slow; + this.align = align; + this.lime = lime; } // Called when the command is initially scheduled. @@ -76,6 +83,13 @@ public void execute() { // SmartDashboard.putNumber("Strafe (mps)", currentStrafe); double driveMultiplier = slow.get() ? kSlowDriveSpeed : 1; double rotationMultiplier = slow.get() ? kSlowDriveRotation : 0.55; + if(align.get()) { + currentForward = Constants.DriveConstants.maxSpeed * kAlignForward * NetworkTableInstance.getDefault().getTable(lime.config.ntName).getEntry("tv").getDouble(0.0); + currentStrafe = 0; + driveMultiplier = 1; + rotateClockwise = lime.steeringAssist(); + rotationMultiplier = Constants.DriveConstants.maxRCW * kAlignMultiplier; + } drivetrain.drive(currentForward * driveMultiplier, currentStrafe * driveMultiplier, rotateClockwise * rotationMultiplier); } diff --git a/src/main/java/org/team199/robot2022/subsystems/Drivetrain.java b/src/main/java/org/team199/robot2022/subsystems/Drivetrain.java index b24633f..0ef7bd3 100644 --- a/src/main/java/org/team199/robot2022/subsystems/Drivetrain.java +++ b/src/main/java/org/team199/robot2022/subsystems/Drivetrain.java @@ -26,7 +26,7 @@ public class Drivetrain extends SubsystemBase implements SwerveDriveInterface { // compassOffset is magnetic north relative to the current heading private final double compassOffset; - private final AHRS gyro = new AHRS(SerialPort.Port.kUSB); // Also try kUSB and kUSB2 + private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 private SwerveDriveKinematics kinematics = null; private SwerveDriveOdometry odometry = null; diff --git a/src/main/java/org/team199/robot2022/subsystems/IntakeFeeder.java b/src/main/java/org/team199/robot2022/subsystems/IntakeFeeder.java index fb6c01a..4555788 100644 --- a/src/main/java/org/team199/robot2022/subsystems/IntakeFeeder.java +++ b/src/main/java/org/team199/robot2022/subsystems/IntakeFeeder.java @@ -51,7 +51,7 @@ public class IntakeFeeder extends SubsystemBase { private double midSpeed = 400; private double botSpeed = 0.7; - private double rpmTolerance = 7; + private double rpmTolerance = 6; // Used to calculate whether there is a ball against the motor private double minProxmity = 21.5; // TODO : Accurately determine minProxmity constant private double maxProxmity = 19.5; // TODO : Accurately determine minProxmity constant @@ -110,6 +110,7 @@ public IntakeFeeder(Robot robot) { m_colorSensor.configureColorSensor(ColorSensorResolution.kColorSensorRes16bit, ColorSensorMeasurementRate.kColorRate25ms, GainFactor.kGain3x); m_colorSensor.configureProximitySensor(ProximitySensorResolution.kProxRes8bit, ProximitySensorMeasurementRate.kProxRate6ms); + cargo.add(true); cargo.add(true); robot.addPeriodic(this::updateColorSensor, 0.003); } diff --git a/src/main/java/org/team199/robot2022/subsystems/Shooter.java b/src/main/java/org/team199/robot2022/subsystems/Shooter.java index 669fe66..bae5934 100644 --- a/src/main/java/org/team199/robot2022/subsystems/Shooter.java +++ b/src/main/java/org/team199/robot2022/subsystems/Shooter.java @@ -87,8 +87,10 @@ public void toggleLongShot() { public void periodic() { pidController.periodic(); SmartDashboard.putBoolean("isAtTargetSpeed", isAtTargetSpeed()); + //SmartDashboard.putNumber("Shooter Target Speed", pidController.getTargetSpeed()); SmartDashboard.putString("Shooter: Mode", dutyCycleMode ? "Duty Cycle" : "PID"); SmartDashboard.putBoolean("Shooter Disabled", shooterDisabled); + SmartDashboard.putNumber("Shooter RPM", master.getEncoder().getVelocity()); linearActuatorPos = SmartDashboard.getNumber("Linear Actuator Position", linearActuatorPos); SmartDashboard.putNumber("Linear Actuator Position", linearActuatorPos); linearActuator.set(linearActuatorPos);