From 5f326a49589f42152f4815053e9cc6f778e88f51 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 09:15:02 -0600 Subject: [PATCH 1/7] camera test --- src/main/java/frc4388/robot/RobotContainer.java | 5 +++-- src/main/java/frc4388/robot/subsystems/Camera.java | 13 ++++++++++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 25d9e70..737530b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Extender; @@ -75,6 +76,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ + public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40); public final Climber m_robotClimber = new Climber(m_robotMap.elbow); public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); @@ -83,7 +85,7 @@ public class RobotContainer { public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); - // private final LED m_robotLED = new LED(m_robotMap.LEDController); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); @@ -137,7 +139,6 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate. // double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0 diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java index 4645663..0be9276 100644 --- a/src/main/java/frc4388/robot/subsystems/Camera.java +++ b/src/main/java/frc4388/robot/subsystems/Camera.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import edu.wpi.first.cscore.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,14 +22,20 @@ public class Camera extends SubsystemBase { * @param height Resolution height. * @param brightness Percent brightness of the stream. */ + + public UsbCamera test; + public Camera(String name, int id, int width, int height, int brightness) { try{ - UsbCamera cam = new UsbCamera(name, id); + UsbCamera cam = CameraServer.startAutomaticCapture();//new UsbCamera(name, id); cam.setResolution(width, height); cam.setBrightness(brightness); cam.setFPS(10); - VideoSource camera = cam; - CameraServer.startAutomaticCapture(camera); + SmartDashboard.putBoolean("cam enabled", cam.isEnabled()); + SmartDashboard.putBoolean("cam connected", cam.isConnected()); + // VideoSource camera = cam; + // CameraServer.addCamera(camera); + // CameraServer.startAutomaticCapture(camera); } catch(Exception e) { System.err.println("Camera broken, pls nerf"); From 3e9dcf6f7762fa1466d3ca55ce1c2abc0180d96b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 09:18:46 -0600 Subject: [PATCH 2/7] smol change --- src/main/java/frc4388/robot/subsystems/Camera.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Camera.java b/src/main/java/frc4388/robot/subsystems/Camera.java index 0be9276..3ae570e 100644 --- a/src/main/java/frc4388/robot/subsystems/Camera.java +++ b/src/main/java/frc4388/robot/subsystems/Camera.java @@ -22,20 +22,12 @@ public class Camera extends SubsystemBase { * @param height Resolution height. * @param brightness Percent brightness of the stream. */ - - public UsbCamera test; - public Camera(String name, int id, int width, int height, int brightness) { try{ UsbCamera cam = CameraServer.startAutomaticCapture();//new UsbCamera(name, id); cam.setResolution(width, height); cam.setBrightness(brightness); cam.setFPS(10); - SmartDashboard.putBoolean("cam enabled", cam.isEnabled()); - SmartDashboard.putBoolean("cam connected", cam.isConnected()); - // VideoSource camera = cam; - // CameraServer.addCamera(camera); - // CameraServer.startAutomaticCapture(camera); } catch(Exception e) { System.err.println("Camera broken, pls nerf"); From 99d1b5fe3dd4373ef2c4396d5bb6f0bef678c5b6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 10:19:13 -0600 Subject: [PATCH 3/7] two ball auto is up to date --- src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 30 +++++++++---------- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ce60173..b93e4d8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -151,6 +151,7 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { + // System.out.println((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 737530b..45fca00 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -495,13 +495,27 @@ public class RobotContainer { // ! DRIVE BACKWARDS AND SHOOT return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving - new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / SwerveDriveConstants.AUTO_INCHES_PER_SECOND_AT_FULL_POWER),//0.269), // * go backwards three feet + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake weirdAutoShootingGroup, // * shoot new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage ); + // ! TWO BALL AUTO (HOPEFULLY) + // return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake + + // new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path + + // new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving + // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-1.0, 0.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball + // // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376), + // new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), + + // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target + // weirdAutoShootingGroup, // * shoot + // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage + // ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY) // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-0.5, 0, 0.0, 0.0}, 1.0), // * drive out of tarmac @@ -518,20 +532,6 @@ public class RobotContainer { // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage // ); - // ! TWO BALL AUTO (HOPEFULLY) - // return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake - - // new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path - - // new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving - // new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-1.0, 0.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball - // // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376), - // new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), - - // new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target - // weirdAutoShootingGroup, // * shoot - // new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage - // ! THREE BALL AUTO (HOPEFULLY) // return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target // weirdAutoShootingGroup, // * shoot From 3096e29d138ec6d2c10eddf17111d8354d8207d9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 10:29:17 -0600 Subject: [PATCH 4/7] drive mode on off mode switching --- .../java/frc4388/robot/RobotContainer.java | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 45fca00..673df69 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -114,6 +114,10 @@ public class RobotContainer { private enum ClimberMode { MANUAL, AUTONOMOUS }; private ClimberMode currentClimberMode = ClimberMode.MANUAL; + // drive on off mode switching + private enum DriveMode { ON, OFF }; + private DriveMode currentDriveMode = DriveMode.ON; + private SendableChooser quickAutoChooser = new SendableChooser<>(); /** @@ -224,13 +228,13 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + if (currentDriveMode.equals(DriveMode.ON)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), true); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + if (currentDriveMode.equals(DriveMode.OFF)) { m_robotSwerveDrive.driveWithInput( 0, 0, 0, @@ -388,15 +392,15 @@ public class RobotContainer { // Middle Switch > Climber and Shooter mode switching new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whileHeld(new InstantCommand(() -> m_robotExtender.invertExtender(-1.0))) - .whenReleased(new InstantCommand(() -> m_robotExtender.invertExtender(1.0))); + .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) + .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); // Left Button > Extender In new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) From bdfa16679c79320908c8a5517b950354887a82c1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 10:59:01 -0600 Subject: [PATCH 5/7] ;qwoeifh --- src/main/java/frc4388/robot/Robot.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index b93e4d8..72b2168 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -151,7 +151,7 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - // System.out.println((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)); + System.out.println((40.44 - 10.0) / 134.0); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 673df69..9b9a793 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -497,14 +497,14 @@ public class RobotContainer { new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 5.0) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. - // ! DRIVE BACKWARDS AND SHOOT + // ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY) return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (3.0 * 12) / distancePerSecond),//0.269), // * go backwards three feet new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake weirdAutoShootingGroup, // * shoot - new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage - ); + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5)); // * stop running storage + //); // ! TWO BALL AUTO (HOPEFULLY) // return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake From 05305d9243ebec24b20b45343e80b1a32d1a3338 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Mar 2022 11:28:28 -0600 Subject: [PATCH 6/7] no more drive on/off switch --- .../java/frc4388/robot/RobotContainer.java | 26 ++++++------------- 1 file changed, 8 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b9a793..1fe9220 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -227,21 +227,11 @@ public class RobotContainer { /* Default Commands */ // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> { - if (currentDriveMode.equals(DriveMode.ON)) { - m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true); } - if (currentDriveMode.equals(DriveMode.OFF)) { - m_robotSwerveDrive.driveWithInput( 0, - 0, - 0, - 0, - false); - }} - , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), + getDriverController().getLeftY(), + getDriverController().getRightX(), + getDriverController().getRightY(), + true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -398,9 +388,9 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER)) .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER)); - new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) - .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF)) + // .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON)); // Left Button > Extender In new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) From e2e69bd1fd50e644e72c48f4defe9a9b696ed4ac Mon Sep 17 00:00:00 2001 From: Ryan Date: Fri, 25 Mar 2022 11:28:57 -0600 Subject: [PATCH 7/7] track target --- .../frc4388/robot/commands/ShooterCommands/TrackTarget.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 97e1913..97ae723 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -66,6 +66,7 @@ public class TrackTarget extends CommandBase { addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); SmartDashboard.putNumber("Distance Adjust", -35); + SmartDashboard.putBoolean("Target Locked", false); } public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { @@ -190,6 +191,7 @@ public class TrackTarget extends CommandBase { @Override public void end(boolean interrupted) { m_visionOdometry.setLEDs(false); + SmartDashboard.putBoolean("Target Locked", false); } // Returns true when the command should end.