diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 198b32f..658699a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -9,6 +9,8 @@ package frc4388.robot; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.geometry.Translation2d; + import frc4388.utility.LEDPatterns; import frc4388.utility.Gains; @@ -119,24 +121,29 @@ public final class Constants { } public static final class VisionConstants { - public static final String NAME = "photonCamera"; + // public static final String NAME = "photonCamera"; - public static final int LIME_HIXELS = 640; - public static final int LIME_VIXELS = 480; + // public static final int LIME_HIXELS = 640; + // public static final int LIME_VIXELS = 480; - public static final double H_FOV = 59.6; - public static final double V_FOV = 45.7; + // public static final double H_FOV = 59.6; + // public static final double V_FOV = 45.7; - public static final double LIME_HEIGHT = 6.0; - public static final double LIME_ANGLE = 55.0; + // public static final double LIME_HEIGHT = 6.0; + // public static final double LIME_ANGLE = 55.0; - // public static final double HIGH_TARGET_HEIGHT = 46.0; - public static final double HIGH_TAPE_HEIGHT = 44.0; + // // public static final double HIGH_TARGET_HEIGHT = 46.0; + // public static final double HIGH_TAPE_HEIGHT = 44.0; - // public static final double MID_TARGET_HEIGHT = 34.0; - public static final double MID_TAPE_HEIGHT = 24.0; + // // public static final double MID_TARGET_HEIGHT = 34.0; + // public static final double MID_TAPE_HEIGHT = 24.0; - public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value + + public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609); + public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593); + + public static final double SpeakerBubbleDistance = 1.5; } @@ -160,9 +167,10 @@ public final class Constants { public static final class ShooterConstants { public static final int LEFT_SHOOTER_ID = 15; // final public static final int RIGHT_SHOOTER_ID = 16; // final + public static final double SHOOTER_SPEED = 1.0; // final public static final double SHOOTER_IDLE = 0.4; // final - + public static final double SHOOTER_IDLE_LIMELIGHT = 0.8; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3ca9bfa..4190119 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -50,12 +50,7 @@ public class Robot extends TimedRobot { * LiveWindow and SmartDashboard integrated updating. */ @Override - public void robotPeriodic() { - // This has to be run after the m_robotContainer is initiated - // It will instantly return after it is run - m_robotContainer.autoAlign.updateAlliance(); - - + public void robotPeriodic() { m_robotTime.updateTimes(); //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f4e37f8..06829b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -51,11 +51,14 @@ public class RobotContainer { m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, + m_robotMap.gyro); - - private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter); + /* Limelight */ + private final Limelight limelight = new Limelight(); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); + private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight); + + private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor); @@ -64,8 +67,8 @@ public class RobotContainer { private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); /* Autos */ - private Limelight limelight = new Limelight(); - public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + // This shoud be in a SequentialCommandGroup vvv + //public AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight, false); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt"); @@ -87,9 +90,10 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) ); + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( new RunCommand(() -> MoveToSpeaker.execute()), - new RunCommand(() -> autoAlign.execute()) + new RunCommand(() -> new AutoAlign(m_robotSwerveDrive, limelight, false)) ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index fe02b6d..4b4ee83 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -22,15 +22,18 @@ public class AutoAlign extends Command { private Optional alliance; - public AutoAlign(SwerveDrive swerve, Limelight limelight) { + private boolean isFinished; + private boolean isReverseFinished; + + private boolean reverseAfterFinish; + private Translation2d[] moveStickReplayArr; + private Translation2d[] rotStickReplayArr; + private int replayIndex; + + public AutoAlign(SwerveDrive swerve, Limelight limelight, boolean reverseAfterFinish) { this.swerve = swerve; this.limelight = limelight; - } - - public void updateAlliance() { - if(alliance == null){ - alliance = DriverStation.getAlliance(); - } + this.reverseAfterFinish = reverseAfterFinish; } private Translation2d calcMoveStick(){ @@ -42,6 +45,8 @@ public class AutoAlign extends Command { final boolean isred = alliance.get() == DriverStation.Alliance.Red; + + // This is not math double stickX = curX * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; double stickY = curY * AutoAlignConstants.MoveSpeed * AutoAlignConstants.RotSpeed; @@ -65,14 +70,63 @@ public class AutoAlign extends Command { return new Translation2d(stickX, stickY); } + // Called when the command is initially scheduled. + @Override + public final void initialize() { + if(reverseAfterFinish){ + isReverseFinished = false; + replayIndex = 0; + moveStickReplayArr = new Translation2d[]{}; + rotStickReplayArr = new Translation2d[]{}; + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override public void execute() { // Update limelight pos pose = limelight.getPose(); - Translation2d moveStick = calcMoveStick(); - Translation2d rotStick = calcRotStick(); + // These must be 0, or it will error + Translation2d moveStick = new Translation2d(0, 0); + Translation2d rotStick = new Translation2d(0, 0); + + // Regular replay + if(!isFinished){ + moveStick = calcMoveStick(); + rotStick = calcRotStick(); + + // This is a way of appending... + if(reverseAfterFinish){ + moveStickReplayArr[moveStickReplayArr.length] = moveStick; + rotStickReplayArr[rotStickReplayArr.length] = rotStick; + } + + // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){ + // replayIndex + // } + isFinished = limelight.isNearSpeaker(); + + // If reverseAfterFinish, then loop back over and replay + }else if(reverseAfterFinish && !isReverseFinished){ + // Get reverse direction + moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; + rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1]; + + // Invert sticks + moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1); + rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1); + + replayIndex++; + } // This would greatly benifit from having feild Relative implemented. swerve.driveWithInput(moveStick, rotStick, false); } + + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return isFinished && (isReverseFinished || !reverseAfterFinish); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index ce7bfe1..a0ea95f 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -6,30 +6,75 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTableInstance; +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.SubsystemBase; +import frc4388.robot.Constants.VisionConstants; + // Look at vvv for networktables stuff // https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data public class Limelight extends SubsystemBase { + + // [X, Y, Z, Roll, Pitch, Yaw] private double[] cameraPose; + private boolean isTag; - public Pose2d getPose() { - //TODO - Get actual values! + private Pose2d pose; + private boolean isNearSpeaker; - double x = 0; - double y = 0; - double yaw = 0; + public boolean getIsTag() { + return isTag; + } + + private void update() { + if(!isTag){ + return; + } + + double x = cameraPose[0]; + double y = cameraPose[1]; + double yaw = cameraPose[5]; Rotation2d rot = Rotation2d.fromDegrees(yaw); - return new Pose2d(x, y, rot); + pose = new Pose2d(x, y, rot); + + boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; + + double distance; + + if(isRed){ + distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + }else{ + distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + } + + isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; + } + + public Pose2d getPose() { + return pose; + } + + public boolean isNearSpeaker() { + return isNearSpeaker; } @Override public void periodic() { // This method will be called once per scheduler run - cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); + double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + + SmartDashboard.putBoolean("Apriltag", isTag); + + if(newPose != cameraPose){ + cameraPose = newPose; + update(); + } } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 8d340ed..4eba3cb 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -20,16 +20,28 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; + private Limelight limelight; + + // 0 = Stop + // 1 = Idle, no limelight + // 2 = limelight + // 3 = Shooting + private int shooterMode; + + /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) { leftShooter = leftTalonFX; rightShooter = rightTalonFX; + this.limelight = limelight; + leftShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast); } public void spin() { + shooterMode = 3; spin(ShooterConstants.SHOOTER_SPEED); } @@ -39,16 +51,30 @@ public class Shooter extends SubsystemBase { } public void stop() { + shooterMode = 0; spin(0.d); } public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); + if(limelight.isNearSpeaker()){ + shooterMode = 2; + spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + }else{ + shooterMode = 1; + spin(ShooterConstants.SHOOTER_IDLE); + } } @Override public void periodic() { // This method will be called once per scheduler run + + if(limelight.isNearSpeaker() && shooterMode == 0){ + shooterMode = 1; + spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + } + + SmartDashboard.putNumber("Shooter Speed mode", shooterMode); SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java deleted file mode 100644 index 371f621..0000000 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc4388.robot.subsystems; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -public class Vision { - private final NetworkTableEntry m_isTags; - private final NetworkTableEntry m_xPoses; - private final NetworkTableEntry m_yPoses; - private final NetworkTableEntry m_zPoses; - - public Vision() { - final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag"); - - m_isTags = tagTable.getEntry("IsTag"); - m_xPoses = tagTable.getEntry("TagPosX"); - m_yPoses = tagTable.getEntry("TagPosY"); - m_zPoses = tagTable.getEntry("TagPosZ"); - } - - public AprilTag[] getAprilTags() { - if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - - double xarr[] = m_xPoses.getDoubleArray(new double[] {}); - double yarr[] = m_yPoses.getDoubleArray(new double[] {}); - double zarr[] = m_zPoses.getDoubleArray(new double[] {}); - - AprilTag tags[] = new AprilTag[xarr.length]; - for (int i = 0; i < tags.length; i++) { - tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d())); - } - - return tags; - } -}