diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 278a409..bd85a56 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,6 +156,12 @@ public static class VisionConstants{ public static final double kCameraPitchRadians = Units.degreesToRadians(30); public static final double kGoalRangeMeters = Units.inchesToMeters(0); public static final double kAreaConstant = 0; + + public static final String kLimelightName = "limelight-high"; + public static final int kAprilTagPipeline = 4; + public static final double kSlidingOffset = 0.4; // Meters away from grid while robot is sliding. + public static final double fieldXOffset = 8; // Guess + public static final double fieldYOffset = 3.5; // Guess } public static final class TankAutoConstants { @@ -398,19 +404,37 @@ public static final class PneumaticsConstants { public static final int kPressureSensorPort = 2; } + // public static final class PathPlannerConstants { + // private static final double kPPMaxVelocity = 1; + // private static final double kPPMaxAcceleration = 1; + // public static final PathConstraints kPPPathConstraints = new PathConstraints(kPPMaxVelocity, kPPMaxAcceleration); + + // public static final double kPP_P = 0.25; + // public static final double kPP_I = 0; + // public static final double kPP_D = 0; + // public static final PIDConstants kPPTranslationPIDConstants = new PIDConstants(kPP_P, kPP_I, kPP_D); + + // public static final double kPP_ThetaP = 0.25; + // public static final double kPP_ThetaI = 0; + // public static final double kPP_ThetaD = 0; + // public static final PIDConstants kPPRotationPIDConstants = new PIDConstants(kPP_ThetaP, kPP_ThetaI, kPP_ThetaD); + + // public static final boolean kUseAllianceColor = true; + // } + public static final class PathPlannerConstants { - private static final double kPPMaxVelocity = 1; - private static final double kPPMaxAcceleration = 1; + public static final double kPPMaxVelocity = 3; + public static final double kPPMaxAcceleration = 3; public static final PathConstraints kPPPathConstraints = new PathConstraints(kPPMaxVelocity, kPPMaxAcceleration); - public static final double kPP_P = 0.25; - public static final double kPP_I = 0; - public static final double kPP_D = 0; + public static final double kPP_P = new PrefDouble("PP_kP", 0.25).get(); + public static final double kPP_I = new PrefDouble("PP_kI", 0.0).get(); + public static final double kPP_D = new PrefDouble("PP_kD", 0.0).get(); public static final PIDConstants kPPTranslationPIDConstants = new PIDConstants(kPP_P, kPP_I, kPP_D); - public static final double kPP_ThetaP = 0.25; - public static final double kPP_ThetaI = 0; - public static final double kPP_ThetaD = 0; + public static final double kPP_ThetaP = new PrefDouble("PP_kThetaP", 0.25).get(); + public static final double kPP_ThetaI = new PrefDouble("PP_kThetaI", 0).get(); + public static final double kPP_ThetaD = new PrefDouble("PP_kThetaD", 0).get(); public static final PIDConstants kPPRotationPIDConstants = new PIDConstants(kPP_ThetaP, kPP_ThetaI, kPP_ThetaD); public static final boolean kUseAllianceColor = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index db799bb..0de72cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import frc.robot.Constants.ArmConstants; import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Reportable.LOG_LEVEL; @@ -30,22 +31,24 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import frc.robot.commands.ChargeAutos; import frc.robot.commands.PathPlannerAutos; -import frc.robot.commands.SwerveAutos; +// import frc.robot.commands.SwerveAutos; import frc.robot.commands.SwerveJoystickCommand; import frc.robot.commands.TestAutos; import frc.robot.commands.TheGreatBalancingAct; -import frc.robot.commands.VisionAllLowAuto; -import frc.robot.commands.VisionCableSideAuto; -import frc.robot.commands.SwerveAutos.StartPosition; +// import frc.robot.commands.VisionAllLowAuto; +// import frc.robot.commands.VisionCableSideAuto; +// import frc.robot.commands.SwerveAutos.StartPosition; import frc.robot.subsystems.imu.Gyro; import frc.robot.subsystems.imu.NavX; import frc.robot.subsystems.swerve.SwerveDrivetrain; import frc.robot.subsystems.swerve.SwerveDrivetrain.DRIVE_MODE; import frc.robot.subsystems.swerve.SwerveDrivetrain.SwerveModuleType; -import frc.robot.subsystems.vision.VROOOOM; -import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; -import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; +import frc.robot.subsystems.vision.primalWallnut.PrimalSunflower; +// import frc.robot.subsystems.vision.VROOOOM; +// import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; +// import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; import frc.robot.commands.SwerveJoystickCommand.DodgeDirection; +import frc.robot.commands.VisionAutos.ToNearestGridDebug; /** * This class is where the bulk of the robot should be declared. Since @@ -64,7 +67,8 @@ public class RobotContainer { public Gyro imu = new NavX(); // public Gyro imu = new Pigeon(60); public SwerveDrivetrain swerveDrive; - public VROOOOM vision; + public PrimalSunflower ps; + // public VROOOOM vision; private final CommandPS4Controller driverController = new CommandPS4Controller( ControllerConstants.kDriverControllerPort); @@ -92,8 +96,8 @@ public class RobotContainer { // private SendableChooser scoreChooser = new SendableChooser(); private SendableChooser allianceChooser = new SendableChooser(); - private SCORE_POS scorePos = SCORE_POS.MID; - private StartPosition startPos = StartPosition.RIGHT; + // private SCORE_POS scorePos = SCORE_POS.MID; + // private StartPosition startPos = StartPosition.RIGHT; private Alliance alliance = Alliance.Invalid; /** @@ -102,13 +106,14 @@ public class RobotContainer { public RobotContainer() { try { swerveDrive = new SwerveDrivetrain(imu, SwerveModuleType.CANCODER); - vision = new VROOOOM(arm, elevator, motorClaw, swerveDrive); + // vision = new VROOOOM(arm, elevator, motorClaw, swerveDrive); + ps = new PrimalSunflower(VisionConstants.kLimelightName, swerveDrive); } catch (IllegalArgumentException e) { DriverStation.reportError("Illegal Swerve Drive Module Type", e.getStackTrace()); } // Initialize vision after swerve has been initialized - vision = new VROOOOM(arm, elevator, motorClaw, swerveDrive); + // vision = new VROOOOM(arm, elevator, motorClaw, swerveDrive); // this.alliance = DriverStation.getAlliance(); initAutoChoosers(); @@ -211,6 +216,7 @@ private void configureBindings() { // driverController.PS().onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new Pose2d()))); // driverController.R1().whileTrue(new TurnToAngle(180, swerveDrive)); // Replaced with turn to angles in the drive command // driverController.L1().whileTrue(new TurnToAngle(0, swerveDrive)); + driverController.L1().whileTrue(new ToNearestGridDebug(swerveDrive, ps)); driverController.triangle().whileTrue(new TheGreatBalancingAct(swerveDrive)); driverController.circle() @@ -221,13 +227,13 @@ private void configureBindings() { // driverController.R2().whileTrue(new Dodge(swerveDrive, -driverController.getLeftY(), driverController.getLeftX(), false)); // ====== Vision Bindings ====== - driverController.cross().whileTrue(vision.VisionPickupOnSubstation(OBJECT_TYPE.CONE)) - .onFalse(Commands.runOnce(swerveDrive::stopModules, swerveDrive)); + // driverController.cross().whileTrue(vision.VisionPickupOnSubstation(OBJECT_TYPE.CONE)) + // .onFalse(Commands.runOnce(swerveDrive::stopModules, swerveDrive)); // driverController.R2().whileTrue(vision.VisionPickupOnSubstation(OBJECT_TYPE.CUBE)) // .onFalse(Commands.runOnce(swerveDrive::stopModules, swerveDrive)); - operatorController.L2().whileTrue(vision.VisionPickupOnGround(OBJECT_TYPE.CUBE)); - operatorController.R2().whileTrue(vision.VisionScore(OBJECT_TYPE.CUBE, SCORE_POS.HIGH)); + // operatorController.L2().whileTrue(vision.VisionPickupOnGround(OBJECT_TYPE.CUBE)); + // operatorController.R2().whileTrue(vision.VisionScore(OBJECT_TYPE.CUBE, SCORE_POS.HIGH)); //operatorController.L2().onTrue(vision.updateCurrentGameObjects(OBJECT_TYPE.CONE)); //operatorController.R2().onTrue(vision.updateCurrentGameObject(OBJECT_TYPE.CUBE)); @@ -286,21 +292,21 @@ private void initAutoChoosers() { // Has and uses alliance parameter. // autoChooser.addOption("Vision Two Piece Smooth (Alliance)", () -> VisionAutos.zoomTwoPieceAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); // autoChooser.addOption("Vision Two Piece Cable (Alliance)", () -> VisionAutos.cableZoomTwoPieceAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); - autoChooser.addOption("Smooth Low Cube Auto", () -> VisionAllLowAuto.ThreeCubesAutoFast(swerveDrive, vision, arm, elevator, motorClaw, alliance)); - autoChooser.addOption("Cable Low Cube Auto", () -> VisionCableSideAuto.LowAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); - autoChooser.addOption("Cable High Cube Auto", () -> VisionCableSideAuto.HighAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); + // autoChooser.addOption("Smooth Low Cube Auto", () -> VisionAllLowAuto.ThreeCubesAutoFast(swerveDrive, vision, arm, elevator, motorClaw, alliance)); + // autoChooser.addOption("Cable Low Cube Auto", () -> VisionCableSideAuto.LowAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); + // autoChooser.addOption("Cable High Cube Auto", () -> VisionCableSideAuto.HighAuto(swerveDrive, vision, arm, elevator, motorClaw, alliance)); // Have alliance parameter but do not use it. - autoChooser.addOption("LAR Auto", () -> SwerveAutos.preloadChargeAuto(swerveDrive, arm, elevator, motorClaw, StartPosition.MIDDLE, SCORE_POS.HIGH, 0, false, alliance)); - autoChooser.addOption("Preload Taxi Auto", () -> SwerveAutos.preloadBackwardAuto(swerveDrive, arm, elevator, motorClaw, SCORE_POS.HIGH, alliance)); + // autoChooser.addOption("LAR Auto", () -> SwerveAutos.preloadChargeAuto(swerveDrive, arm, elevator, motorClaw, StartPosition.MIDDLE, SCORE_POS.HIGH, 0, false, alliance)); + // autoChooser.addOption("Preload Taxi Auto", () -> SwerveAutos.preloadBackwardAuto(swerveDrive, arm, elevator, motorClaw, SCORE_POS.HIGH, alliance)); allianceChooser.setDefaultOption("Red", Alliance.Red); allianceChooser.addOption("Red", Alliance.Red); allianceChooser.addOption("Blue", Alliance.Blue); autosTab.add("Alliance", allianceChooser); - autosTab.addString("Selected Score Position", () -> scorePos.toString()); + // autosTab.addString("Selected Score Position", () -> scorePos.toString()); } public void initShuffleboard() { @@ -316,7 +322,7 @@ public void initShuffleboard() { motorClaw.initShuffleboard(loggingLevel); swerveDrive.initShuffleboard(loggingLevel); swerveDrive.initModuleShuffleboard(loggingLevel); - vision.initShuffleboard(loggingLevel); + // vision.initShuffleboard(loggingLevel); } public void reportAllToSmartDashboard() { @@ -326,7 +332,7 @@ public void reportAllToSmartDashboard() { motorClaw.reportToSmartDashboard(loggingLevel); arm.reportToSmartDashboard(loggingLevel); elevator.reportToSmartDashboard(loggingLevel); - vision.reportToSmartDashboard(loggingLevel); + // vision.reportToSmartDashboard(loggingLevel); swerveDrive.reportToSmartDashboard(loggingLevel); swerveDrive.reportModulesToSmartDashboard(loggingLevel); } diff --git a/src/main/java/frc/robot/commands/SwerveAutos.java b/src/main/java/frc/robot/commands/SwerveAutos.java index 598db07..0b2df67 100644 --- a/src/main/java/frc/robot/commands/SwerveAutos.java +++ b/src/main/java/frc/robot/commands/SwerveAutos.java @@ -1,511 +1,508 @@ -package frc.robot.commands; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ClawConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.SwerveAutoConstants; -import frc.robot.Constants.SwerveDriveConstants; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.claw.MotorClaw; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.vision.VROOOOM; -import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; -import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; - -import static frc.robot.Constants.SwerveAutoConstants.*; -import static edu.wpi.first.wpilibj2.command.Commands.*; - -import java.util.List; - -public class SwerveAutos { - public enum StartPosition { - LEFT, - RIGHT, - MIDDLE - } - - public static CommandBase driveBackwardAuto(SwerveDrivetrain swerveDrive) { - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); +// package frc.robot.commands; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import frc.robot.Constants.ArmConstants; +// import frc.robot.Constants.ClawConstants; +// import frc.robot.Constants.ElevatorConstants; +// import frc.robot.Constants.SwerveAutoConstants; +// import frc.robot.Constants.SwerveDriveConstants; +// import frc.robot.subsystems.Arm; +// import frc.robot.subsystems.Elevator; +// import frc.robot.subsystems.claw.MotorClaw; +// import frc.robot.subsystems.swerve.SwerveDrivetrain; + +// import static frc.robot.Constants.SwerveAutoConstants.*; +// import static edu.wpi.first.wpilibj2.command.Commands.*; + +// import java.util.List; + +// public class SwerveAutos { +// public enum StartPosition { +// LEFT, +// RIGHT, +// MIDDLE +// } + +// public static CommandBase driveBackwardAuto(SwerveDrivetrain swerveDrive) { +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - Trajectory driveForward = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-2, 0.02)), - new Pose2d(-4.5, 0.045, Rotation2d.fromDegrees(180)), - trajectoryConfig); +// Trajectory driveForward = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-2, 0.02)), +// new Pose2d(-4.5, 0.045, Rotation2d.fromDegrees(180)), +// trajectoryConfig); - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); - SwerveControllerCommand driveForwardCommand = new SwerveControllerCommand( - driveForward, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand driveForwardCommand = new SwerveControllerCommand( +// driveForward, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - return sequence( - runOnce(() -> swerveDrive.setPoseMeters(driveForward.getInitialPose())), - driveForwardCommand, - runOnce(swerveDrive::stopModules) - ); - } - - public static CommandBase driveBackwardLeftAuto(SwerveDrivetrain swerveDrive) { - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); +// return sequence( +// runOnce(() -> swerveDrive.setPoseMeters(driveForward.getInitialPose())), +// driveForwardCommand, +// runOnce(swerveDrive::stopModules) +// ); +// } + +// public static CommandBase driveBackwardLeftAuto(SwerveDrivetrain swerveDrive) { +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - Trajectory driveForward = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-2, 0)), - new Pose2d(-4, -1, Rotation2d.fromDegrees(180)), - trajectoryConfig); +// Trajectory driveForward = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-2, 0)), +// new Pose2d(-4, -1, Rotation2d.fromDegrees(180)), +// trajectoryConfig); - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); - SwerveControllerCommand driveForwardCommand = new SwerveControllerCommand( - driveForward, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand driveForwardCommand = new SwerveControllerCommand( +// driveForward, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - return sequence( - runOnce(() -> swerveDrive.setPoseMeters(driveForward.getInitialPose())), - driveForwardCommand, - runOnce(swerveDrive::stopModules) - ); - } - - /** - * Start with the front left swerve module aligned with the charging station's edge - * in the x axis and around 8 inches to the right in the y axis - * @param swerveDrive - * @return - */ - public static CommandBase pickupAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, Alliance alliance, SCORE_POS scorePosition) { - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); +// return sequence( +// runOnce(() -> swerveDrive.setPoseMeters(driveForward.getInitialPose())), +// driveForwardCommand, +// runOnce(swerveDrive::stopModules) +// ); +// } + +// /** +// * Start with the front left swerve module aligned with the charging station's edge +// * in the x axis and around 8 inches to the right in the y axis +// * @param swerveDrive +// * @return +// */ +// public static CommandBase pickupAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, Alliance alliance, SCORE_POS scorePosition) { +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - double pickupXDistance = 0; - double pickupYDistance = 0; - - if (alliance == Alliance.Red) { - if (position == StartPosition.RIGHT) position = StartPosition.LEFT; - if (position == StartPosition.LEFT) position = StartPosition.RIGHT; - } - - - int elevatorPos = ElevatorConstants.kElevatorScoreMid; - switch (scorePosition) { - case LOW: - elevatorPos = ElevatorConstants.kElevatorStow; - break; - case MID: - elevatorPos = ElevatorConstants.kElevatorScoreMid; - break; - case HIGH: - elevatorPos = ElevatorConstants.kElevatorScoreHighCube; - break; - } - - final int elevatorPosFinal = elevatorPos; - - - switch (position) { - // Reversed because gyro starts reversed - case RIGHT: - pickupXDistance = -4.58; - pickupYDistance = -0.08; - break; - case LEFT: - pickupXDistance = -4.58; - pickupYDistance = 0.17; - break; - case MIDDLE: - pickupXDistance = 5; // TODO: Measure IRL - pickupYDistance = -0.5; - break; - } +// double pickupXDistance = 0; +// double pickupYDistance = 0; + +// if (alliance == Alliance.Red) { +// if (position == StartPosition.RIGHT) position = StartPosition.LEFT; +// if (position == StartPosition.LEFT) position = StartPosition.RIGHT; +// } + + +// int elevatorPos = ElevatorConstants.kElevatorScoreMid; +// switch (scorePosition) { +// case LOW: +// elevatorPos = ElevatorConstants.kElevatorStow; +// break; +// case MID: +// elevatorPos = ElevatorConstants.kElevatorScoreMid; +// break; +// case HIGH: +// elevatorPos = ElevatorConstants.kElevatorScoreHighCube; +// break; +// } + +// final int elevatorPosFinal = elevatorPos; + + +// switch (position) { +// // Reversed because gyro starts reversed +// case RIGHT: +// pickupXDistance = -4.58; +// pickupYDistance = -0.08; +// break; +// case LEFT: +// pickupXDistance = -4.58; +// pickupYDistance = 0.17; +// break; +// case MIDDLE: +// pickupXDistance = 5; // TODO: Measure IRL +// pickupYDistance = -0.5; +// break; +// } - // if (alliance == Alliance.Red) { - // pickupYDistance *= -1; - // } +// // if (alliance == Alliance.Red) { +// // pickupYDistance *= -1; +// // } - Trajectory scoreToPickup = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(pickupXDistance / 2, pickupYDistance / 2)), - new Pose2d(pickupXDistance, pickupYDistance, Rotation2d.fromDegrees(180)), - trajectoryConfig); - - Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( - new Pose2d(pickupXDistance, pickupYDistance, new Rotation2d(180)), - List.of( - new Translation2d(pickupXDistance / 2, pickupYDistance / 2)), - new Pose2d(0, 0, Rotation2d.fromDegrees(180)), - trajectoryConfig); - - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); +// Trajectory scoreToPickup = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(pickupXDistance / 2, pickupYDistance / 2)), +// new Pose2d(pickupXDistance, pickupYDistance, Rotation2d.fromDegrees(180)), +// trajectoryConfig); + +// Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( +// new Pose2d(pickupXDistance, pickupYDistance, new Rotation2d(180)), +// List.of( +// new Translation2d(pickupXDistance / 2, pickupYDistance / 2)), +// new Pose2d(0, 0, Rotation2d.fromDegrees(180)), +// trajectoryConfig); + +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); - SwerveControllerCommand scoreToPickupCommand = new SwerveControllerCommand( - scoreToPickup, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand scoreToPickupCommand = new SwerveControllerCommand( +// scoreToPickup, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - SwerveControllerCommand pickupToScoreCommand = new SwerveControllerCommand( - pickupToScore, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand pickupToScoreCommand = new SwerveControllerCommand( +// pickupToScore, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - return deadline( - sequence( - parallel( - runOnce(() -> SmartDashboard.putString("Stage", "Start")), - runOnce(() -> swerveDrive.resetOdometry(scoreToPickup.getInitialPose())), - runOnce(() -> swerveDrive.stopModules()), - scoreToPickupCommand - ), +// return deadline( +// sequence( +// parallel( +// runOnce(() -> SmartDashboard.putString("Stage", "Start")), +// runOnce(() -> swerveDrive.resetOdometry(scoreToPickup.getInitialPose())), +// runOnce(() -> swerveDrive.stopModules()), +// scoreToPickupCommand +// ), - deadline( - waitSeconds(2), - runOnce(() -> swerveDrive.stopModules()), - runOnce(() -> SmartDashboard.putString("Stage", "Ground")), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - ), - - waitSeconds(0.75), - claw.intake(), - waitSeconds(0.25), - - deadline( - waitSeconds(2), - runOnce(() -> SmartDashboard.putString("Stage", "Stow 2")), - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - waitUntil(arm.atTargetPosition) - ), - - pickupToScoreCommand, - - deadline( - waitSeconds(2), - runOnce(() -> swerveDrive.stopModules()), - runOnce(() -> SmartDashboard.putString("Stage", "Score 2")), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ), - sequence( - runOnce(() -> elevator.setTargetTicks(elevatorPosFinal)), - waitSeconds(0.5), - waitUntil(elevator.atTargetPosition) - ) - ), - - waitSeconds(0.75), - claw.outtake(), - waitSeconds(0.25), - - deadline( - waitSeconds(2), - runOnce(() -> SmartDashboard.putString("Stage", "Stow 3")), - sequence( - runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)), - waitSeconds(0.5), - waitUntil(elevator.atTargetPosition) - ), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - )) - ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } - - public static CommandBase preloadAuto(Arm arm, Elevator elevator, MotorClaw claw, SCORE_POS scorePos) { - int elevatorPos = ElevatorConstants.kElevatorScoreMid; - int armPos = ArmConstants.kArmScoreCubeMid; - - switch (scorePos) { - case LOW: - elevatorPos = ElevatorConstants.kElevatorStow; - armPos = ArmConstants.kArmScore; // Not real(ly accurate) - break; - case MID: - elevatorPos = ElevatorConstants.kElevatorScoreMid; - armPos = ArmConstants.kArmScore; - break; - case HIGH: - elevatorPos = ElevatorConstants.kElevatorScoreHigh; //ElevatorConstants.kElevatorScoreHighCube; // Note: this is a bandaid - armPos = ArmConstants.kArmScore; - break; - } - - final int elevatorPosFinal = elevatorPos; - final int armPosFinal = armPos; - - return race( - waitSeconds(5), - sequence( - claw.intake(), - deadline( - waitSeconds(2), - runOnce(() -> SmartDashboard.putString("Stage", "Score")), - sequence( - runOnce(() -> arm.setTargetTicks(armPosFinal)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ), - sequence( - waitSeconds(0.5), - runOnce(() -> elevator.setTargetTicks(elevatorPosFinal)), - waitSeconds(0.5), - waitUntil(elevator.atTargetPosition) - ) - ), - waitSeconds(0.5), - // claw.outtake(), - claw.setPower(.3), - waitSeconds(0.5), - claw.setPowerZero(), +// deadline( +// waitSeconds(2), +// runOnce(() -> swerveDrive.stopModules()), +// runOnce(() -> SmartDashboard.putString("Stage", "Ground")), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// ), + +// waitSeconds(0.75), +// claw.intake(), +// waitSeconds(0.25), + +// deadline( +// waitSeconds(2), +// runOnce(() -> SmartDashboard.putString("Stage", "Stow 2")), +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// waitUntil(arm.atTargetPosition) +// ), + +// pickupToScoreCommand, + +// deadline( +// waitSeconds(2), +// runOnce(() -> swerveDrive.stopModules()), +// runOnce(() -> SmartDashboard.putString("Stage", "Score 2")), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ), +// sequence( +// runOnce(() -> elevator.setTargetTicks(elevatorPosFinal)), +// waitSeconds(0.5), +// waitUntil(elevator.atTargetPosition) +// ) +// ), + +// waitSeconds(0.75), +// claw.outtake(), +// waitSeconds(0.25), + +// deadline( +// waitSeconds(2), +// runOnce(() -> SmartDashboard.putString("Stage", "Stow 3")), +// sequence( +// runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)), +// waitSeconds(0.5), +// waitUntil(elevator.atTargetPosition) +// ), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// )) +// ), +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } + +// public static CommandBase preloadAuto(Arm arm, Elevator elevator, MotorClaw claw, SCORE_POS scorePos) { +// int elevatorPos = ElevatorConstants.kElevatorScoreMid; +// int armPos = ArmConstants.kArmScoreCubeMid; + +// switch (scorePos) { +// case LOW: +// elevatorPos = ElevatorConstants.kElevatorStow; +// armPos = ArmConstants.kArmScore; // Not real(ly accurate) +// break; +// case MID: +// elevatorPos = ElevatorConstants.kElevatorScoreMid; +// armPos = ArmConstants.kArmScore; +// break; +// case HIGH: +// elevatorPos = ElevatorConstants.kElevatorScoreHigh; //ElevatorConstants.kElevatorScoreHighCube; // Note: this is a bandaid +// armPos = ArmConstants.kArmScore; +// break; +// } + +// final int elevatorPosFinal = elevatorPos; +// final int armPosFinal = armPos; + +// return race( +// waitSeconds(5), +// sequence( +// claw.intake(), +// deadline( +// waitSeconds(2), +// runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// sequence( +// runOnce(() -> arm.setTargetTicks(armPosFinal)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ), +// sequence( +// waitSeconds(0.5), +// runOnce(() -> elevator.setTargetTicks(elevatorPosFinal)), +// waitSeconds(0.5), +// waitUntil(elevator.atTargetPosition) +// ) +// ), +// waitSeconds(0.5), +// // claw.outtake(), +// claw.setPower(.3), +// waitSeconds(0.5), +// claw.setPowerZero(), - deadline( - waitSeconds(0.5), - runOnce(() -> SmartDashboard.putString("Stage", "Stow")), - sequence( - runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)), - waitSeconds(0.5), - waitUntil(elevator.atTargetPosition) - ), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmSubstation)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - ) - ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } - - public static CommandBase pickupChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, Alliance alliance, SCORE_POS scorePos) { - return sequence( - - pickupAuto(swerveDrive, arm, elevator, claw, position, alliance, scorePos), - chargeAuto(swerveDrive, position, alliance, 0, false)); - } - - public static CommandBase pickupBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, SCORE_POS scorePos, Alliance alliance) { - return sequence( - pickupAuto(swerveDrive, arm, elevator, claw, position, alliance, scorePos), - driveBackwardAuto(swerveDrive) - ); - } - - public static CommandBase preloadChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, double waitTime, boolean goAround, Alliance alliance) { - return sequence( - ChargeAutos.preloadHigh(arm, elevator, claw), - deadline( - chargeAuto(swerveDrive, startPos, alliance, waitTime, goAround), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ) - ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); - } - - public static CommandBase preloadBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, SCORE_POS scorePos, Alliance alliance) { - return sequence( - ChargeAutos.preloadHigh(arm, elevator, claw), - driveBackwardAuto(swerveDrive) - ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); - } +// deadline( +// waitSeconds(0.5), +// runOnce(() -> SmartDashboard.putString("Stage", "Stow")), +// sequence( +// runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)), +// waitSeconds(0.5), +// waitUntil(elevator.atTargetPosition) +// ), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmSubstation)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } + +// public static CommandBase pickupChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, Alliance alliance, SCORE_POS scorePos) { +// return sequence( + +// pickupAuto(swerveDrive, arm, elevator, claw, position, alliance, scorePos), +// chargeAuto(swerveDrive, position, alliance, 0, false)); +// } + +// public static CommandBase pickupBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition position, SCORE_POS scorePos, Alliance alliance) { +// return sequence( +// pickupAuto(swerveDrive, arm, elevator, claw, position, alliance, scorePos), +// driveBackwardAuto(swerveDrive) +// ); +// } + +// public static CommandBase preloadChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, double waitTime, boolean goAround, Alliance alliance) { +// return sequence( +// ChargeAutos.preloadHigh(arm, elevator, claw), +// deadline( +// chargeAuto(swerveDrive, startPos, alliance, waitTime, goAround), +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ) +// ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); +// } + +// public static CommandBase preloadBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, SCORE_POS scorePos, Alliance alliance) { +// return sequence( +// ChargeAutos.preloadHigh(arm, elevator, claw), +// driveBackwardAuto(swerveDrive) +// ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); +// } - public static CommandBase preloadBackwardLeftAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { - return sequence( - preloadAuto(arm, elevator, claw, scorePos), - driveBackwardLeftAuto(swerveDrive) - ); - } - - public static CommandBase twoPieceAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { - return sequence( - preloadAuto(arm, elevator, claw, scorePos), - pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos) - ); - } - - public static CommandBase twoPieceChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, double waitTime, boolean goAround, Alliance alliance) { - return sequence( - preloadAuto(arm, elevator, claw, scorePos), - pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos), - chargeAuto(swerveDrive, startPos, alliance, waitTime, goAround) - ); - } - - public static CommandBase twoPieceBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { - return sequence( - preloadAuto(arm, elevator, claw, scorePos), - pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos), - driveBackwardAuto(swerveDrive) - ); - } +// public static CommandBase preloadBackwardLeftAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { +// return sequence( +// preloadAuto(arm, elevator, claw, scorePos), +// driveBackwardLeftAuto(swerveDrive) +// ); +// } + +// public static CommandBase twoPieceAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { +// return sequence( +// preloadAuto(arm, elevator, claw, scorePos), +// pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos) +// ); +// } + +// public static CommandBase twoPieceChargeAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, double waitTime, boolean goAround, Alliance alliance) { +// return sequence( +// preloadAuto(arm, elevator, claw, scorePos), +// pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos), +// chargeAuto(swerveDrive, startPos, alliance, waitTime, goAround) +// ); +// } + +// public static CommandBase twoPieceBackwardAuto(SwerveDrivetrain swerveDrive, Arm arm, Elevator elevator, MotorClaw claw, StartPosition startPos, SCORE_POS scorePos, Alliance alliance) { +// return sequence( +// preloadAuto(arm, elevator, claw, scorePos), +// pickupAuto(swerveDrive, arm, elevator, claw, startPos, alliance, scorePos), +// driveBackwardAuto(swerveDrive) +// ); +// } - /** - * Start with the swerve drive facing the driver at either the rightmost cone grid, the leftmost cone grid, or directly in front of the charging station (middle) - * @param swerveDrive - * @return Command to reset odometry run auto to go onto charging station then run balancing auto - */ - public static CommandBase chargeAuto(SwerveDrivetrain swerveDrive, StartPosition startPos, Alliance alliance, double waitTime, boolean goAround) { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - kChargeSpeedMetersPerSecond, kChargeAccelerationMetersPerSecondSquared); +// /** +// * Start with the swerve drive facing the driver at either the rightmost cone grid, the leftmost cone grid, or directly in front of the charging station (middle) +// * @param swerveDrive +// * @return Command to reset odometry run auto to go onto charging station then run balancing auto +// */ +// public static CommandBase chargeAuto(SwerveDrivetrain swerveDrive, StartPosition startPos, Alliance alliance, double waitTime, boolean goAround) { +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// kChargeSpeedMetersPerSecond, kChargeAccelerationMetersPerSecondSquared); - double yTranslation = 0; - double yOvershoot = 0; - - switch (startPos) { - case LEFT: - yTranslation = -2.75; - yOvershoot = -2.75; - break; - case RIGHT: - yTranslation = 2.75; - yOvershoot = 2.75; - break; - case MIDDLE: - break; - } - - // if (alliance == Alliance.Red) { - // yTranslation *= -1; - // yOvershoot *= -1; - // } - - Trajectory trajectory; +// double yTranslation = 0; +// double yOvershoot = 0; + +// switch (startPos) { +// case LEFT: +// yTranslation = -2.75; +// yOvershoot = -2.75; +// break; +// case RIGHT: +// yTranslation = 2.75; +// yOvershoot = 2.75; +// break; +// case MIDDLE: +// break; +// } + +// // if (alliance == Alliance.Red) { +// // yTranslation *= -1; +// // yOvershoot *= -1; +// // } + +// Trajectory trajectory; - if (goAround && startPos == StartPosition.MIDDLE) { - trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-4, 0.01), - new Translation2d(-5, yTranslation / 4), - new Translation2d(-4, yTranslation + 0.01)), - new Pose2d(-0.5, yTranslation - 0.01, Rotation2d.fromDegrees(0)), - trajectoryConfig); - } else if (!goAround) { - trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(-0.125, 0, new Rotation2d(0)), - List.of( - new Translation2d(-0.25, 0), - new Translation2d(-0.25, yOvershoot)), - new Pose2d(-3, yTranslation - 0.01, Rotation2d.fromDegrees(0)), // -2 - trajectoryConfig); - } else { - trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-4, 0.01), - new Translation2d(-4.5, yTranslation / 4), - new Translation2d(-4, yTranslation + 0.01)), - new Pose2d(-0.5, yTranslation - 0.01, Rotation2d.fromDegrees(0)), - trajectoryConfig); - } - - - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - SwerveControllerCommand autoCommand = new SwerveControllerCommand( - trajectory, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// if (goAround && startPos == StartPosition.MIDDLE) { +// trajectory = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-4, 0.01), +// new Translation2d(-5, yTranslation / 4), +// new Translation2d(-4, yTranslation + 0.01)), +// new Pose2d(-0.5, yTranslation - 0.01, Rotation2d.fromDegrees(0)), +// trajectoryConfig); +// } else if (!goAround) { +// trajectory = TrajectoryGenerator.generateTrajectory( +// new Pose2d(-0.125, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-0.25, 0), +// new Translation2d(-0.25, yOvershoot)), +// new Pose2d(-3, yTranslation - 0.01, Rotation2d.fromDegrees(0)), // -2 +// trajectoryConfig); +// } else { +// trajectory = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-4, 0.01), +// new Translation2d(-4.5, yTranslation / 4), +// new Translation2d(-4, yTranslation + 0.01)), +// new Pose2d(-0.5, yTranslation - 0.01, Rotation2d.fromDegrees(0)), +// trajectoryConfig); +// } + + +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// SwerveControllerCommand autoCommand = new SwerveControllerCommand( +// trajectory, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - return sequence( - runOnce(() -> swerveDrive.resetOdometry(trajectory.getInitialPose())), - waitSeconds(waitTime), - autoCommand, - new TheGreatBalancingAct(swerveDrive) - // new TowSwerve(swerveDrive) - ); - } +// return sequence( +// runOnce(() -> swerveDrive.resetOdometry(trajectory.getInitialPose())), +// waitSeconds(waitTime), +// autoCommand, +// new TheGreatBalancingAct(swerveDrive) +// // new TowSwerve(swerveDrive) +// ); +// } - /** - * Start with the front left swerve module aligned with the charging station's edge - * in the x axis and around 8 inches to the right in the y axis - * - * Use as a backup only - * - * @param swerveDrive - * @return Command to reset odometry run auto to go onto charging station then run balancing auto - */ - public static CommandBase backupChargeAuto(SwerveDrivetrain swerveDrive) { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); +// /** +// * Start with the front left swerve module aligned with the charging station's edge +// * in the x axis and around 8 inches to the right in the y axis +// * +// * Use as a backup only +// * +// * @param swerveDrive +// * @return Command to reset odometry run auto to go onto charging station then run balancing auto +// */ +// public static CommandBase backupChargeAuto(SwerveDrivetrain swerveDrive) { +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-0.75, 0), // -0.75 - new Translation2d(-0.75, -1.25)), - new Pose2d(1, -1.75, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - - SwerveControllerCommand autoCommand = new SwerveControllerCommand( - trajectory, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// Trajectory trajectory = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-0.75, 0), // -0.75 +// new Translation2d(-0.75, -1.25)), +// new Pose2d(1, -1.75, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); + +// SwerveControllerCommand autoCommand = new SwerveControllerCommand( +// trajectory, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - return sequence( - runOnce(() -> swerveDrive.resetOdometry(trajectory.getInitialPose())), - deadline( - waitSeconds(3), - autoCommand - ), - new TheGreatBalancingAct(swerveDrive, SwerveAutoConstants.kPBalancing + 0.05, SwerveAutoConstants.kIBalancing, SwerveAutoConstants.kDBalancing) - // new TowSwerve(swerveDrive) - ); - } -} +// return sequence( +// runOnce(() -> swerveDrive.resetOdometry(trajectory.getInitialPose())), +// deadline( +// waitSeconds(3), +// autoCommand +// ), +// new TheGreatBalancingAct(swerveDrive, SwerveAutoConstants.kPBalancing + 0.05, SwerveAutoConstants.kIBalancing, SwerveAutoConstants.kDBalancing) +// // new TowSwerve(swerveDrive) +// ); +// } +// } diff --git a/src/main/java/frc/robot/commands/VisionAllLowAuto.java b/src/main/java/frc/robot/commands/VisionAllLowAuto.java index cc60089..fdc0649 100644 --- a/src/main/java/frc/robot/commands/VisionAllLowAuto.java +++ b/src/main/java/frc/robot/commands/VisionAllLowAuto.java @@ -1,206 +1,206 @@ -package frc.robot.commands; - -import java.util.List; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.SwerveAutoConstants; -import frc.robot.Constants.SwerveDriveConstants; -import frc.robot.commands.SwerveAutos.StartPosition; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.claw.MotorClaw; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.vision.VROOOOM; -import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; -import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; - -import static frc.robot.Constants.SwerveAutoConstants.*; -import static edu.wpi.first.wpilibj2.command.Commands.*; - -// Trajectory must stop ~32 inches in front of the cone -// Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station -// All score positions are assumed to be high for this file right now -// Positive Y = left, positive X = towards drivers, rotation positive is clockwise - -public class VisionAllLowAuto { +// package frc.robot.commands; + +// import java.util.List; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.RunCommand; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants.ArmConstants; +// import frc.robot.Constants.ElevatorConstants; +// import frc.robot.Constants.SwerveAutoConstants; +// import frc.robot.Constants.SwerveDriveConstants; +// import frc.robot.commands.SwerveAutos.StartPosition; +// import frc.robot.subsystems.Arm; +// import frc.robot.subsystems.Elevator; +// import frc.robot.subsystems.claw.MotorClaw; +// import frc.robot.subsystems.swerve.SwerveDrivetrain; +// import frc.robot.subsystems.vision.VROOOOM; +// import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; +// import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; + +// import static frc.robot.Constants.SwerveAutoConstants.*; +// import static edu.wpi.first.wpilibj2.command.Commands.*; + +// // Trajectory must stop ~32 inches in front of the cone +// // Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station +// // All score positions are assumed to be high for this file right now +// // Positive Y = left, positive X = towards drivers, rotation positive is clockwise + +// public class VisionAllLowAuto { - public static CommandBase ThreeCubesAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = - new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); - - double zoooomAllianceThingy = 1.0; - if (alliance == Alliance.Red) { - zoooomAllianceThingy = -1.0; - } +// public static CommandBase ThreeCubesAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = +// new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); + +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); + +// double zoooomAllianceThingy = 1.0; +// if (alliance == Alliance.Red) { +// zoooomAllianceThingy = -1.0; +// } - //trajectory stuff - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.1, 0 * zoooomAllianceThingy), - new Translation2d(-0.1, 0 * zoooomAllianceThingy), // push the cube to hybrid zone - new Translation2d(0.18, 0.18 * zoooomAllianceThingy) - //new Translation2d(-1.8, -0.4) - ), - new Pose2d(4.4, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(3.6, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // TODO: Run with and without this line - new Pose2d(1.5, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, 0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.5, 0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); - - Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( - List.of( - //new Pose2d(0.2, 1.0, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // tested, somehow the trajectory 0,0 was shifted - new Pose2d(0, -0.3 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(4.2, -0.3 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - // new Pose2d(3.6, 2.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); +// //trajectory stuff + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.1, 0 * zoooomAllianceThingy), +// new Translation2d(-0.1, 0 * zoooomAllianceThingy), // push the cube to hybrid zone +// new Translation2d(0.18, 0.18 * zoooomAllianceThingy) +// //new Translation2d(-1.8, -0.4) +// ), +// new Pose2d(4.4, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(3.6, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // TODO: Run with and without this line +// new Pose2d(1.5, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, 0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.5, 0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); + +// Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( +// List.of( +// //new Pose2d(0.2, 1.0, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, 0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // tested, somehow the trajectory 0,0 was shifted +// new Pose2d(0, -0.3 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(4.2, -0.3 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// // new Pose2d(3.6, 2.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( - zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - - return Commands.race( - Commands.waitSeconds(15), // TODO DEL - //init - Commands.sequence( - Commands.parallel( - Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())) - //claw.setPower(-0.5) for shoot out cone - ), - //Commands.waitSeconds(0.1),for shoot out cone - - //trajectory to cube - Commands.parallel( - zoooomToCubeCommand, - //claw.setPower(0),for shoot out cone +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( +// zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + + +// return Commands.race( +// Commands.waitSeconds(15), // TODO DEL +// //init +// Commands.sequence( +// Commands.parallel( +// Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())) +// //claw.setPower(-0.5) for shoot out cone +// ), +// //Commands.waitSeconds(0.1),for shoot out cone + +// //trajectory to cube +// Commands.parallel( +// zoooomToCubeCommand, +// //claw.setPower(0),for shoot out cone - //Drop arm to half way - Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands - Commands.waitSeconds(0.5), - sequence( - runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickup) / 2)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - ), +// //Drop arm to half way +// Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands +// Commands.waitSeconds(0.5), +// sequence( +// runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickup) / 2)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// ), - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) - ), - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - Commands.runOnce(() -> swerveDrive.stopModules()), - - Commands.race( - new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(20) // kill this auto - // TODO need add protection here!!!!!! - ), - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - Commands.runOnce(() -> swerveDrive.stopModules()), - - claw.setPower(-0.35), - Commands.deadline( - Commands.waitSeconds(0.5), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - ), - // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.20), - - Commands.deadline( - Commands.waitSeconds(0.5), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - ), +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) +// ), +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// Commands.runOnce(() -> swerveDrive.stopModules()), + +// Commands.race( +// new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(20) // kill this auto +// // TODO need add protection here!!!!!! +// ), +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// Commands.runOnce(() -> swerveDrive.stopModules()), + +// claw.setPower(-0.35), +// Commands.deadline( +// Commands.waitSeconds(0.5), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// ), +// // Close claw/stop claw intake rollers/low background rolling to keep control of game piece +// claw.setPower(-0.20), + +// Commands.deadline( +// Commands.waitSeconds(0.5), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// ), - new TurnToAngle(-179.9, swerveDrive), +// new TurnToAngle(-179.9, swerveDrive), - cubeToZoooomCommand, - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - Commands.runOnce(() -> swerveDrive.stopModules()), +// cubeToZoooomCommand, +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// Commands.runOnce(() -> swerveDrive.stopModules()), - // TODO: apriltag? +// // TODO: apriltag? - claw.setPower(0.3), +// claw.setPower(0.3), - Commands.parallel( - Commands.sequence( - waitSeconds(0.2), - claw.setPower(0) - ), - zoooomPartTwoCommand, - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) - ), - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - Commands.runOnce(() -> swerveDrive.stopModules()), +// Commands.parallel( +// Commands.sequence( +// waitSeconds(0.2), +// claw.setPower(0) +// ), +// zoooomPartTwoCommand, +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) +// ), +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// Commands.runOnce(() -> swerveDrive.stopModules()), - new TurnToAngle(-45, swerveDrive), +// new TurnToAngle(-45, swerveDrive), - /* uncommment it if we have time to run, otherwise be safe to stop here - Commands.race( - new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(20) - // TODO need add protection here!!!!!! - ),*/ - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - Commands.runOnce(() -> swerveDrive.stopModules()) - ), +// /* uncommment it if we have time to run, otherwise be safe to stop here +// Commands.race( +// new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(20) +// // TODO need add protection here!!!!!! +// ),*/ +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// Commands.runOnce(() -> swerveDrive.stopModules()) +// ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) - ); - } +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) +// ); +// } @@ -230,150 +230,150 @@ public static CommandBase ThreeCubesAuto(SwerveDrivetrain swerveDrive, VROOOOM v - public static CommandBase ThreeCubesAutoFast(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = - new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// public static CommandBase ThreeCubesAutoFast(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = +// new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); - double zoooomAllianceThingy = 1.0; - if (alliance == Alliance.Red) { - zoooomAllianceThingy = -1.0; - } +// double zoooomAllianceThingy = 1.0; +// if (alliance == Alliance.Red) { +// zoooomAllianceThingy = -1.0; +// } - //trajectory stuff - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.18, 0 * zoooomAllianceThingy), - new Translation2d(-0.18, 0 * zoooomAllianceThingy), // push the cube to hybrid zone - new Translation2d(0, 0 * zoooomAllianceThingy), - new Translation2d(0.18, 0.18 * zoooomAllianceThingy) - //new Translation2d(-1.8, -0.4) - ), - new Pose2d(4.3, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(4.3, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(3.8, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(1.5, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.3, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.3, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.45, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); - - Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( - List.of( - //new Pose2d(0.2, 1.0, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, 0 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // tested, somehow the trajectory 0,0 was shifted - new Pose2d(0.5, -0.5 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(4.2, -0.5 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - // new Pose2d(3.6, 2.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); +// //trajectory stuff + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.18, 0 * zoooomAllianceThingy), +// new Translation2d(-0.18, 0 * zoooomAllianceThingy), // push the cube to hybrid zone +// new Translation2d(0, 0 * zoooomAllianceThingy), +// new Translation2d(0.18, 0.18 * zoooomAllianceThingy) +// //new Translation2d(-1.8, -0.4) +// ), +// new Pose2d(4.3, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(4.3, 0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(3.8, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(1.5, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.3, -0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.3, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.45, 0.4 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); + +// Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( +// List.of( +// //new Pose2d(0.2, 1.0, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, 0 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), // tested, somehow the trajectory 0,0 was shifted +// new Pose2d(0.5, -0.5 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(4.2, -0.5 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// // new Pose2d(3.6, 2.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( - zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - - return Commands.race( - Commands.waitSeconds(15), // TODO DEL - //init - Commands.sequence( - - Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), - - //trajectory to cube - Commands.parallel( - zoooomToCubeCommand, - runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickup) / 2)), - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) - ), - - Commands.race( - new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(20) // kill this auto - // TODO need add protection here!!!!!! - ), - - Commands.parallel( - claw.setPower(-0.36), - Commands.deadline( - Commands.waitSeconds(0.6), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - waitUntil(arm.atTargetPosition) - ) - ) - ), - - Commands.waitSeconds(0.1), - - Commands.parallel( - // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.20), - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.sequence( - waitSeconds(0.2), - new TurnToAngle(-179.9, swerveDrive) // it needs zoooomAllianceThingy? TODO - ) - ), - - cubeToZoooomCommand, - - claw.setPower(0.3), - - Commands.parallel( - Commands.sequence( - waitSeconds(0.2), - claw.setPower(0) - ), - zoooomPartTwoCommand, - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) - ), - - new TurnToAngle(45 * zoooomAllianceThingy, swerveDrive), +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( +// zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + + +// return Commands.race( +// Commands.waitSeconds(15), // TODO DEL +// //init +// Commands.sequence( + +// Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), + +// //trajectory to cube +// Commands.parallel( +// zoooomToCubeCommand, +// runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickup) / 2)), +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) +// ), + +// Commands.race( +// new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(20) // kill this auto +// // TODO need add protection here!!!!!! +// ), + +// Commands.parallel( +// claw.setPower(-0.36), +// Commands.deadline( +// Commands.waitSeconds(0.6), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), +// waitUntil(arm.atTargetPosition) +// ) +// ) +// ), + +// Commands.waitSeconds(0.1), + +// Commands.parallel( +// // Close claw/stop claw intake rollers/low background rolling to keep control of game piece +// claw.setPower(-0.20), +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.sequence( +// waitSeconds(0.2), +// new TurnToAngle(-179.9, swerveDrive) // it needs zoooomAllianceThingy? TODO +// ) +// ), + +// cubeToZoooomCommand, + +// claw.setPower(0.3), + +// Commands.parallel( +// Commands.sequence( +// waitSeconds(0.2), +// claw.setPower(0) +// ), +// zoooomPartTwoCommand, +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) +// ), + +// new TurnToAngle(45 * zoooomAllianceThingy, swerveDrive), - // Commands.race( - // new RunCommand(() -> vision.driveToCubeOnGround(claw, 2), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - // Commands.waitSeconds(5) // kill this auto - // ), - - Commands.parallel( - Commands.runOnce(() -> swerveDrive.stopModules()) - // claw.setPower(-0.36), - // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)) - ), - Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)) - - // Commands.parallel( - // // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - // claw.setPower(-0.20), - // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)) - // ) - ), - - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) - ); - } -} +// // Commands.race( +// // new RunCommand(() -> vision.driveToCubeOnGround(claw, 2), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// // Commands.waitSeconds(5) // kill this auto +// // ), + +// Commands.parallel( +// Commands.runOnce(() -> swerveDrive.stopModules()) +// // claw.setPower(-0.36), +// // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)) +// ), +// Commands.runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)) + +// // Commands.parallel( +// // // Close claw/stop claw intake rollers/low background rolling to keep control of game piece +// // claw.setPower(-0.20), +// // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)) +// // ) +// ), + +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) +// ); +// } +// } diff --git a/src/main/java/frc/robot/commands/VisionAutos.java b/src/main/java/frc/robot/commands/VisionAutos.java index 2288e88..f44b4f8 100644 --- a/src/main/java/frc/robot/commands/VisionAutos.java +++ b/src/main/java/frc/robot/commands/VisionAutos.java @@ -1,570 +1,570 @@ -package frc.robot.commands; - -import java.util.List; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ClawConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.SwerveAutoConstants; -import frc.robot.Constants.SwerveDriveConstants; -import frc.robot.commands.SwerveAutos.StartPosition; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.claw.MotorClaw; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.vision.VROOOOM; -import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; -import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; - -import static frc.robot.Constants.SwerveAutoConstants.*; -import static edu.wpi.first.wpilibj2.command.Commands.*; - -// Trajectory must stop ~32 inches in front of the cone -// Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station -// All score positions are assumed to be high for this file right now -// Positive Y = left, positive X = towards drivers, rotation positive is clockwise - -public class VisionAutos { +// package frc.robot.commands; + +// import java.util.List; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants.ArmConstants; +// import frc.robot.Constants.ClawConstants; +// import frc.robot.Constants.ElevatorConstants; +// import frc.robot.Constants.SwerveAutoConstants; +// import frc.robot.Constants.SwerveDriveConstants; +// import frc.robot.commands.SwerveAutos.StartPosition; +// import frc.robot.subsystems.Arm; +// import frc.robot.subsystems.Elevator; +// import frc.robot.subsystems.claw.MotorClaw; +// import frc.robot.subsystems.swerve.SwerveDrivetrain; +// import frc.robot.subsystems.vision.VROOOOM; +// import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; +// import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; + +// import static frc.robot.Constants.SwerveAutoConstants.*; +// import static edu.wpi.first.wpilibj2.command.Commands.*; + +// // Trajectory must stop ~32 inches in front of the cone +// // Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station +// // All score positions are assumed to be high for this file right now +// // Positive Y = left, positive X = towards drivers, rotation positive is clockwise + +// public class VisionAutos { - /** - * You should not be calling this function directly (nothing will break if you do, - * but it is meant to be paired with vision auto choosing). Instead, call visionAutoChoose below. - * - * @return Auto Command - */ - public static CommandBase visionPreloadPickupScore(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance, StartPosition position, SCORE_POS scorePosition) { - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); +// /** +// * You should not be calling this function directly (nothing will break if you do, +// * but it is meant to be paired with vision auto choosing). Instead, call visionAutoChoose below. +// * +// * @return Auto Command +// */ +// public static CommandBase visionPreloadPickupScore(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance, StartPosition position, SCORE_POS scorePosition) { +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); - double pickupXDistance = 0; - double pickupYDistance = 0; - double pickupRotation = 0; - - double avoidCollisionYOffset = 0; // So we don't bump into the charging station - - // Position when scoring the object we picked up - double scoreXDistance = 0; - double scoreYDistance = 0; - - if (alliance == Alliance.Red) { - if (position == StartPosition.RIGHT) position = StartPosition.LEFT; - if (position == StartPosition.LEFT) position = StartPosition.RIGHT; - } - - switch (position) { - // Reversed because gyro starts reversed - case RIGHT: - pickupXDistance = -3.73; - pickupYDistance = 0.93; - pickupRotation = -175; - scoreYDistance = 0.5; - avoidCollisionYOffset = 0.25; - break; - case LEFT: - pickupXDistance = -3.73; - pickupYDistance = -0.93; - scoreYDistance = -0.5; - pickupRotation = 175; // Tested at Da Vinci, not accurate likely because of limelight placement - avoidCollisionYOffset = -0.25; // Was 0.75 when tested at Da Vinci with only 1 waypoint, but now we're using 2 - break; - case MIDDLE: - pickupXDistance = -5; // TODO: Measure IRL - pickupYDistance = -0.3; - pickupRotation = -165; - break; - } - - // Negate all measurements related to the y-axis because we are on the opposite side of the field - if (alliance == Alliance.Red) { - if (position != StartPosition.MIDDLE) { - pickupYDistance *= -1; - avoidCollisionYOffset *= -1; - pickupRotation *= -1; - } - } - - Trajectory scoreToPickup = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(pickupXDistance * 0.25, avoidCollisionYOffset), - new Translation2d(pickupXDistance * 0.9, 0)), - new Pose2d(pickupXDistance, pickupYDistance, Rotation2d.fromDegrees(pickupRotation)), - trajectoryConfig); - - // Trajectory with an inital pose - // Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( - // new Pose2d(pickupXDistance, pickupYDistance, new Rotation2d(pickupRotation)), - // List.of( - // new Translation2d(pickupXDistance * 0.75, yOffset), - // new Translation2d(pickupXDistance * 0.25, yOffset)), - // new Pose2d(scoreXDistance, scoreYDistance, Rotation2d.fromDegrees(0)), - // trajectoryConfig); - - // Trajectory without an intial pose so we don't do unnecessary movement after vision pickup - Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(pickupXDistance * 0.75, avoidCollisionYOffset, Rotation2d.fromDegrees(pickupRotation * 0.75)), - new Pose2d(pickupXDistance * 0.1, avoidCollisionYOffset, Rotation2d.fromDegrees(pickupRotation * 0.25)), - new Pose2d(scoreXDistance, scoreYDistance, Rotation2d.fromDegrees(-10))) - , trajectoryConfig); - - //Create PID Controllers - PIDController xController = new PIDController(kPXController, kIXController, kDXController); - PIDController yController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController thetaController = new ProfiledPIDController( - kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - thetaController.enableContinuousInput(-Math.PI, Math.PI); +// double pickupXDistance = 0; +// double pickupYDistance = 0; +// double pickupRotation = 0; + +// double avoidCollisionYOffset = 0; // So we don't bump into the charging station + +// // Position when scoring the object we picked up +// double scoreXDistance = 0; +// double scoreYDistance = 0; + +// if (alliance == Alliance.Red) { +// if (position == StartPosition.RIGHT) position = StartPosition.LEFT; +// if (position == StartPosition.LEFT) position = StartPosition.RIGHT; +// } + +// switch (position) { +// // Reversed because gyro starts reversed +// case RIGHT: +// pickupXDistance = -3.73; +// pickupYDistance = 0.93; +// pickupRotation = -175; +// scoreYDistance = 0.5; +// avoidCollisionYOffset = 0.25; +// break; +// case LEFT: +// pickupXDistance = -3.73; +// pickupYDistance = -0.93; +// scoreYDistance = -0.5; +// pickupRotation = 175; // Tested at Da Vinci, not accurate likely because of limelight placement +// avoidCollisionYOffset = -0.25; // Was 0.75 when tested at Da Vinci with only 1 waypoint, but now we're using 2 +// break; +// case MIDDLE: +// pickupXDistance = -5; // TODO: Measure IRL +// pickupYDistance = -0.3; +// pickupRotation = -165; +// break; +// } + +// // Negate all measurements related to the y-axis because we are on the opposite side of the field +// if (alliance == Alliance.Red) { +// if (position != StartPosition.MIDDLE) { +// pickupYDistance *= -1; +// avoidCollisionYOffset *= -1; +// pickupRotation *= -1; +// } +// } + +// Trajectory scoreToPickup = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(pickupXDistance * 0.25, avoidCollisionYOffset), +// new Translation2d(pickupXDistance * 0.9, 0)), +// new Pose2d(pickupXDistance, pickupYDistance, Rotation2d.fromDegrees(pickupRotation)), +// trajectoryConfig); + +// // Trajectory with an inital pose +// // Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( +// // new Pose2d(pickupXDistance, pickupYDistance, new Rotation2d(pickupRotation)), +// // List.of( +// // new Translation2d(pickupXDistance * 0.75, yOffset), +// // new Translation2d(pickupXDistance * 0.25, yOffset)), +// // new Pose2d(scoreXDistance, scoreYDistance, Rotation2d.fromDegrees(0)), +// // trajectoryConfig); + +// // Trajectory without an intial pose so we don't do unnecessary movement after vision pickup +// Trajectory pickupToScore = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(pickupXDistance * 0.75, avoidCollisionYOffset, Rotation2d.fromDegrees(pickupRotation * 0.75)), +// new Pose2d(pickupXDistance * 0.1, avoidCollisionYOffset, Rotation2d.fromDegrees(pickupRotation * 0.25)), +// new Pose2d(scoreXDistance, scoreYDistance, Rotation2d.fromDegrees(-10))) +// , trajectoryConfig); + +// //Create PID Controllers +// PIDController xController = new PIDController(kPXController, kIXController, kDXController); +// PIDController yController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController thetaController = new ProfiledPIDController( +// kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// thetaController.enableContinuousInput(-Math.PI, Math.PI); - SwerveControllerCommand scoreToPickupCommand = new SwerveControllerCommand( - scoreToPickup, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand scoreToPickupCommand = new SwerveControllerCommand( +// scoreToPickup, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - SwerveControllerCommand pickupToScoreCommand = new SwerveControllerCommand( - pickupToScore, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand pickupToScoreCommand = new SwerveControllerCommand( +// pickupToScore, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// xController, yController, thetaController, swerveDrive::setModuleStates, swerveDrive); - final StartPosition startPositionFinal = position; - final Alliance allianceFinal = alliance; +// final StartPosition startPositionFinal = position; +// final Alliance allianceFinal = alliance; - return race( - sequence( +// return race( +// sequence( - // ======== Drop Preload Start ======== - Commands.parallel( - runOnce(() -> SmartDashboard.putString("Stage", "Start")), - runOnce(() -> swerveDrive.resetOdometry(scoreToPickup.getInitialPose())), - runOnce(() -> swerveDrive.stopModules()) - ), - - // Move arm and elevator, arm is moved 0.5 seconds after the elevator to prevent power chain from getting caught - Commands.race( - Commands.waitSeconds(5), // Timeout - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), - Commands.waitSeconds(0.5), - - Commands.parallel( // End when target positions reached - Commands.waitUntil(elevator.atTargetPosition), - Commands.waitUntil(arm.atTargetPosition), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreHigh)) - ) - ) - ), - waitSeconds(0.25), - - // Open claw/eject piece with rollers - claw.setPower(0.5), - - // Wait to outtake - Commands.waitSeconds(.5), - - // Close claw/stop rollers - claw.setPower(0), - - // Parallel driving to pickup position and moving arm/elev to ready-to-pickup position - Commands.deadline( - Commands.waitSeconds(2), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - ) - ), - - // Commands.deadline( - scoreToPickupCommand, - - // Move arm to ready-to-pickup position - // Commands.sequence( - // Commands.deadline( - // Commands.waitSeconds(2), - // Commands.parallel( // End command once both arm and elevator have reached their target position - // Commands.waitUntil(arm.atTargetPosition), - // Commands.waitUntil(elevator.atTargetPosition), - // Commands.runOnce(() -> arm.setTargetTicks(-328500 + 7950)), - // Commands.runOnce(() -> elevator.setTargetTicks(-160000)) - // ) - // ) - // ) - // ), - runOnce(() -> swerveDrive.stopModules()), - - // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw - vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), // Added claw parameter to get rid of error - - Commands.deadline( - Commands.waitSeconds(2), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - ) - ), - - // Drive to score in parallel with arm moving to score position and elevator stowing - // Commands.parallel( - pickupToScoreCommand, - // ), - - parallel( - runOnce(() -> swerveDrive.stopModules()), - runOnce(() -> SmartDashboard.putString("Stage", "Score 2")) - ), - - // Drive to target and score, ends with arm/elev fully extended to score - new TurnToAngle(0, swerveDrive), - - Commands.deadline( - Commands.waitSeconds(2), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreMid)) - ) - ), +// // ======== Drop Preload Start ======== +// Commands.parallel( +// runOnce(() -> SmartDashboard.putString("Stage", "Start")), +// runOnce(() -> swerveDrive.resetOdometry(scoreToPickup.getInitialPose())), +// runOnce(() -> swerveDrive.stopModules()) +// ), + +// // Move arm and elevator, arm is moved 0.5 seconds after the elevator to prevent power chain from getting caught +// Commands.race( +// Commands.waitSeconds(5), // Timeout +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), +// Commands.waitSeconds(0.5), + +// Commands.parallel( // End when target positions reached +// Commands.waitUntil(elevator.atTargetPosition), +// Commands.waitUntil(arm.atTargetPosition), +// Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreHigh)) +// ) +// ) +// ), +// waitSeconds(0.25), + +// // Open claw/eject piece with rollers +// claw.setPower(0.5), + +// // Wait to outtake +// Commands.waitSeconds(.5), + +// // Close claw/stop rollers +// claw.setPower(0), + +// // Parallel driving to pickup position and moving arm/elev to ready-to-pickup position +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.parallel( // End command once both arm and elevator have reached their target position +// Commands.waitUntil(arm.atTargetPosition), +// Commands.waitUntil(elevator.atTargetPosition), +// Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) +// ) +// ), + +// // Commands.deadline( +// scoreToPickupCommand, + +// // Move arm to ready-to-pickup position +// // Commands.sequence( +// // Commands.deadline( +// // Commands.waitSeconds(2), +// // Commands.parallel( // End command once both arm and elevator have reached their target position +// // Commands.waitUntil(arm.atTargetPosition), +// // Commands.waitUntil(elevator.atTargetPosition), +// // Commands.runOnce(() -> arm.setTargetTicks(-328500 + 7950)), +// // Commands.runOnce(() -> elevator.setTargetTicks(-160000)) +// // ) +// // ) +// // ) +// // ), +// runOnce(() -> swerveDrive.stopModules()), + +// // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw +// vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), // Added claw parameter to get rid of error + +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.parallel( // End command once both arm and elevator have reached their target position +// Commands.waitUntil(arm.atTargetPosition), +// Commands.waitUntil(elevator.atTargetPosition), +// Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) +// ) +// ), + +// // Drive to score in parallel with arm moving to score position and elevator stowing +// // Commands.parallel( +// pickupToScoreCommand, +// // ), + +// parallel( +// runOnce(() -> swerveDrive.stopModules()), +// runOnce(() -> SmartDashboard.putString("Stage", "Score 2")) +// ), + +// // Drive to target and score, ends with arm/elev fully extended to score +// new TurnToAngle(0, swerveDrive), + +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.parallel( // End command once both arm and elevator have reached their target position +// Commands.waitUntil(arm.atTargetPosition), +// Commands.waitUntil(elevator.atTargetPosition), +// Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), +// Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreMid)) +// ) +// ), - vision.VisionScoreNoArm(OBJECT_TYPE.CUBE, SCORE_POS.MID), - // new TurnToAngle(0, swerveDrive), +// vision.VisionScoreNoArm(OBJECT_TYPE.CUBE, SCORE_POS.MID), +// // new TurnToAngle(0, swerveDrive), - // Stow the elevator, move arm to substation pos, and charge in parallel - Commands.parallel( - Commands.deadline( - Commands.waitSeconds(2), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - ) - ) - ) - ), +// // Stow the elevator, move arm to substation pos, and charge in parallel +// Commands.parallel( +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.parallel( // End command once both arm and elevator have reached their target position +// Commands.waitUntil(arm.atTargetPosition), +// Commands.waitUntil(elevator.atTargetPosition), +// Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) +// ) +// ) +// ) +// ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); - } +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ).finallyDo((x) -> swerveDrive.getImu().setOffset(180)); +// } - public static CommandBase zoomTwoPieceAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kTwoPieceSpeedMetersPerSecond, SwerveAutoConstants.kTwoPieceAccelerationMetersPerSecondSquared); - - double allianceFactor = 1.0; - if (alliance == Alliance.Red) { - allianceFactor = -1.0; - } +// public static CommandBase zoomTwoPieceAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); + +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kTwoPieceSpeedMetersPerSecond, SwerveAutoConstants.kTwoPieceAccelerationMetersPerSecondSquared); + +// double allianceFactor = 1.0; +// if (alliance == Alliance.Red) { +// allianceFactor = -1.0; +// } - //trajectory stuff - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.2, 0.2 * allianceFactor) - //new Translation2d(-1.8, -0.4) - ), - new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), - new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), - // new Pose2d(-0.8, -1.0, Rotation2d.fromDegrees(0)), - new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(180)) - ), - trajectoryConfig); - - Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(3.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(4, 2.2 * allianceFactor, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); - - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( - zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - final int armPosFinal = ArmConstants.kArmScore; - final int elevatorPosFinal = ElevatorConstants.kElevatorScoreHigh; - - return Commands.race( - //init - Commands.sequence( - parallel( - runOnce(() -> SmartDashboard.putString("Stage", "Start")), - runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()) - ), - - //trajectory to cube - Commands.runOnce(() -> SmartDashboard.putString("Moved On", "zoomin")), - Commands.parallel( - claw.setPower(ClawConstants.kIntakePower), - zoooomToCubeCommand, - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - // // new TurnToAngle(170, swerveDrive), - // runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - // runOnce(() -> swerveDrive.stopModules()), - - vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), - - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - // Cube should be in the claw at this point - - Commands.parallel( - new TurnToAngle(180, swerveDrive), - claw.setPower(ClawConstants.kIntakeNeutralPower), - - // Drop arm and elevator so the game piece can be intook - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) / 2)), // Halfway between scoring and ground pickup - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - cubeToZoooomCommand, - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - claw.setPower(ClawConstants.kOuttakePower), - Commands.waitSeconds(0.5), - claw.setPowerZero(), +// //trajectory stuff + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.2, 0.2 * allianceFactor) +// //new Translation2d(-1.8, -0.4) +// ), +// new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), +// new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), +// // new Pose2d(-0.8, -1.0, Rotation2d.fromDegrees(0)), +// new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(180)) +// ), +// trajectoryConfig); + +// Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(3.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(4, 2.2 * allianceFactor, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); + +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( +// zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// final int armPosFinal = ArmConstants.kArmScore; +// final int elevatorPosFinal = ElevatorConstants.kElevatorScoreHigh; + +// return Commands.race( +// //init +// Commands.sequence( +// parallel( +// runOnce(() -> SmartDashboard.putString("Stage", "Start")), +// runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()) +// ), + +// //trajectory to cube +// Commands.runOnce(() -> SmartDashboard.putString("Moved On", "zoomin")), +// Commands.parallel( +// claw.setPower(ClawConstants.kIntakePower), +// zoooomToCubeCommand, +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// // // new TurnToAngle(170, swerveDrive), +// // runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// // runOnce(() -> swerveDrive.stopModules()), + +// vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), + +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// // Cube should be in the claw at this point + +// Commands.parallel( +// new TurnToAngle(180, swerveDrive), +// claw.setPower(ClawConstants.kIntakeNeutralPower), + +// // Drop arm and elevator so the game piece can be intook +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) / 2)), // Halfway between scoring and ground pickup +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// cubeToZoooomCommand, +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// claw.setPower(ClawConstants.kOuttakePower), +// Commands.waitSeconds(0.5), +// claw.setPowerZero(), - zoooomPartTwoCommand, - claw.setPower(ClawConstants.kIntakePower), - - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - new TurnToAngle(0, swerveDrive), - Commands.parallel( - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - // //vision pickup - // // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw - vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), - claw.setPower(ClawConstants.kIntakeNeutralPower) - ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } - - public static CommandBase cableZoomTwoPieceAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kTwoPieceSpeedMetersPerSecond, SwerveAutoConstants.kTwoPieceAccelerationMetersPerSecondSquared); - - double allianceFactor = -1.0; - if (alliance == Alliance.Red) { - allianceFactor = 1.0; - } +// zoooomPartTwoCommand, +// claw.setPower(ClawConstants.kIntakePower), + +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// new TurnToAngle(0, swerveDrive), +// Commands.parallel( +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// // //vision pickup +// // // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw +// vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), +// claw.setPower(ClawConstants.kIntakeNeutralPower) +// ), +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } + +// public static CommandBase cableZoomTwoPieceAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); + +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kTwoPieceSpeedMetersPerSecond, SwerveAutoConstants.kTwoPieceAccelerationMetersPerSecondSquared); + +// double allianceFactor = -1.0; +// if (alliance == Alliance.Red) { +// allianceFactor = 1.0; +// } - //trajectory stuff - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.2, 0.2 * allianceFactor) - //new Translation2d(-1.8, -0.4) - ), - new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), - new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), - // new Pose2d(-0.8, -1.0, Rotation2d.fromDegrees(0)), - new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(180)) - ), - trajectoryConfig); - - Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), - new Pose2d(4.2, 2.2 * allianceFactor, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); - - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( - zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - final int armPosFinal = ArmConstants.kArmScore; - final int elevatorPosFinal = ElevatorConstants.kElevatorScoreHigh; - - return Commands.race( - //init - Commands.sequence( - parallel( - runOnce(() -> SmartDashboard.putString("Stage", "Start")), - runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()) - ), - - //trajectory to cube - Commands.runOnce(() -> SmartDashboard.putString("Moved On", "zoomin")), - Commands.parallel( - claw.setPower(ClawConstants.kIntakePower), - zoooomToCubeCommand, - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.waitSeconds(1), - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - // // new TurnToAngle(170, swerveDrive), - // runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - // runOnce(() -> swerveDrive.stopModules()), - - vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), - - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - // Cube should be in the claw at this point - - Commands.parallel( - new TurnToAngle(180, swerveDrive), - claw.setPower(ClawConstants.kIntakeNeutralPower), - - // Drop arm and elevator so the game piece can be intook - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) / 2)), // Halfway between scoring and ground pickup - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - cubeToZoooomCommand, - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - claw.setPower(ClawConstants.kOuttakePower), - Commands.waitSeconds(0.5), - claw.setPowerZero(), +// //trajectory stuff + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.2, 0.2 * allianceFactor) +// //new Translation2d(-1.8, -0.4) +// ), +// new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), +// new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(180)), +// // new Pose2d(-0.8, -1.0, Rotation2d.fromDegrees(0)), +// new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(180)) +// ), +// trajectoryConfig); + +// Trajectory zoooomPartTwo = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(0.2, 1.0 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0.8, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(3.9, 0.2 * allianceFactor, Rotation2d.fromDegrees(179.9)), +// new Pose2d(4.2, 2.2 * allianceFactor, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); + +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// SwerveControllerCommand zoooomPartTwoCommand = new SwerveControllerCommand( +// zoooomPartTwo, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// final int armPosFinal = ArmConstants.kArmScore; +// final int elevatorPosFinal = ElevatorConstants.kElevatorScoreHigh; + +// return Commands.race( +// //init +// Commands.sequence( +// parallel( +// runOnce(() -> SmartDashboard.putString("Stage", "Start")), +// runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()) +// ), + +// //trajectory to cube +// Commands.runOnce(() -> SmartDashboard.putString("Moved On", "zoomin")), +// Commands.parallel( +// claw.setPower(ClawConstants.kIntakePower), +// zoooomToCubeCommand, +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.waitSeconds(1), +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// // // new TurnToAngle(170, swerveDrive), +// // runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// // runOnce(() -> swerveDrive.stopModules()), + +// vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), + +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// // Cube should be in the claw at this point + +// Commands.parallel( +// new TurnToAngle(180, swerveDrive), +// claw.setPower(ClawConstants.kIntakeNeutralPower), + +// // Drop arm and elevator so the game piece can be intook +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) / 2)), // Halfway between scoring and ground pickup +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// cubeToZoooomCommand, +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// claw.setPower(ClawConstants.kOuttakePower), +// Commands.waitSeconds(0.5), +// claw.setPowerZero(), - zoooomPartTwoCommand, - claw.setPower(ClawConstants.kIntakePower), - - runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), - runOnce(() -> swerveDrive.stopModules()), - - new TurnToAngle(0, swerveDrive), - Commands.parallel( - Commands.deadline( - Commands.waitSeconds(2), - Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), - Commands.waitSeconds(0.5), - Commands.waitUntil(arm.atTargetPosition) - ) - ) - ), - // //vision pickup - // // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw - vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), - claw.setPower(ClawConstants.kIntakeNeutralPower) - ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } -} \ No newline at end of file +// zoooomPartTwoCommand, +// claw.setPower(ClawConstants.kIntakePower), + +// runOnce(() -> swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates)), +// runOnce(() -> swerveDrive.stopModules()), + +// new TurnToAngle(0, swerveDrive), +// Commands.parallel( +// Commands.deadline( +// Commands.waitSeconds(2), +// Commands.runOnce(() -> SmartDashboard.putString("Stage", "Score")), +// Commands.sequence( +// Commands.runOnce(() -> arm.setTargetTicks((ArmConstants.kArmScore + ArmConstants.kArmGroundPickupVision) /2)), +// Commands.waitSeconds(0.5), +// Commands.waitUntil(arm.atTargetPosition) +// ) +// ) +// ), +// // //vision pickup +// // // Arm is moved to pick up cube, ends with arm/elev extended and cube in the claw +// vision.VisionPickupGroundNoArm(OBJECT_TYPE.CUBE, claw), +// claw.setPower(ClawConstants.kIntakeNeutralPower) +// ), +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/VisionAutos/FollowVisionPath.java b/src/main/java/frc/robot/commands/VisionAutos/FollowVisionPath.java new file mode 100644 index 0000000..950359f --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAutos/FollowVisionPath.java @@ -0,0 +1,37 @@ +package frc.robot.commands.VisionAutos; + +import com.pathplanner.lib.auto.SwerveAutoBuilder; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.vision.primalWallnut.PrimalSunflower; + +public class FollowVisionPath extends CommandBase { + private SwerveAutoBuilder swerveAutoBuilder; + private PrimalSunflower sunflower; + + /** + * Construct a new FollowVisionPath command + * + * Uses primal sunflower to follow a path using pathplanner trajectory + * + * @param autoBuilder Swerve Auto Builder for Path Planner + * @param sunflower Primal Sunflower + */ + public FollowVisionPath(SwerveAutoBuilder autoBuilder, PrimalSunflower sunflower) { + this.swerveAutoBuilder = autoBuilder; + this.sunflower = sunflower; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + swerveAutoBuilder.followPathWithEvents(sunflower.toNearestGrid()); + } + + @Override + public boolean isFinished() { + return false; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/VisionAutos/ToNearestGridDebug.java b/src/main/java/frc/robot/commands/VisionAutos/ToNearestGridDebug.java new file mode 100644 index 0000000..9b5cb5a --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAutos/ToNearestGridDebug.java @@ -0,0 +1,37 @@ +package frc.robot.commands.VisionAutos; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.swerve.SwerveDrivetrain; +import frc.robot.subsystems.vision.primalWallnut.PrimalSunflower; + +public class ToNearestGridDebug extends CommandBase { + private SwerveDrivetrain swerve; + private PrimalSunflower vision; + + /** + * Construct a new ToNearestGrid command + * + * Drive the robot to the nearest grid using odometry. + * + * @param swerveDrive Swerve drivetrain to move + */ + public ToNearestGridDebug(SwerveDrivetrain swerve, PrimalSunflower vision) { + this.swerve = swerve; + this.vision = vision; + + addRequirements(swerve); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + vision.toNearestGridDebug(swerve); + } + + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/VisionCableSideAuto.java b/src/main/java/frc/robot/commands/VisionCableSideAuto.java index c930f98..3025024 100644 --- a/src/main/java/frc/robot/commands/VisionCableSideAuto.java +++ b/src/main/java/frc/robot/commands/VisionCableSideAuto.java @@ -1,229 +1,229 @@ -package frc.robot.commands; - -import java.util.List; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.SwerveAutoConstants; -import frc.robot.Constants.SwerveDriveConstants; -import frc.robot.commands.SwerveAutos.StartPosition; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.claw.MotorClaw; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.vision.VROOOOM; -import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; -import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; - -import static frc.robot.Constants.SwerveAutoConstants.*; -import static edu.wpi.first.wpilibj2.command.Commands.*; - -// Trajectory must stop ~32 inches in front of the cone -// Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station -// All score positions are assumed to be high for this file right now -// Positive Y = left, positive X = towards drivers, rotation positive is clockwise - -public class VisionCableSideAuto { +// package frc.robot.commands; + +// import java.util.List; + +// import edu.wpi.first.math.controller.PIDController; +// import edu.wpi.first.math.controller.ProfiledPIDController; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.trajectory.Trajectory; +// import edu.wpi.first.math.trajectory.TrajectoryConfig; +// import edu.wpi.first.math.trajectory.TrajectoryGenerator; +// import edu.wpi.first.wpilibj.DriverStation.Alliance; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import edu.wpi.first.wpilibj2.command.Commands; +// import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.RunCommand; +// import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +// import edu.wpi.first.wpilibj2.command.WaitCommand; +// import frc.robot.Constants.ArmConstants; +// import frc.robot.Constants.ElevatorConstants; +// import frc.robot.Constants.SwerveAutoConstants; +// import frc.robot.Constants.SwerveDriveConstants; +// import frc.robot.commands.SwerveAutos.StartPosition; +// import frc.robot.subsystems.Arm; +// import frc.robot.subsystems.Elevator; +// import frc.robot.subsystems.claw.MotorClaw; +// import frc.robot.subsystems.swerve.SwerveDrivetrain; +// import frc.robot.subsystems.vision.VROOOOM; +// import frc.robot.subsystems.vision.VROOOOM.OBJECT_TYPE; +// import frc.robot.subsystems.vision.VROOOOM.SCORE_POS; + +// import static frc.robot.Constants.SwerveAutoConstants.*; +// import static edu.wpi.first.wpilibj2.command.Commands.*; + +// // Trajectory must stop ~32 inches in front of the cone +// // Trajectory can stop anywhere in front of the tape/tag as long as the robot is in front of the charging station +// // All score positions are assumed to be high for this file right now +// // Positive Y = left, positive X = towards drivers, rotation positive is clockwise + +// public class VisionCableSideAuto { - public static CommandBase LowAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = - new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kMaxSpeedMetersPerSecond * 0.75, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); - - double zoooomAllianceThingy = 1.0; - int atagId = 8; - if (alliance == Alliance.Red) { - zoooomAllianceThingy = -1.0; - atagId = 1; - } +// public static CommandBase LowAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = +// new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); + +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kMaxSpeedMetersPerSecond * 0.75, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); + +// double zoooomAllianceThingy = 1.0; +// int atagId = 8; +// if (alliance == Alliance.Red) { +// zoooomAllianceThingy = -1.0; +// atagId = 1; +// } - //trajectory stuff - - /*Trajectory zoooomToCube_A = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.18, -0.18 * zoooomAllianceThingy) - ), - new Pose2d(2.4, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory zoooomToCube_B = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(2.5, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(4.4, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)) - ), - trajectoryConfig);*/ - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0.2, 0 * zoooomAllianceThingy), - new Translation2d(-0.2, 0 * zoooomAllianceThingy), // push the cube to hybrid zone - //new Translation2d(0, 0 * zoooomAllianceThingy), // comment it out, grid holds previous -0.2 - new Translation2d(0.2, -0.2 * zoooomAllianceThingy), - new Translation2d(2.5, -0.2 * zoooomAllianceThingy), - new Translation2d(4.0, -0.2 * zoooomAllianceThingy) - ), - new Pose2d(4.4, -0.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - /*Trajectory cubeToZoooom_A = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(3.6, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); - - Trajectory cubeToZoooom_B = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(1.9, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(0, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.5, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig);*/ - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(3.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.2, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.2, -0.62 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), - new Pose2d(-0.7, -0.62 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) - ), - trajectoryConfig); +// //trajectory stuff + +// /*Trajectory zoooomToCube_A = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.18, -0.18 * zoooomAllianceThingy) +// ), +// new Pose2d(2.4, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory zoooomToCube_B = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(2.5, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(4.4, -0.18 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)) +// ), +// trajectoryConfig);*/ + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(0.2, 0 * zoooomAllianceThingy), +// new Translation2d(-0.2, 0 * zoooomAllianceThingy), // push the cube to hybrid zone +// //new Translation2d(0, 0 * zoooomAllianceThingy), // comment it out, grid holds previous -0.2 +// new Translation2d(0.2, -0.2 * zoooomAllianceThingy), +// new Translation2d(2.5, -0.2 * zoooomAllianceThingy), +// new Translation2d(4.0, -0.2 * zoooomAllianceThingy) +// ), +// new Pose2d(4.4, -0.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// /*Trajectory cubeToZoooom_A = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(3.6, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); + +// Trajectory cubeToZoooom_B = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(1.9, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(0, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.5, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig);*/ + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(3.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.2, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.2, -0.62 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)), +// new Pose2d(-0.7, -0.62 * zoooomAllianceThingy, Rotation2d.fromDegrees(179.9)) +// ), +// trajectoryConfig); - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - /*SwerveControllerCommand zoooomToCubeCommand_B = new SwerveControllerCommand( - zoooomToCube_B, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive);*/ - - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - - /*SwerveControllerCommand cubeToZoooomCommand_B = new SwerveControllerCommand( - cubeToZoooom_B, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive);*/ - - final int atagIdFinal = atagId; - - return Commands.race( - Commands.waitSeconds(15), // TODO DEL - //init - Commands.sequence( - - Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), - - //trajectory to cube - Commands.parallel( - zoooomToCubeCommand, - Commands.sequence( - Commands.waitSeconds(1.5), - runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe - ), - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) - ), - - Commands.race( - new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(20) // kill this auto - // TODO need add protection here!!!!!! - ), - - Commands.parallel( - claw.setPower(-0.36), - Commands.deadline( - Commands.waitSeconds(1.2), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - waitUntil(arm.atTargetPosition) - ) - ) - ), - - Commands.waitSeconds(0.1), - - Commands.parallel( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.sequence( - waitSeconds(0.2), - new TurnToAngle(-179.9, swerveDrive) - ) - ), - // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.20), - - Commands.parallel( - cubeToZoooomCommand, - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.ATAG)) - ), - - new TurnToAngle(-179.9, swerveDrive), +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// /*SwerveControllerCommand zoooomToCubeCommand_B = new SwerveControllerCommand( +// zoooomToCube_B, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive);*/ + +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); + +// /*SwerveControllerCommand cubeToZoooomCommand_B = new SwerveControllerCommand( +// cubeToZoooom_B, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive);*/ + +// final int atagIdFinal = atagId; + +// return Commands.race( +// Commands.waitSeconds(15), // TODO DEL +// //init +// Commands.sequence( + +// Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), + +// //trajectory to cube +// Commands.parallel( +// zoooomToCubeCommand, +// Commands.sequence( +// Commands.waitSeconds(1.5), +// runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe +// ), +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)) +// ), + +// Commands.race( +// new RunCommand(() -> vision.driveToCubeOnGround(claw, 5), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(20) // kill this auto +// // TODO need add protection here!!!!!! +// ), + +// Commands.parallel( +// claw.setPower(-0.36), +// Commands.deadline( +// Commands.waitSeconds(1.2), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), +// waitUntil(arm.atTargetPosition) +// ) +// ) +// ), + +// Commands.waitSeconds(0.1), + +// Commands.parallel( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.sequence( +// waitSeconds(0.2), +// new TurnToAngle(-179.9, swerveDrive) +// ) +// ), +// // Close claw/stop claw intake rollers/low background rolling to keep control of game piece +// claw.setPower(-0.20), + +// Commands.parallel( +// cubeToZoooomCommand, +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.ATAG)) +// ), + +// new TurnToAngle(-179.9, swerveDrive), - parallel ( - Commands.race( - new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(3) - )/* , - - //Drop arm High drop off - deadline( - waitSeconds(2), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeHigh)), - waitSeconds(0.5), - waitUntil(arm.atTargetPosition) - ) - )*/ - ), - - /*Commands.deadline( - Commands.waitSeconds(0.5), - sequence( - runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreHighCube)), - waitSeconds(0.5), - waitUntil(elevator.atTargetPosition) - ) - ),*/ - - claw.setPower(0.3) +// parallel ( +// Commands.race( +// new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(3) +// )/* , + +// //Drop arm High drop off +// deadline( +// waitSeconds(2), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeHigh)), +// waitSeconds(0.5), +// waitUntil(arm.atTargetPosition) +// ) +// )*/ +// ), + +// /*Commands.deadline( +// Commands.waitSeconds(0.5), +// sequence( +// runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorScoreHighCube)), +// waitSeconds(0.5), +// waitUntil(elevator.atTargetPosition) +// ) +// ),*/ + +// claw.setPower(0.3) - ), +// ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) - // run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())) +// // run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } @@ -239,139 +239,139 @@ public static CommandBase LowAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, - public static CommandBase HighAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, - Alliance alliance){ - PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); - PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); - ProfiledPIDController trajectoryThetaController = - new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); +// public static CommandBase HighAuto(SwerveDrivetrain swerveDrive, VROOOOM vision, Arm arm, Elevator elevator, MotorClaw claw, +// Alliance alliance){ +// PIDController trajectoryXController = new PIDController(kPXController, kIXController, kDXController); +// PIDController trajectoryYController = new PIDController(kPYController, kIYController, kDYController); +// ProfiledPIDController trajectoryThetaController = +// new ProfiledPIDController(kPThetaController, kIThetaController, kDThetaController, kThetaControllerConstraints); - // Create trajectory settings - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - SwerveAutoConstants.kMaxSpeedMetersPerSecond * 0.75, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); +// // Create trajectory settings +// TrajectoryConfig trajectoryConfig = new TrajectoryConfig( +// SwerveAutoConstants.kMaxSpeedMetersPerSecond * 0.75, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared); - double zoooomAllianceThingy = 1.0; - int atagId = 8; - if (alliance == Alliance.Red) { - zoooomAllianceThingy = -1.0; - atagId = 1; - } +// double zoooomAllianceThingy = 1.0; +// int atagId = 8; +// if (alliance == Alliance.Red) { +// zoooomAllianceThingy = -1.0; +// atagId = 1; +// } - //trajectory stuff - - Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(-0.2, -0.2 * zoooomAllianceThingy), - new Translation2d(-2.5, -0.2 * zoooomAllianceThingy), - new Translation2d(-4, -0.2 * zoooomAllianceThingy)), - new Pose2d(-4.4, -0.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - trajectoryConfig); - - Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( - List.of( - new Pose2d(-4, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(-2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(-1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(0, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), - new Pose2d(-0.5, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)) - ), - trajectoryConfig); +// //trajectory stuff + +// Trajectory zoooomToCube = TrajectoryGenerator.generateTrajectory( +// new Pose2d(0, 0, new Rotation2d(0)), +// List.of( +// new Translation2d(-0.2, -0.2 * zoooomAllianceThingy), +// new Translation2d(-2.5, -0.2 * zoooomAllianceThingy), +// new Translation2d(-4, -0.2 * zoooomAllianceThingy)), +// new Pose2d(-4.4, -0.2 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// trajectoryConfig); + +// Trajectory cubeToZoooom = TrajectoryGenerator.generateTrajectory( +// List.of( +// new Pose2d(-4, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(-2.0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(-1.5, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(0, -0.1 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(0, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)), +// new Pose2d(-0.5, -0.61 * zoooomAllianceThingy, Rotation2d.fromDegrees(0)) +// ), +// trajectoryConfig); - SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( - zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand zoooomToCubeCommand = new SwerveControllerCommand( +// zoooomToCube, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( - cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, - trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); +// SwerveControllerCommand cubeToZoooomCommand = new SwerveControllerCommand( +// cubeToZoooom, swerveDrive::getPose, SwerveDriveConstants.kDriveKinematics, +// trajectoryXController, trajectoryYController, trajectoryThetaController, swerveDrive::setModuleStates, swerveDrive); - final int atagIdFinal = atagId; +// final int atagIdFinal = atagId; - return Commands.race( - Commands.waitSeconds(15), // TODO DEL - //init - Commands.sequence( +// return Commands.race( +// Commands.waitSeconds(15), // TODO DEL +// //init +// Commands.sequence( - ChargeAutos.preloadHigh(arm, elevator, claw), +// ChargeAutos.preloadHigh(arm, elevator, claw), - //TODO confirm if we need it!!!!!! - Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), +// //TODO confirm if we need it!!!!!! +// Commands.runOnce(() -> swerveDrive.resetOdometry(zoooomToCube.getInitialPose())), - //trajectory to cube - Commands.parallel( - zoooomToCubeCommand - // Commands.sequence( - // Commands.waitSeconds(1.5), - // runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe - // ), +// //trajectory to cube +// Commands.parallel( +// zoooomToCubeCommand +// // Commands.sequence( +// // Commands.waitSeconds(1.5), +// // runOnce(() -> arm.setTargetTicks((ArmConstants.kArmStow) )) // to be safe +// // ), - ), - new TurnToAngle(179.9, swerveDrive), - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)), +// ), +// new TurnToAngle(179.9, swerveDrive), +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.CUBE)), - Commands.race( +// Commands.race( - new RunCommand(() -> vision.driveToCubeOnGround(claw, 4), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - Commands.waitSeconds(20) // kill this auto - // TODO need add protection here!!!!!! - ), - - Commands.parallel( - claw.setPower(-0.36), - Commands.deadline( - Commands.waitSeconds(1.2), - sequence( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - waitUntil(arm.atTargetPosition) - ) - ) - ), - - Commands.waitSeconds(0.1), - - Commands.parallel( - runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.sequence( - waitSeconds(0.2), - new TurnToAngle(0, swerveDrive) - ) - ), - // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.20), - - Commands.parallel( - cubeToZoooomCommand, - Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.ATAG)) - ), - - new TurnToAngle(0, swerveDrive), - - // parallel ( - // Commands.race( - // new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), - // Commands.waitSeconds(3) - // )/* , - - // //Drop arm high drop off - // Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands - // Commands.waitSeconds(0.5), - // sequence( - // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeMid)), - // waitSeconds(0.5), - // waitUntil(arm.atTargetPosition) - // ) - // )*/ - // ), - - claw.setPower(0.3) +// new RunCommand(() -> vision.driveToCubeOnGround(claw, 4), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// Commands.waitSeconds(20) // kill this auto +// // TODO need add protection here!!!!!! +// ), + +// Commands.parallel( +// claw.setPower(-0.36), +// Commands.deadline( +// Commands.waitSeconds(1.2), +// sequence( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), +// waitUntil(arm.atTargetPosition) +// ) +// ) +// ), + +// Commands.waitSeconds(0.1), + +// Commands.parallel( +// runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), +// Commands.sequence( +// waitSeconds(0.2), +// new TurnToAngle(0, swerveDrive) +// ) +// ), +// // Close claw/stop claw intake rollers/low background rolling to keep control of game piece +// claw.setPower(-0.20), + +// Commands.parallel( +// cubeToZoooomCommand, +// Commands.runOnce(() -> vision.initVisionPickupOnGround(OBJECT_TYPE.ATAG)) +// ), + +// new TurnToAngle(0, swerveDrive), + +// // parallel ( +// // Commands.race( +// // new RunCommand(() -> vision.driveToGridTag(claw, atagIdFinal), arm, elevator, claw, swerveDrive).until(vision.cameraStatusSupplier), +// // Commands.waitSeconds(3) +// // )/* , + +// // //Drop arm high drop off +// // Commands.deadline( // TODO: Fix this and the other two Deadline arm Commands +// // Commands.waitSeconds(0.5), +// // sequence( +// // runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScoreCubeMid)), +// // waitSeconds(0.5), +// // waitUntil(arm.atTargetPosition) +// // ) +// // )*/ +// // ), + +// claw.setPower(0.3) - ), +// ), - run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - run(() -> elevator.moveMotionMagic(arm.getArmAngle())) - ); - } -} +// run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), +// run(() -> elevator.moveMotionMagic(arm.getArmAngle())) +// ); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/vision/VROOOOM.java b/src/main/java/frc/robot/subsystems/vision/VROOOOM.java deleted file mode 100644 index 235bb44..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VROOOOM.java +++ /dev/null @@ -1,1095 +0,0 @@ -package frc.robot.subsystems.vision; - -import java.util.function.BooleanSupplier; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.ArmConstants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.SwerveDriveConstants; -import frc.robot.commands.TurnToAngle; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Reportable; -import frc.robot.subsystems.claw.Claw; -import frc.robot.subsystems.claw.MotorClaw; -import frc.robot.subsystems.swerve.SwerveDrivetrain; -import frc.robot.subsystems.vision.Limelight.LightMode; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.util.NerdyMath; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj.Timer; - - -public class VROOOOM extends SubsystemBase implements Reportable{ - - private Limelight limelightHigh; - private Limelight limelightLow; - private Limelight currentLimelight = null; - - // Pipelines that should be set on each limelight: cone = 1, cube = 2, tape = 3, tag = 4 - - public enum OBJECT_TYPE - { - CONE, - CUBE, - ATAG - } - - public enum CAMERA_MODE - { - WAIT, // found nothing - IDLE, // doing nothing, init - ACTION,// detected one, and approach to it - ARRIVED // found - } - - public enum SCORE_POS - { - LOW, // Can be ground in pickup mode - MID, - HIGH // Can be substation in pickup mode - } - - private OBJECT_TYPE currentGameObject; - private SCORE_POS currentHeightPos; - private CAMERA_MODE currentCameraMode; - private double goalArea; - private double goalTX; - private double goalYaw; - private boolean rotationIsNeeded; - public BooleanSupplier cameraStatusSupplier; - - - // Current PID Controllers - private PIDController PIDArea = new PIDController(0, 0, 0); - private PIDController PIDTX = new PIDController(0, 0, 0); - private PIDController PIDYaw = new PIDController(0, 0, 0); - - private Arm arm; - private Elevator elevator; - private MotorClaw claw; - private SwerveDrivetrain drivetrain; - - public VROOOOM(Arm arm, Elevator elevator, MotorClaw claw, SwerveDrivetrain drivetrain) { - this.arm = arm; - this.elevator = elevator; - this.claw = claw; - this.drivetrain = drivetrain; - - // defaults - goalArea = 0; - goalTX = 0; - goalYaw = 0; - rotationIsNeeded = false; - - try { - limelightHigh = new Limelight("limelight-high"); - limelightHigh.setLightState(Limelight.LightMode.OFF); - } catch (Exception ex) { - limelightHigh = null; - DriverStation.reportWarning("Error instantiating high camera: " + ex.getMessage(), true); - } - - try { - limelightLow = new Limelight("limelight-low"); - limelightLow.setLightState(Limelight.LightMode.OFF); - } catch (Exception ex) { - limelightLow = null; - DriverStation.reportWarning("Error instantiating low camera: " + ex.getMessage(), true); - } - - cameraStatusSupplier = () -> (currentCameraMode == CAMERA_MODE.ARRIVED); - //cameraConfig(limelightLow, OBJECT_TYPE.CONE, SCORE_POS.HIGH, CAMERA_MODE.IDLE); - SmartDashboard.putNumber("Tx P", 0); - SmartDashboard.putNumber("Tx I", 0); - SmartDashboard.putNumber("Tx D", 0); - - SmartDashboard.putNumber("Ta P", 0); - SmartDashboard.putNumber("Ta I", 0); - SmartDashboard.putNumber("Ta D", 0); - - SmartDashboard.putNumber("Yaw P", 0); - SmartDashboard.putNumber("Yaw I", 0); - SmartDashboard.putNumber("Yaw D", 0); - } - - public void cameraConfig(Limelight l, OBJECT_TYPE t, SCORE_POS p, CAMERA_MODE m) { - currentLimelight = l; - currentGameObject = t; - currentHeightPos = p; - currentCameraMode = m; - } - - - private double Xarray[] = new double[10]; - private int xIndex = 0; - private boolean initDoneX = false; - - public double getAvgTX(double newValue) - { - Xarray[xIndex] = newValue; - xIndex ++; - if(xIndex >= Xarray.length) { - xIndex = 0; - initDoneX = true; - } - - double TXSum = 0; - if(initDoneX) { - for(int i = 0; i < Xarray.length; i++) { - TXSum += Xarray[i]; - } - - return TXSum / Xarray.length; - } - else { - for(int i = 0; i < xIndex; i++) { - TXSum += Xarray[i]; - } - - return TXSum / xIndex; - } - } - - private double areaArray[] = new double[10]; - private int areaIndex = 0; - private boolean initDoneArea = false; - - public double getAvgArea(double newValue) - { - areaArray[areaIndex] = newValue; - areaIndex ++; - if(areaIndex >= areaArray.length) { - areaIndex = 0; - initDoneArea = true; - } - - double TYSum = 0; - if(initDoneArea) { - for(int i = 0; i < areaArray.length; i++) { - TYSum += areaArray[i]; - } - - return TYSum / areaArray.length; - } - else { - for(int i = 0; i < areaIndex; i++) { - TYSum += areaArray[i]; - } - - return TYSum / areaIndex; - } - } - - // Returns ID of AprilTag if one is in front of the robot when this command is called. Otherwise, returns -1. - public int getAprilTagID() { - if (limelightLow != null) { - limelightLow.setPipeline(4); // April tag pipeline - if (limelightLow.hasValidTarget()) { - return limelightLow.getAprilTagID(); - } - } - - return -1; - } - - - public void initVisionCommands( ) { - currentCameraMode = CAMERA_MODE.IDLE; - initDoneX = false; - initDoneArea = false; - xIndex = 0; - areaIndex = 0; - } - - Timer timer = new Timer(); - - public void initVisionPickupOnGround(OBJECT_TYPE objType) { - initVisionCommands(); - - timer.reset(); - timer.start(); - - currentGameObject = objType; - currentHeightPos = SCORE_POS.LOW; - rotationIsNeeded = false; // Reset rotation variable - currentLimelight = limelightLow; - currentLimelight.setLightState(LightMode.OFF); - - // This doesn't work for some reason, so we might need to pass the currentGameObject into the drive command directly. (3/11/2023) - if (currentGameObject == OBJECT_TYPE.CONE) { - goalArea = 2.8; // Alex changed from 3.4 to 2.6 //3.8; // This line is running, so we know the conditional is working (3/11/2023) - currentLimelight.setPipeline(1); - - // Old PID for max 4 m/s - PIDArea.setPID(0.4, 0.01, 0.01); - PIDTX.setPID(0.05, 0.01, 0.01); - PIDYaw.setPID(0, 0, 0); - - // New PID for max 5 m/s - // PIDArea.setPID(0.5, 0, 0.0125); - // PIDTX.setPID(0.05, 0, 0.0125); - // PIDYaw.setPID(0, 0, 0); - } else if (currentGameObject == OBJECT_TYPE.CUBE) { - goalArea = 4.1; // Goal area for cube ground pickup // 4.2 OG TODO: DA VINCI RED SIDE WAS CHANGED TO 4.2 - currentLimelight.setPipeline(2); - - // PIDArea.setPID( - // SmartDashboard.getNumber("Ta P", 0.75), - // SmartDashboard.getNumber("Ta I", 0.0), - // SmartDashboard.getNumber("Ta D", 0.02) - // ); - // PIDTX.setPID( - // SmartDashboard.getNumber("Tx P", 0.05), - // SmartDashboard.getNumber("Tx I", 0.0), - // SmartDashboard.getNumber("Tx D", 0.008) - // ); - - PIDArea.setPID(0.75, 0, 0.02); - PIDTX.setPID(0.05, 0, 0.008); - PIDYaw.setPID(0, 0, 0); - } - else if (currentGameObject == OBJECT_TYPE.ATAG) { - goalArea = 3.8; - currentLimelight.setPipeline(4); - } - else { - PIDArea.setPID(0, 0, 0); - PIDTX.setPID(0, 0, 0); - PIDYaw.setPID(0, 0, 0); - } - } - - public CommandBase VisionPickupOnGround(OBJECT_TYPE objType) { - final PIDController pidAreaFinal = PIDArea; - final PIDController pidTXFinal = PIDTX; - final PIDController pidYawFinal = PIDYaw; - - if(limelightLow != null) { - return Commands.race( - // Constantly run elevator and arm motion magic - // Commands.run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - // Commands.run(() -> elevator.moveMotionMagic(arm.getArmAngle())), - - Commands.sequence( - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", true)), - Commands.runOnce(() -> initVisionPickupOnGround(objType)), - - // Move arm and elevator to near ground position in parallel with approaching target - // Commands.deadline( - // Commands.waitSeconds(2), - // Commands.parallel( // End command once both arm and elevator have reached their target position - // Commands.waitUntil(arm.atTargetPosition), - // Commands.waitUntil(elevator.atTargetPosition), - // Commands.runOnce(() -> arm.setTargetTicks(-328500)), - // Commands.runOnce(() -> elevator.setTargetTicks(-36000)) - // ), - // new RunCommand(() -> driveRotateToTarget(pidAreaFinal, pidTXFinal, pidYawFinal), arm, elevator, claw, drivetrain).until(cameraStatusSupplier) - // ), - - - // Drop arm and elevator so the game piece can be intook - // Commands.race( - // Commands.waitSeconds(5), - // Commands.parallel( // End command once both arm and elevator have reached their target position - // Commands.waitUntil(arm.atTargetPosition), - // Commands.waitUntil(elevator.atTargetPosition), - // Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - // Commands.runOnce(() -> elevator.setTargetTicks(-160000)) - // ) - // ), - - // Open claw/Start claw intake rollers - claw.setPower(-0.3), - new WaitCommand(.5), - - // // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.15), - - // Stow arm/elev - // Commands.race( - // Commands.waitSeconds(5), - // Commands.parallel( // End command once both arm and elevator have reached their target position - // Commands.waitUntil(arm.atTargetPosition), - // Commands.waitUntil(elevator.atTargetPosition), - // Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - // Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - // ) - // ), - - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", false)) - ) - ); - - } - else { - return runOnce(() -> SmartDashboard.putString("Limelight command status:", "Sequence cancelled")); - } - } - - public CommandBase VisionPickupGroundNoArm(OBJECT_TYPE objType, MotorClaw claw) { - final PIDController pidAreaFinal = PIDArea; - final PIDController pidTXFinal = PIDTX; - final PIDController pidYawFinal = PIDYaw; - - // if(limelightLow != null) - { - return - // Constantly run elevator and arm motion magic - // Commands.run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - // Commands.run(() -> elevator.moveMotionMagic(arm.getArmAngle())), - - Commands.sequence( - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", true)), - Commands.runOnce(() -> initVisionPickupOnGround(objType)), - - Commands.race( - new RunCommand(() -> driveRotateToTarget(pidAreaFinal, pidTXFinal, pidYawFinal), arm, elevator, claw, drivetrain).until(cameraStatusSupplier), - Commands.waitSeconds(2) - ), - - // Drop arm and elevator so the game piece can be intook - // Commands.race( - // Commands.waitSeconds(5), - // Commands.parallel( // End command once both arm and elevator have reached their target position - // Commands.waitUntil(arm.atTargetPosition), - // Commands.waitUntil(elevator.atTargetPosition), - // Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmGroundPickup)), - // Commands.sequence( - // Commands.waitSeconds(0.25), - // Commands.runOnce(() -> elevator.setTargetTicks(-160000)) - // ) - // ) - // ), - - // Open claw/Start claw intake rollers - claw.setPower(-0.3), - new WaitCommand(.5), - - // // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.15), - - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", false)) - - ); - - } - // else { - // return runOnce(() -> SmartDashboard.putString("Limelight command status:", "Sequence cancelled")); - // } - } - - public void initVisionPickupOnSubstation(OBJECT_TYPE objType) { - initVisionCommands(); - //int armPositionTicks = ArmConstants.kArmStow; - //int elevatorPositionTicks = ElevatorConstants.kElevatorStow; - - currentGameObject = objType; - - rotationIsNeeded = false; // Reset rotation variable - - currentHeightPos = SCORE_POS.HIGH; - - //armPositionTicks = ArmConstants.kArmSubstation; // Pickup substation - //elevatorPositionTicks = ElevatorConstants.kElevatorSubstation; - currentLimelight = limelightHigh; - rotationIsNeeded = true; - goalYaw = 0; // Facing away from drivers, towards substation - - if (currentGameObject == OBJECT_TYPE.CONE) { - goalArea = 21; // Goal area for cone substation pickup, area is an estimate because a different camera position was used, updated 2/23/2023 - currentLimelight.setPipeline(1); - - PIDArea.setPID( - SmartDashboard.getNumber("Ta P", 0.4), - SmartDashboard.getNumber("Ta I", 0.01), - SmartDashboard.getNumber("Ta D", 0.01) - ); - PIDTX.setPID( - SmartDashboard.getNumber("Tx P", 0.04), - SmartDashboard.getNumber("Tx I", 0.01), - SmartDashboard.getNumber("Tx D", 0.01) - ); - - // PIDArea.setPID(0.4, 0.01, 0.01); - // PIDTX.setPID(0.04, 0.01, 0.01); - PIDYaw.setPID(0, 0, 0); - } else { - goalArea = 0; // Goal area for cube substation pickup - currentLimelight.setPipeline(2); - - - PIDArea.setPID(SmartDashboard.getNumber("Ta P", 0), SmartDashboard.getNumber("Ta I", 0), SmartDashboard.getNumber("Ta D", 0)); - PIDTX.setPID(SmartDashboard.getNumber("Tx P", 0), SmartDashboard.getNumber("Tx I", 0), SmartDashboard.getNumber("Tx D", 0)); - PIDYaw.setPID(0, 0, 0); - } - - //final int armPositionTicksFinal = armPositionTicks; - //final int elevatorPositionTicksFinal = elevatorPositionTicks; - } - public CommandBase VisionPickupOnSubstation(OBJECT_TYPE objType) { - if(limelightHigh != null) { - - return Commands.race( - // Constantly run elevator and arm motion magic - // run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - // run(() -> elevator.moveMotionMagic(arm.getArmAngle())), - - Commands.sequence( - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", true)), - Commands.runOnce(() -> initVisionPickupOnSubstation(objType)), - - // Arm and elevator to selected position - /*Commands.race( - Commands.waitSeconds(5), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmSubstation)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorSubstation)) - ) - ),*/ - - new RunCommand(() -> driveRotateToTarget(PIDArea, PIDTX, PIDYaw), arm, elevator, claw, drivetrain).until(cameraStatusSupplier).withTimeout(5), // Timeout after 30 seconds - - // Open claw/Start claw intake rollers - claw.setPower(-0.3), - new WaitCommand(2), - - // Close claw/stop claw intake rollers/low background rolling to keep control of game piece - claw.setPower(-0.15), - - // Stow arm/elev - Commands.race( - Commands.waitSeconds(5), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - ) - ), - - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Pickup Running", false)) - ) - ); - } - else { - return runOnce(() -> SmartDashboard.putString("Limelight command status:", "Sequence cancelled")); - } - } - - - int armPositionTicks = ArmConstants.kArmStow; - int elevatorPositionTicks = ElevatorConstants.kElevatorStow; - public void initVisionScore(OBJECT_TYPE objType, SCORE_POS pos) { - - initVisionCommands(); - - currentGameObject = objType; - currentHeightPos = pos; - rotationIsNeeded = true; - goalYaw = 0; // All scoring is facing towards our drivers - - switch(currentGameObject) { - case CONE: - currentLimelight = limelightLow; // Use low for testing - currentLimelight.setPipeline(3); // Tape pipeline - currentLimelight.setLightState(LightMode.ON); - - // PID when the max speed was 4 m/s - PIDArea.setPID(2, 0, 0); - PIDTX.setPID(0.08, 0.02, 0.02); - PIDYaw.setPID(0, 0, 0); - - // New PID for max speed of 5 m/s, just calculated (multiplied by 5/4) but has to be tuned - // PIDArea.setPID(2.5, 0, 0); - // PIDTX.setPID(0.1, 0, 0.025); - // PIDYaw.setPID(0, 0, 0); - - goalArea = 0.55; - - switch(currentHeightPos) { - case HIGH: - armPositionTicks = ArmConstants.kArmScore - 15900; // Score high - elevatorPositionTicks = ElevatorConstants.kElevatorScoreHigh; - break; - - case MID: - - armPositionTicks = ArmConstants.kArmScore; // Score mid - elevatorPositionTicks = ElevatorConstants.kElevatorScoreMid; - break; - - case LOW: // Score ground - armPositionTicks = ArmConstants.kArmGroundPickup; - elevatorPositionTicks = ElevatorConstants.kElevatorStow; - break; - } - break; - - case CUBE: - currentLimelight = limelightLow; - currentLimelight.setPipeline(4); // April tag pipeline - - PIDArea.setPID(SmartDashboard.getNumber("Ta P", 0), SmartDashboard.getNumber("Ta I", 0), SmartDashboard.getNumber("Ta D", 0)); - PIDTX.setPID(SmartDashboard.getNumber("Tx P", 0), SmartDashboard.getNumber("Tx I", 0), SmartDashboard.getNumber("Tx D", 0)); - // PIDYaw.setPID(10, 0, 0.2); - PIDYaw.setPID(0, 0, 0); - - goalArea = 7.2; // April tag target area, unsure if correct, updated 2/23/2023 - - switch(currentHeightPos) { - case HIGH: - armPositionTicks = ArmConstants.kArmScore; // Score high - elevatorPositionTicks = ElevatorConstants.kElevatorScoreHigh; - break; - - case MID: - - armPositionTicks = ArmConstants.kArmScore; // Score mid - elevatorPositionTicks = ElevatorConstants.kElevatorScoreMid; - break; - - case LOW: // Score ground - armPositionTicks = ArmConstants.kArmGroundPickup; - elevatorPositionTicks = ElevatorConstants.kElevatorStow; - break; - } - - break; - } - } - - public CommandBase VisionScore(OBJECT_TYPE objType, SCORE_POS pos) { - final PIDController pidAreaFinal = PIDArea; - final PIDController pidTXFinal = PIDTX; - final PIDController pidYawFinal = PIDYaw; - - if (limelightLow != null) { - return Commands.race( - // Constantly run elevator and arm motion magic - Commands.run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - Commands.run(() -> elevator.moveMotionMagic(arm.getArmAngle())), - - Commands.sequence( - Commands.parallel( - Commands.runOnce(() -> SmartDashboard.putString("Vision Score Stage", "Stow")), - Commands.runOnce(() -> initVisionScore(objType, pos)) - ), - - new TurnToAngle(0, drivetrain), // TODO: merge with driveRotateToTarget Yaw PID - - Commands.parallel( - new RunCommand(() -> driveRotateToTarget(pidAreaFinal, pidTXFinal, pidYawFinal), arm, elevator, claw, drivetrain) - .until(cameraStatusSupplier) - .withTimeout(2), - - // Move arm and elevator, arm is moved 0.5 seconds after the elevator to prevent power chain from getting caught - Commands.race( - Commands.waitSeconds(5), // Timeout - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), - Commands.waitSeconds(0.5), - - Commands.parallel( // End when target positions reached - Commands.waitUntil(elevator.atTargetPosition), - Commands.waitUntil(arm.atTargetPosition), - Commands.runOnce(() -> elevator.setTargetTicks(elevatorPositionTicks)) - ) - ) - ) - ), - - new TurnToAngle(0, drivetrain), - - Commands.waitSeconds(0.5), - // Open claw/eject piece with rollers - claw.setPower(1), - - // Wait to outtake - Commands.waitSeconds(.5), - - // Close claw/stop rollers - claw.setPower(0), - - // Stow arm - Commands.race( - Commands.waitSeconds(5), - Commands.parallel( // End command once both arm and elevator have reached their target position - Commands.waitUntil(arm.atTargetPosition), - Commands.waitUntil(elevator.atTargetPosition), - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmStow)), - Commands.runOnce(() -> elevator.setTargetTicks(ElevatorConstants.kElevatorStow)) - ) - ), - - // new TurnToAngle(0, drivetrain), // Turn back towards field after scoring - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Score Running", false)) - ) - ); - } - else { - return runOnce(() -> SmartDashboard.putString("Limelight command status:", "Sequence cancelled")); - } - } - - // Moves arm, but doesn't stow after vision align - public CommandBase VisionScoreNoArm(OBJECT_TYPE objType, SCORE_POS pos) { - final PIDController pidAreaFinal = PIDArea; - final PIDController pidTXFinal = PIDTX; - final PIDController pidYawFinal = PIDYaw; - - if (limelightLow != null) { - return Commands.race( - // Constantly run elevator and arm motion magic - Commands.run(() -> arm.moveArmMotionMagic(elevator.percentExtended())), - Commands.run(() -> elevator.moveMotionMagic(arm.getArmAngle())), - - Commands.sequence( - Commands.parallel( - Commands.runOnce(() -> SmartDashboard.putString("Vision Score Stage", "Stow")), - Commands.runOnce(() -> initVisionScore(objType, pos)) - ), - - Commands.parallel( - new RunCommand(() -> driveRotateToTarget(pidAreaFinal, pidTXFinal, pidYawFinal), arm, elevator, claw, drivetrain) - .until(cameraStatusSupplier) - .withTimeout(0.5), - - // Move arm and elevator, arm is moved 0.5 seconds after the elevator to prevent power chain from getting caught - Commands.race( - Commands.waitSeconds(5), // Timeout - Commands.sequence( - Commands.runOnce(() -> arm.setTargetTicks(ArmConstants.kArmScore)), - // Commands.waitSeconds(0.5), - - Commands.parallel( // End when target positions reached - Commands.waitUntil(elevator.atTargetPosition), - Commands.waitUntil(arm.atTargetPosition), - Commands.runOnce(() -> elevator.setTargetTicks(elevatorPositionTicks)) - ) - ) - ) - ), - - Commands.waitSeconds(0.5), - // Open claw/eject piece with rollers - claw.setPower(1), - - // Wait to outtake - Commands.waitSeconds(.5), - - // Close claw/stop rollers - claw.setPower(0), - - // new TurnToAngle(0, drivetrain), // Turn back towards field after scoring - Commands.runOnce(() -> SmartDashboard.putBoolean("Vision Score Running", false)) - ) - ); - } - else { - return runOnce(() -> SmartDashboard.putString("Limelight command status:", "Sequence cancelled")); - } - } - - public void driveRotateToTarget(PIDController pidArea, PIDController pidTX, PIDController pidYaw) { - // Initialize all variables to 0 - double xSpeed = 0; - double ySpeed = 0; - double rotationSpeed = 0; - - if (currentLimelight == null) - return; - - ChassisSpeeds chassisSpeeds; - - SmartDashboard.putBoolean("Vision has target", currentLimelight.hasValidTarget()); - - if(!currentLimelight.hasValidTarget()) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.WAIT; - } - else { - // NOTE (3/11/2023): This should be commented out, BUT if the PID is still 0, - // you can use this but we still need to find a permenant solution - - // Score cone (low tape, max distance is the charging station) - // pidArea.setP(2); - // pidArea.setD(0); - // pidTX.setP(0.08); - // pidTX.setD(0.02); - - // Cone ground - // pidArea.setP(0.4); - // pidArea.setD(0.01); - // pidTX.setP(0.04); - // pidTX.setD(0.01); - - double calculatedX = getAvgArea(currentLimelight.getArea_avg()); - double calculatedY = getAvgTX(currentLimelight.getXAngle_avg()); - SmartDashboard.putNumber("Vision average X", calculatedX); - SmartDashboard.putNumber("Vision average Y", calculatedY); - - if(currentLimelight.getPipeIndex()==4){ - if (NerdyMath.inRange(calculatedY, -2.2, 1) - && calculatedX > 7) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - currentLimelight.setLightState(LightMode.ON); - return; - } - } - xSpeed = pidArea.calculate(calculatedX, goalArea) * (5/4); - ySpeed = -pidTX.calculate(calculatedY, goalTX) * (5/4); - rotationSpeed = pidYaw.calculate(drivetrain.getImu().getHeading(), goalYaw); - - if (NerdyMath.inRange(xSpeed, -.1, .1) && - NerdyMath.inRange(ySpeed, -.1, .1) && - NerdyMath.inRange(rotationSpeed, -.1, .1)) - { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - currentLimelight.setLightState(LightMode.ON); - } - else{ - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ACTION; - } - } - - SmartDashboard.putString("Vision status", currentCameraMode.toString()); - SmartDashboard.putNumber("Vision X speed", xSpeed); - SmartDashboard.putNumber("Vision Y speed", ySpeed); - } - - // Drive to target without rotation - private void skrttttToTarget(PIDController pidArea, PIDController pidTX) { - // Initialize all variables to 0 - double xSpeed = 0; - double ySpeed = 0; - - if (currentLimelight == null) - return; - - ChassisSpeeds chassisSpeeds; - - SmartDashboard.putBoolean("Vision has target", currentLimelight.hasValidTarget()); - - if(!currentLimelight.hasValidTarget()) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.WAIT; - } - else { - double calculatedX = getAvgArea(currentLimelight.getArea_avg()); - double calculatedY = getAvgTX(currentLimelight.getXAngle_avg()); - SmartDashboard.putNumber("Vision average X", calculatedX); - SmartDashboard.putNumber("Vision average Y", calculatedY); - - xSpeed = pidArea.calculate(calculatedX, goalArea); - ySpeed = -pidTX.calculate(calculatedY, goalTX); - - if (NerdyMath.inRange(xSpeed, -.1, .1) && - NerdyMath.inRange(ySpeed, -.1, .1)) - { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - } - else{ - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ACTION; - } - } - - SmartDashboard.putString("Vision status", currentCameraMode.toString()); - SmartDashboard.putNumber("Vision X speed", xSpeed); - SmartDashboard.putNumber("Vision Y speed", ySpeed); - } - - PIDController pidTX_driveToCubeOnGround = new PIDController(0.05, 0, 0.008); - PIDController pidYaw_driveToCubeOnGround = new PIDController(0, 0, 0); - PIDController pidArea_driveToCubeOnGround = new PIDController(0.8, 0.01, 0.02); //0.75 P OG - - public void driveToCubeOnGround(MotorClaw claw, int timeoutSec) - { - - - //PIDController pidArea, PIDController pidTX, PIDController pidYaw) { - // Initialize all variables to 0 - double xSpeed = 0; - double ySpeed = 0; - double rotationSpeed = 0; - - if (limelightLow == null) - return; - - ChassisSpeeds chassisSpeeds; - - SmartDashboard.putBoolean("Vision has target", limelightLow.hasValidTarget()); - - double elapsedTime = timer.get(); - - if(elapsedTime >= timeoutSec){ - drivetrain.setModuleStates(SwerveDriveConstants.towModuleStates); - drivetrain.stopModules(); - limelightLow.setLightState(LightMode.BLINK); - return; - } - - if(!limelightLow.hasValidTarget()) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.WAIT; - } - else { - // NOTE (3/11/2023): This should be commented out, BUT if the PID is still 0, - // you can use this but we still need to find a permenant solution - - // Score cone (low tape, max distance is the charging station) - // pidArea.setP(2); - // pidArea.setD(0); - // pidTX.setP(0.08); - // pidTX.setD(0.02); - - // Cone ground - // pidArea.setP(0.4); - // pidArea.setD(0.01); - // pidTX.setP(0.04); - // pidTX.setD(0.01); - - double calculatedX = getAvgArea(currentLimelight.getArea_avg()); - double calculatedY = getAvgTX(currentLimelight.getXAngle_avg()); - SmartDashboard.putNumber("Vision average X", calculatedX); - SmartDashboard.putNumber("Vision average Y", calculatedY); - - if(limelightLow.getPipeIndex()==2){ // TODO change it to cube-2 - if (NerdyMath.inRange(calculatedY, -15, 15) - && NerdyMath.inRange(calculatedX, 4.0, 4.9)) { // OG 3.7 to 4.2 , 39 inches TODO: Da Vinci changed from 4.2, 5.0 - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - limelightLow.setLightState(LightMode.ON); - return; - } - } - xSpeed = pidArea_driveToCubeOnGround.calculate(calculatedX, goalArea); - ySpeed = -pidTX_driveToCubeOnGround.calculate(calculatedY, goalTX); - //rotationSpeed = pidYaw.calculate(drivetrain.getImu().getHeading(), goalYaw); - - if (NerdyMath.inRange(xSpeed, -.1, .1) && - NerdyMath.inRange(ySpeed, -.1, .1) && - NerdyMath.inRange(rotationSpeed, -.1, .1)) - { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - limelightLow.setLightState(LightMode.ON); - } - else{ - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ACTION; - } - } - - SmartDashboard.putString("Vision status", currentCameraMode.toString()); - SmartDashboard.putNumber("Vision X speed", xSpeed); - SmartDashboard.putNumber("Vision Y speed", ySpeed); - } - - PIDController pidTX_driveToGridTag = new PIDController(0.5, 0, 0.5); - PIDController pidYaw_driveToGridTag = new PIDController(0, 0, 0); - PIDController pidArea_driveToGridTag = new PIDController(0.5, 0.01, 1.5); //TODO: TUNE - - public void driveToGridTag(MotorClaw claw, int targetId) // if id == -1, don't care - { - - //PIDController pidArea, PIDController pidTX, PIDController pidYaw) { - // Initialize all variables to 0 - double xSpeed = 0; - double ySpeed = 0; - double rotationSpeed = 0; - - if (limelightLow == null) - return; - - ChassisSpeeds chassisSpeeds; - - SmartDashboard.putBoolean("Vision has target", limelightLow.hasValidTarget()); - - if(!limelightLow.hasValidTarget()) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.WAIT; - } - else { - int aid = limelightLow.getAprilTagID(); - if(targetId != -1) { - if(targetId != aid) { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.WAIT; - return; - } - } - - double calculatedX = getAvgArea(currentLimelight.getArea()); - double calculatedY = getAvgTX(currentLimelight.getXAngle()); - SmartDashboard.putNumber("Vision average X", calculatedX); - SmartDashboard.putNumber("Vision average Y", calculatedY); - - if(limelightLow.getPipeIndex()==4){// TAG ID? TODO - if (NerdyMath.inRange(calculatedY, -6, 6) - && NerdyMath.inRange(calculatedX, 3.5, 4.2)) { // TODO: TUNE FOR APRIL TAG - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - limelightLow.setLightState(LightMode.ON); - return; - } - } - xSpeed = pidArea_driveToGridTag.calculate(calculatedX, goalArea); - ySpeed = -pidTX_driveToGridTag.calculate(calculatedY, goalTX); - //rotationSpeed = pidYaw.calculate(drivetrain.getImu().getHeading(), goalYaw); - - if (NerdyMath.inRange(xSpeed, -.1, .1) && - NerdyMath.inRange(ySpeed, -.1, .1) && - NerdyMath.inRange(rotationSpeed, -.1, .1)) - { - chassisSpeeds = new ChassisSpeeds(0, 0, 0); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ARRIVED; - limelightLow.setLightState(LightMode.ON); - } - else{ - chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); - SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); - drivetrain.setModuleStates(moduleStates); - currentCameraMode = CAMERA_MODE.ACTION; - } - } - - SmartDashboard.putString("Vision status", currentCameraMode.toString()); - SmartDashboard.putNumber("Vision X speed", xSpeed); - SmartDashboard.putNumber("Vision Y speed", ySpeed); - } - - // Mutators - - /*public CommandBase updateCurrentGameObject(OBJECT_TYPE oType) { - return runOnce( - () -> { - currentGameObject = oType; - }); - }*/ - - // public CommandBase updateCurrentHeight(SCORE_POS sPos) { - // return runOnce( - // () -> { - // currentHeightPos = sPos; - // }); - // } - - // Smartdashboard - - @Override - public void reportToSmartDashboard(LOG_LEVEL level) { - switch (level) { - case OFF: - break; - case ALL: - case MEDIUM: - if( currentGameObject != null) - SmartDashboard.putString("Vision Current Object", currentGameObject.toString()); - if(currentHeightPos != null) - SmartDashboard.putString("Vision Current Height", currentHeightPos.toString()); - if(currentLimelight != null) { - SmartDashboard.putString("Vision Current Limelight", currentLimelight.getName()); - SmartDashboard.putNumber("Vision Current Pipeline", currentLimelight.getPipeIndex()); - } - else { - SmartDashboard.putString("Vision Current Limelight", "L + ratio"); - } - case MINIMAL: - break; - } - } - - @Override - public void initShuffleboard(LOG_LEVEL level) { - if (level == LOG_LEVEL.OFF || level == LOG_LEVEL.MINIMAL) { - return; - } - ShuffleboardTab tab = Shuffleboard.getTab(this.getName()); - switch (level) { - case OFF: - break; - case ALL: - case MEDIUM: - tab.addString("Vision Current Object", () -> { - if (currentGameObject != null) { - return currentGameObject.toString(); - } - return ""; - }); - tab.addString("Vision Current Height", () -> { - if(currentHeightPos != null) { - return currentHeightPos.toString(); - } - return ""; - }); - tab.addBoolean("has target", () -> { - if(currentLimelight != null) { - return currentLimelight.hasValidTarget(); - } - return false; - }); - tab.addNumber("Pipeline", () -> { - if(currentLimelight != null) { - return currentLimelight.getPipeIndex(); - } - return -1; - }); - tab.addString("Current Limelight", () -> { - if(currentLimelight != null) { - return currentLimelight.getName(); - } - return "L + ratio"; - }); - case MINIMAL: - break; - } - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelperUser.java b/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelperUser.java new file mode 100644 index 0000000..be1d132 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelperUser.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.vision.jurrasicMarsh; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.vision.jurrasicMarsh.LimelightHelpers; + +public class LimelightHelperUser extends SubsystemBase { + String limelightName = "limelight"; + + /** Creates a new ExampleSubsystem. */ + public LimelightHelperUser(String limelightName) { + this.limelightName = limelightName; + } + + /** + * Example command factory method. + * + * @return a command + */ + public CommandBase exampleMethodCommand() { + // Inline construction of command goes here. + // Subsystem::RunOnce implicitly requires `this` subsystem. + return runOnce( + () -> { + /* one-time action goes here */ + }); + } + + private Pose3d getRawPose3d() { + return LimelightHelpers.getLatestResults(limelightName).targetingResults.getBotPose3d(); + } + + private double getRawX() { + return getRawPose3d().getX(); + } + + private double getRawY() { + return getRawPose3d().getY(); + } + + private double getRawZ() { + return getRawPose3d().getZ(); + } + + private Rotation3d getRawRotation() { + return getRawPose3d().getRotation(); + } + + public Pose3d getPose3d() { + return new Pose3d(getRawX() + VisionConstants.fieldXOffset, getRawY() + VisionConstants.fieldYOffset, getRawZ(), getRawRotation()); + } + + public double getX() { + return getPose3d().getX(); + } + + public double getY() { + return getPose3d().getY(); + } + + public void reportToSmartDashboard() { + SmartDashboard.putNumber("Vision Robot X", getX()); + SmartDashboard.putNumber("Vision Robot Y", getY()); + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelpers.java b/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelpers.java new file mode 100644 index 0000000..db256d1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/jurrasicMarsh/LimelightHelpers.java @@ -0,0 +1,780 @@ +//LimelightHelpers v1.2.1 (March 1, 2023) + +package frc.robot.subsystems.vision.jurrasicMarsh; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public LimelightResults() { + targetingResults = new Results(); + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static double getNeuralClassID(String limelightName) { + return getLimelightNTDouble(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + System.err.println("lljson error: " + e.getMessage()); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalPotatoMine.java b/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalPotatoMine.java new file mode 100644 index 0000000..769d61c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalPotatoMine.java @@ -0,0 +1,104 @@ +package frc.robot.subsystems.vision.primalWallnut; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants.SwerveDriveConstants; +import frc.robot.subsystems.swerve.SwerveDrivetrain; +import frc.robot.subsystems.vision.Limelight; +import frc.robot.subsystems.vision.Limelight; +import frc.robot.subsystems.vision.Limelight.LightMode; +import frc.robot.util.NerdyMath; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class PrimalPotatoMine { + + private Limelight limelight; + private SwerveDrivetrain drivetrain; + + private double goalArea = 4.1; + private double goalTX = 0; + private double goalYaw = 0; + + final PIDController PIDArea = new PIDController(0.75, 0, 0.02); + final PIDController PIDTX = new PIDController(0.05, 0, 0.008); + final PIDController PIDYaw = new PIDController(0, 0, 0); + + public PrimalPotatoMine(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + try { + limelight = new Limelight("limelight-low"); + limelight.setLightState(Limelight.LightMode.OFF); + SmartDashboard.putString("LL Status", "Error Instantiating"); + } catch (Exception ex) { + limelight = null; + } + + limelight.setPipeline(2); + } + + + public CommandBase PickupGroundNoArm() { + return Commands.race( + new RunCommand(() -> driveToTarget()), + Commands.waitSeconds(2) + ); + } + + + public void driveToTarget() { + double xSpeed = 0; + double ySpeed = 0; + double rotationSpeed = 0; + + if(limelight == null) { + return; + } + + ChassisSpeeds chassisSpeeds; + + if(!limelight.hasValidTarget()) { + chassisSpeeds = new ChassisSpeeds(0, 0, 0); + SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + drivetrain.setModuleStates(moduleStates); + SmartDashboard.putString("driveToTarget status", "no valid target"); + } + else { + double averageX = limelight.getArea_avg(); + double averageY = limelight.getArea_avg(); + + if((NerdyMath.inRange(averageY, -2, 2)) && (averageX > 7)) { + chassisSpeeds = new ChassisSpeeds(0, 0, 0); + SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + drivetrain.setModuleStates(moduleStates); + SmartDashboard.putString("driveToTarget status", "averageY and averageX conditions met"); + return; + } + + xSpeed = PIDArea.calculate(averageX, goalArea); + ySpeed = PIDTX.calculate(averageY, goalTX); + rotationSpeed = PIDYaw.calculate(drivetrain.getImu().getHeading(), goalYaw); + + if(NerdyMath.inRange(xSpeed, -0.1, 0.1) && + NerdyMath.inRange(ySpeed, -0.1, 0.1)) { + chassisSpeeds = new ChassisSpeeds(0, 0, 0); + SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + drivetrain.setModuleStates(moduleStates); + SmartDashboard.putString("driveToTarget status", "xSpeed ySpeed in range conditions met"); + return; + } + else { + chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed); + SwerveModuleState[] moduleStates = SwerveDriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds); + drivetrain.setModuleStates(moduleStates); + SmartDashboard.putString("driveToTarget status", "chassisSpeeds in motion"); + return; + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalSunflower.java b/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalSunflower.java new file mode 100644 index 0000000..4416de9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/primalWallnut/PrimalSunflower.java @@ -0,0 +1,257 @@ +package frc.robot.subsystems.vision.primalWallnut; + +import java.util.List; + +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPoint; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Constants.PathPlannerConstants; +import frc.robot.Constants.SwerveDriveConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.Reportable; +// import frc.robot.subsystems.Shooter; +// import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.Reportable.LOG_LEVEL; +import frc.robot.subsystems.swerve.SwerveDrivetrain; +import frc.robot.subsystems.vision.Limelight; +import frc.robot.subsystems.vision.Limelight.LightMode; +import frc.robot.subsystems.vision.jurrasicMarsh.LimelightHelperUser; +import frc.robot.subsystems.vision.jurrasicMarsh.LimelightHelpers; + +public class PrimalSunflower implements Reportable { + // XYZ coordinates of all grid positions. + // Relative to bottom left corner of field (closest to blue alliance cable side) being (0, 0, 0) + private Double[][] gridPositions = { + // {6.5, 1.1, 0.0}, + // {6.5, 0.4, 0.0}, + // {6.5, -0.15, 0.0}, + // {6.5, -0.7, 0.0}, + // {6.5, -1.25, 0.0}, + // {6.5, -1.8, 0.0}, + // {6.5, -2.35, 0.0}, + // {6.5, -2.95, 0.0}, + // {6.5, -3.55, 0.0} + + {1.75, 0.41, 0.0}, + {1.75, 1.06, 0.0}, + {1.75, 1.61, 0.0}, + {1.75, 2.19, 0.0}, + {1.75, 2.75, 0.0}, + {1.75, 3.30, 0.0}, + {1.75, 3.87, 0.0}, + {1.75, 4.42, 0.0}, + {1.75, 5.06, 0.0} + }; + + //robot position + private Double[] robotPos = {0.0, 0.0, 0.0}; + + //points in the path to get to the closest grid + PathPoint firstPoint = new PathPoint(new Translation2d(robotPos[0], robotPos[1]), Rotation2d.fromDegrees(0)); + PathPoint secondPoint = new PathPoint(new Translation2d(robotPos[0], robotPos[1]), Rotation2d.fromDegrees(0)); + PathPoint thirdPoint = new PathPoint(new Translation2d(robotPos[0], robotPos[1]), Rotation2d.fromDegrees(0)); + + private Limelight limelight; + private LimelightHelperUser limelightUser; + + private SwerveDrivetrain drivetrain; + + private PIDController PIDArea = new PIDController(0, 0, 0); + private PIDController PIDTX = new PIDController(0, 0, 0); + private PIDController PIDYaw = new PIDController(0, 0, 0); + + private String llname; + + private Field2d field; + + /* + * Params: + * limelightName = name of the limelight + * drivetrain = swerve drive + */ + public PrimalSunflower(String limelightName, SwerveDrivetrain drivetrain) { + // this.drivetrain = drivetrain; UNCOMMENR LATER + try { + SmartDashboard.putBoolean("LimelightHelper inited", true); + limelight = new Limelight(limelightName); + limelight.setLightState(LightMode.OFF); + limelight.setPipeline(4); + limelightUser = new LimelightHelperUser(limelightName); + + } catch (Exception e) { + limelight = null; + // DriverStation.reportWarning("Error instantiating limelight with name " + limelightName + ": " + e.getMessage(), true); + SmartDashboard.putBoolean("LimelightHelper inited", false); + limelightUser = null; + } + + field = new Field2d(); + + // SmartDashboard.putNumber("Tx P", 0); + // SmartDashboard.putNumber("Tx I", 0); + // SmartDashboard.putNumber("Tx D", 0); + + // SmartDashboard.putNumber("Ta P", 0); + // SmartDashboard.putNumber("Ta I", 0); + // SmartDashboard.putNumber("Ta D", 0); + + // SmartDashboard.putNumber("Yaw P", 0); + // SmartDashboard.putNumber("Yaw I", 0); + // SmartDashboard.putNumber("Yaw D", 0); + } + + + //get robot position if limelight has target else, return 0, 0, 0 (https://docs.limelightvision.io/en/latest/coordinate_systems_fiducials.html#field-space) + public Double[] generateSun() { + Double[] yee = {0.0, 0.0, 0.0}; + if (limelight == null) { + return yee; + } + limelight.setPipeline(VisionConstants.kAprilTagPipeline); + + Pose3d pos = new Pose3d(); + if(limelight.hasValidTarget()) { + pos = limelightUser.getPose3d(); // Replace w different met. + field.setRobotPose(pos.toPose2d()); + return new Double[]{pos.getX(), pos.getY(), pos.getZ()}; + } + return yee; + } + + public Pose3d getPose3d() { + if (limelight == null) { + return null; + } + + limelight.setPipeline(VisionConstants.kAprilTagPipeline); + + if(limelight.hasValidTarget()) { + return limelightUser.getPose3d(); // Replace w different met? Idk i just copied it from generateSun() + } + + return null; + } + + /** + * @return index of the closest grid to the robot + */ + public int getClosestZombieLane() { + robotPos = generateSun(); + int gridNumber = 0; + Double distance = Math.sqrt(Math.pow(gridPositions[0][1] - robotPos[1], 2) + Math.pow(gridPositions[0][0] - robotPos[0], 2)); // distance formula + for (int i = 0; i < gridPositions.length; i++) { + Double newDistance = Math.sqrt(Math.pow(gridPositions[i][1] - robotPos[1], 2) + Math.pow(gridPositions[i][0] - robotPos[0], 2)); // distance formula + if(newDistance < distance) { + distance = newDistance; + gridNumber = i; + } + } + + return gridNumber; + } + + /** + * @return position of the closest grid to the robot + */ + public Double[] getClosestZombieTile() { + return gridPositions[getClosestZombieLane()]; + } + + /** + * @return PathPlannerTrajectory to get to the closest grid + */ + public PathPlannerTrajectory toNearestGrid() { + robotPos = generateSun(); + Double[] gridPos = getClosestZombieTile(); + + return PathPlanner.generatePath( + PathPlannerConstants.kPPPathConstraints, + List.of( + new PathPoint(new Translation2d(gridPos[0], gridPos[1]), Rotation2d.fromDegrees(180)) + ) + ); + } + + /** + * Debug method to generate trajectory to nearest grid and display on shuffleboard. + * + * @return Trajectory to get to the closest grid. + */ + public Trajectory toNearestGridDebug(SwerveDrivetrain swerveDrive) { + robotPos = generateSun(); + Double[] gridPos = getClosestZombieTile(); // Get coordinates of closest grid + + Trajectory trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + swerveDrive.getPose(), + new Pose2d(new Translation2d(gridPos[0], gridPos[1]), Rotation2d.fromDegrees(180)) + ), + new TrajectoryConfig(PathPlannerConstants.kPPMaxVelocity, PathPlannerConstants.kPPMaxAcceleration) // constants for debugging purposes + ); + + field.getObject("traj").setTrajectory(trajectory); + return trajectory; + } + + public void reportToSmartDashboard(LOG_LEVEL priority) { + if(limelightUser != null) limelightUser.reportToSmartDashboard(); + } + + public void initShuffleboard(LOG_LEVEL level) { + if (level == LOG_LEVEL.OFF) { + return; + } + ShuffleboardTab tab; + + switch (level) { + case OFF: + break; + case ALL: + tab = Shuffleboard.getTab("Vision"); + tab.addNumber("Robot Pose X", () -> generateSun()[0]); + tab.addNumber("Robot Pose Y", () -> generateSun()[1]); + tab.addNumber("Robot Pose Z", () -> generateSun()[2]); + + tab.addNumber("Closest Grid X", () -> getClosestZombieTile()[0]); + tab.addNumber("Closest Grid Y", () -> getClosestZombieTile()[1]); + tab.addNumber("Closest Grid Z", () -> getClosestZombieTile()[2]); + + tab.addNumber("Closest Grid ID", () -> getClosestZombieLane()); + tab.addBoolean("AprilTag Found", () -> limelight.hasValidTarget()); + + // Only trajectory point is the grid position now. + // tab.addNumber("Traj Point 1 Pose X", () -> firstPoint.position.getX()); + // tab.addNumber("Traj Point 1 Pose Y", () -> firstPoint.position.getY()); + + // tab.addNumber("Traj Point 2 Pose X", () -> secondPoint.position.getX()); + // tab.addNumber("Traj Point 2 Pose Y", () -> secondPoint.position.getY()); + + // tab.addNumber("Traj Point 3 Pose X", () -> thirdPoint.position.getX()); + // tab.addNumber("Traj Point 3 Pose Y", () -> thirdPoint.position.getY()); + + tab.add("Field Position", field).withSize(6, 3); + case MEDIUM: + + case MINIMAL: + + break; + } + } +}