From 6ae41e3d4b26d10c14819afea19ae3a435513f21 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 4 Apr 2022 18:53:38 -0600 Subject: [PATCH] two ball auto --- src/main/deploy/ShooterData.csv | 7 +++--- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/Robot.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 22 +++++++++++-------- .../ExtenderIntakeGroup.java | 4 ++-- 5 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 0b2b5aa..d833fcb 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,9 +1,10 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) 0 ,-29.5 ,8000 -78.5 ,-29.5 ,8000 +78.5 ,-29.5 ,8500 88 ,-34.2 ,8600 -99 ,-39.62 ,8600 -111 ,-42 ,9000 +90 ,-35.47 ,9500 +99 ,-39.62 ,9500 +111 ,-42 ,9500 127.25 ,-49.12 ,9500 141 ,-59.4 ,9900 150 ,-66.22 ,10000 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2f917d4..2266b68 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -287,8 +287,8 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; //Gains for turret - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/); - public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.6/*TURRET_SPEED_MULTIPLIER*/); + public static final double SHOOTER_TURRET_MIN = -0.6;//-TURRET_SPEED_MULTIPLIER; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 6eab374..15ee89d 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.utility.RobotTime; import frc4388.utility.Vector2D; @@ -76,6 +77,7 @@ public class Robot extends TimedRobot { // desmosServer.start(); m_robotContainer.m_robotVisionOdometry.setLEDs(false); + ExtenderIntakeGroup.setDirectionToOut(); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 48f3289..b3efd62 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -352,7 +352,7 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0)); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(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)); // * aim with turret to target); + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // * aim with turret to target); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -528,13 +528,13 @@ public class RobotContainer { SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), new ParallelCommandGroup( new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true) + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), new ParallelCommandGroup( new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true), - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true) + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true) )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. // ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY) @@ -620,15 +620,19 @@ public class RobotContainer { ParallelDeadlineGroup intakeWithPath = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true), new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer), buildAuto(1.0, 1.0, "JMove")); + ParallelCommandGroup extendWhileTurretIsAiming = new ParallelCommandGroup(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), new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath, weirdAutoShootingGroup2); // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward")); - return new SequentialCommandGroup(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 + return new SequentialCommandGroup(extendWhileTurretIsAiming,//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 RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true), - new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), - intakeWithPath, - weirdAutoShootingGroup2, + // extendWhileTurretIsAiming, + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true), + // new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), + intakeWithPathAndTrackTarget, + // intakeWithPath, + // weirdAutoShootingGroup2, // 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); // * shoot - new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true)); + new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.1, true)); // return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond")); } diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 21fb95d..20b2669 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -20,7 +20,7 @@ public class ExtenderIntakeGroup extends ParallelRaceGroup { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely + // ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } @@ -28,7 +28,7 @@ public class ExtenderIntakeGroup extends ParallelRaceGroup { ExtenderIntakeGroup.direction = 1; } - public static void changeDirection() { // Never implemented? + public static void changeDirection() { ExtenderIntakeGroup.direction *= -1; } }