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 082c376..f870106 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,27 +121,38 @@ 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 = 3; + public static final double targetPosDistance = 1.5; } - + + public static final class AutoAlignConstants { + public static final double MoveSpeed = 0.0; + public static final double RotSpeed = 0.0; + } + public static final class DriveConstants { public static final int DRIVE_PIGEON_ID = 14; @@ -157,7 +170,7 @@ public final class Constants { public static final int RIGHT_SHOOTER_ID = 16; // final public static final double SHOOTER_SPEED = 0.35; // 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 dca3e82..7a67970 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -49,7 +49,7 @@ public class Robot extends TimedRobot { * LiveWindow and SmartDashboard integrated updating. */ @Override - public void robotPeriodic() { + 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 67cfede..4e11195 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,8 +15,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.commands.Autos.AutoAlign; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; @@ -50,11 +52,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); + + 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); @@ -68,7 +73,6 @@ public class RobotContainer { private final VirtualController m_virtualDriver = new VirtualController(0); private final VirtualController m_virtualOperator = new VirtualController(1); - private Limelight limelight = new Limelight(); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup( @@ -76,6 +80,33 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.spin()) ); + private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spin()), + new InstantCommand(() -> m_robotIntake.handoff()) + ); + + private SequentialCommandGroup intakeInToOut = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.rotateArmOut2(), m_robotIntake), + new RunCommand(() -> m_robotIntake.limitNote(), m_robotIntake).until(m_robotIntake.getArmFowardLimitState()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter) + ); + + + private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); + + private SequentialCommandGroup autoShoot = new SequentialCommandGroup( + // MoveToSpeaker, + autoAlign, + new InstantCommand(() -> m_robotShooter.spin()), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotShooter.idle()), + new InstantCommand(() -> autoAlign.reverse()), + autoAlign.asProxy() + ); + + private SequentialCommandGroup i = new SequentialCommandGroup( intakeToShootStuff, intakeToShoot ); diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java new file mode 100644 index 0000000..ce8b37c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -0,0 +1,208 @@ +package frc4388.robot.commands.Autos; +import frc4388.robot.subsystems.Limelight; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.Constants.AutoAlignConstants; +import frc4388.robot.Constants.VisionConstants; +import edu.wpi.first.wpilibj2.command.Command; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; + +import java.util.Optional; + +import org.opencv.core.RotatedRect; + +import edu.wpi.first.math.geometry.Pose2d; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class AutoAlign extends Command { + private SwerveDrive swerve; + private Limelight limelight; + private Pose2d pose; + private Translation2d targetPos; + private Rotation2d targetRot; + + private Optional alliance; + private boolean isRed; + + private boolean isFinished; + private boolean isReverseFinished; + + private boolean reverseAfterFinish; + private Translation2d[] moveStickReplayArr; + private Translation2d[] rotStickReplayArr; + private int replayIndex; + + // PID Stuff + private double prevError; + private double cumError; + + public AutoAlign(SwerveDrive swerve, Limelight limelight) { + this.swerve = swerve; + this.limelight = limelight; + } + + // Calc the closest point on a circle, to the center of the speaker + private Translation2d calcTargetPos(){ + final double R = VisionConstants.targetPosDistance; + final double cX; + final double cY; + if(isRed){ + cX = VisionConstants.RedSpeakerCenter.getX(); + cY = VisionConstants.RedSpeakerCenter.getY(); + }else{ + cX = VisionConstants.BlueSpeakerCenter.getX(); + cY = VisionConstants.BlueSpeakerCenter.getY(); + } + final double pX = pose.getX(); + final double pY = pose.getY(); + + // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point + double vX = pX - cX; + double vY = pY - cY; + double magV = Math.sqrt(vX*vX + vY*vY); + double aX = cX + vX / magV * R; + double aY = cY + vY / magV * R; + + return new Translation2d(aX, aY); + } + + // Calculate the angle facing the center, at the target rot + private Rotation2d calcTargetRot() { + final double R = VisionConstants.targetPosDistance; + final double cX; + final double cY; + if(isRed){ + cX = VisionConstants.RedSpeakerCenter.getX(); + cY = VisionConstants.RedSpeakerCenter.getY(); + }else{ + cX = VisionConstants.BlueSpeakerCenter.getX(); + cY = VisionConstants.BlueSpeakerCenter.getY(); + } + final double pX = pose.getX(); + final double pY = pose.getY(); + + final double dX = pX - cX; + final double dY = pY - cY; + + final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360); + + return Rotation2d.fromDegrees(yaw); + } + + // Clamp to a circle, like an xbox controller + private Translation2d clamp(double oldX, double oldY){ + // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley + final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360; + final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI)); + final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI)); + + final double newX = Math.max(-maxX, Math.min(maxX, oldX)); + final double newY = Math.max(-maxY, Math.min(maxY, oldY)); + + return new Translation2d(newX, newY); + } + + private Translation2d calcMoveStick(){ + final double curX = pose.getX(); + final double curY = pose.getY(); + + // I think this might work, assuming the constants are good + double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed; + double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed; + + return clamp(stickX, stickY); + } + + private Translation2d calcRotStick(){ + double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); + cumError += error * .02; // 20 ms + double delta = error - prevError; + + final double kP = 4; + final double kI = 4; + final double kD = 4; + final double kF = 4; + + double output = error * kP; + output += cumError * kI; + output += delta * kD; + output += kF; + + prevError = error; + return clamp(output, 0); + } + + public void reverse() { + this.reverseAfterFinish = true; + } + + // Called when the command is initially scheduled. + @Override + public final void initialize() { + isRed = alliance.get() == DriverStation.Alliance.Red; + if(reverseAfterFinish){ + // isReverseFinished = false; + replayIndex = 0; + }else{ + 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(); + + // These must be 0, or it will error + Translation2d moveStick = new Translation2d(0, 0); + Translation2d rotStick = new Translation2d(0, 0); + + // Regular replay + if(!isFinished){ + targetPos = calcTargetPos(); + targetRot = calcTargetRot(); + + moveStick = calcMoveStick(); + rotStick = calcRotStick(); + + // This is a way of appending... + 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 in reverse + }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++; + + if(replayIndex >= moveStickReplayArr.length){ + reverseAfterFinish = true; + } + } + + // This would greatly benifit from having feild Relative implemented. + swerve.driveWithInput(moveStick, rotStick, true); + } + + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return isFinished && (isReverseFinished || !reverseAfterFinish); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index 989b55b..9ece406 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -6,37 +6,77 @@ 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; + private boolean isTag; + + private Pose2d pose; + private boolean isNearSpeaker; public boolean getIsTag() { return isTag; } - public Pose2d getPose() { - //TODO - Get actual values! + private void update() { + if(!isTag){ + return; + } - double x = 0; - double y = 0; - double yaw = 0; + 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; + + SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + SmartDashboard.putBoolean("Apriltag", isTag); + SmartDashboard.putNumber("speakerDistance", distance); + } + + public Pose2d getPose() { + return pose; + } + + public boolean isNearSpeaker() { + return isNearSpeaker; } @Override public void periodic() { // This method will be called once per scheduler run - + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false); - cameraPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("camerapose_targetspace").getDoubleArray(new double[6]); + double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + + 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 776dd98..d3100a2 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,8 +21,9 @@ public class Shooter extends SubsystemBase { private TalonFX leftShooter; private TalonFX rightShooter; - + private double smartDashboardShooterSpeed; + /** Creates a new Shooter. */ public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) { leftShooter = leftTalonFX; @@ -66,7 +67,7 @@ public class Shooter extends SubsystemBase { } public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); + spin(ShooterConstants.SHOOTER_IDLE); } @Override 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; - } -} diff --git a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java index c0384db..fe15818 100644 --- a/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.configurable; +package frc4388.utility.Configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java index c0384db..fe15818 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableDouble.java @@ -1,4 +1,4 @@ -package frc4388.utility.configurable; +package frc4388.utility.Configurable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;