mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'cleanup' of https://github.com/Team4388/2022NoWayHome into cleanup
This commit is contained in:
aarav18
@@ -116,93 +116,97 @@ public class RobotContainer {
|
||||
* - field
|
||||
*/
|
||||
|
||||
private SequentialCommandGroup makeTheWeirdGroup() {
|
||||
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)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
return weirdAutoShootingGroup;
|
||||
}
|
||||
// private SequentialCommandGroup makeTheWeirdGroup() {
|
||||
// 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)
|
||||
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
// return weirdAutoShootingGroup;
|
||||
// }
|
||||
/**
|
||||
* 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
|
||||
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
|
||||
var justShoot = 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
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
|
||||
// ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY)
|
||||
var offTheLine = 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
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 0.25))); // * drive off line
|
||||
|
||||
// ! TWO BALL AUTO (HOPEFULLY)
|
||||
var twoBall = 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
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
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[] {0.0, 1.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(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
|
||||
|
||||
// ! THREE BALL AUTO (HOPEFULLY)
|
||||
var threeBall = 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
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
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[] {0.0, 1.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 363)
|
||||
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
//new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
|
||||
//new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
|
||||
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
|
||||
new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
|
||||
makeTheWeirdGroup(), // * shoot
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
||||
|
||||
)));
|
||||
|
||||
|
||||
quickAutoChooser.setDefaultOption("justShoot", justShoot);
|
||||
quickAutoChooser.addOption("offTheLine", offTheLine);
|
||||
quickAutoChooser.addOption("twoBall", twoBall);
|
||||
quickAutoChooser.addOption("threeBall", threeBall);
|
||||
// double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
||||
|
||||
SmartDashboard.putData("__QUICK AUTO CHOOSE__", quickAutoChooser);
|
||||
// double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
||||
// double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
|
||||
// Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
// Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
// Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
|
||||
// var justShoot = 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
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
|
||||
// // ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY)
|
||||
// var offTheLine = 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
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 0.25))); // * drive off line
|
||||
|
||||
// // ! TWO BALL AUTO (HOPEFULLY)
|
||||
// var twoBall = 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
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// 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[] {0.0, 1.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(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
|
||||
|
||||
// // ! THREE BALL AUTO (HOPEFULLY)
|
||||
// var threeBall = 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
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// 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[] {0.0, 1.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 363)
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
|
||||
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
|
||||
// new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
||||
|
||||
// )));
|
||||
|
||||
|
||||
// autoChoices = Map.of(
|
||||
// "justShoot", justShoot,
|
||||
// "offTheLine", offTheLine,
|
||||
// "twoBall", twoBall,
|
||||
// "threeBall", threeBall
|
||||
// );
|
||||
|
||||
// SmartDashboard.putData(quickAutoChooser);
|
||||
|
||||
|
||||
|
||||
@@ -329,7 +333,7 @@ public class RobotContainer {
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, false));
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
@@ -462,24 +466,91 @@ public class RobotContainer {
|
||||
// ? positive leftY went left, negative leftY went right?
|
||||
// TODO: if line 372 is true, switch joystick inputs accordingly
|
||||
|
||||
return Objects.requireNonNullElseGet(quickAutoChooser.getSelected(), () -> new PrintCommand("---------- NO AUTO SELECTED --------------"));
|
||||
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
||||
|
||||
// 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
|
||||
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
|
||||
// double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
||||
// double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
|
||||
// Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
// Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
// Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
// 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)
|
||||
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
// 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
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
|
||||
// ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (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
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.6, 0.0, 0.0}, 0.5))); // * drive off line
|
||||
|
||||
// ! TWO 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
|
||||
// makeTheWeirdGroup(), // * shoot
|
||||
// weirdAutoShootingGroup, // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// 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[] {0.0, 1.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(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), 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
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 0.25))); // * drive off line
|
||||
// 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[] {0.0, 1.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 363)
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
|
||||
// weirdAutoShootingGroup, // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
|
||||
|
||||
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d),
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball
|
||||
// //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363)
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
|
||||
|
||||
// new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target
|
||||
// weirdAutoShootingGroup, // * shoot
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
|
||||
|
||||
// )));
|
||||
|
||||
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
|
||||
|
||||
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, 1.0),
|
||||
// new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive),
|
||||
|
||||
// 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), 1.0))
|
||||
|
||||
// );
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -35,7 +35,7 @@ public class TrackTarget extends CommandBase {
|
||||
BoomBoom m_boomBoom;
|
||||
Hood m_hood;
|
||||
|
||||
boolean isAuto;
|
||||
// boolean isAuto;
|
||||
|
||||
static double velocity;
|
||||
static double hoodPosition;
|
||||
@@ -55,22 +55,22 @@ public class TrackTarget extends CommandBase {
|
||||
long startTime;
|
||||
private double timeTolerance;
|
||||
|
||||
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
|
||||
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry/*, boolean isAuto*/) {
|
||||
m_turret = turret;
|
||||
m_boomBoom = boomBoom;
|
||||
m_hood = hood;
|
||||
m_visionOdometry = visionOdometry;
|
||||
|
||||
this.isAuto = isAuto;
|
||||
// this.isAuto = isAuto;
|
||||
this.timeTolerance = 1000;
|
||||
|
||||
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
|
||||
SmartDashboard.putNumber("Distance Adjust", 20);
|
||||
SmartDashboard.putNumber("Distance Adjust", -35);
|
||||
}
|
||||
|
||||
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
|
||||
this(turret, boomBoom, hood, visionOdometry, false);
|
||||
}
|
||||
// public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
|
||||
// this(turret, boomBoom, hood, visionOdometry, false);
|
||||
// }
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
@@ -195,14 +195,14 @@ public class TrackTarget extends CommandBase {
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
if (this.isAuto) {
|
||||
if (targetLocked && !timerStarted) {
|
||||
timerStarted = true;
|
||||
startTime = System.currentTimeMillis();
|
||||
}
|
||||
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
|
||||
} else {
|
||||
// if (this.isAuto) {
|
||||
// if (targetLocked && !timerStarted) {
|
||||
// timerStarted = true;
|
||||
// startTime = System.currentTimeMillis();
|
||||
// }
|
||||
// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
|
||||
// } else {
|
||||
return false;
|
||||
}
|
||||
// }
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user