Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 08:48:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Lots of bs (im geekin)

Browse Source
This commit is contained in:
Abhishrek05
2024-02-16 21:49:50 -07:00
parent 2825af3cde
commit 890a3de02a
11 changed files with 135 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Apriltags.java
-36
View File
@@ -1,36 +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.NetworkTable;
//import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
public class Apriltags {
public static class Tag {
public boolean visible = true;
public double x, y, z = 0;
public double ry, rp, rr = 0;
}
public Tag getTagPosRot() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
final Tag tag = new Tag();
tag.visible = isAprilTag();
tag.x = tagTable.getEntry("TagPosX").getDouble(0);
tag.y = tagTable.getEntry("TagPosY").getDouble(0);
tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
return tag;
}
public boolean isAprilTag() {
final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
return tagTable.getEntry("IsTag").getBoolean(false);
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+33 -10
View File
@@ -35,6 +35,8 @@ public class Intake extends SubsystemBase {
private BooleanSupplier sup = () -> true;
private BooleanSupplier dup = () -> false;
private double val;
@@ -53,9 +55,6 @@ public class Intake extends SubsystemBase {
reverseLimit.enableLimitSwitch(true);
intakeMotor.restoreFactoryDefaults();
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
@@ -76,7 +75,6 @@ public class Intake extends SubsystemBase {
//Rotate robot in for handoff
public void rotateArmIn() {
//pivot.set(IntakeConstants.PIVOT_SPEED);
pivot.set(IntakeConstants.PIVOT_SPEED);
}
@@ -90,6 +88,10 @@ public class Intake extends SubsystemBase {
m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
//SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
}
public void pidOut() {
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
}
public void limitNote() {
if (intakeforwardLimit.isPressed()) {
@@ -99,10 +101,6 @@ public class Intake extends SubsystemBase {
}
}
public void pidOut() {
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
}
public void rotateArmOut2() {
if(reverseLimit.isPressed()){
stopArmMotor();
@@ -120,7 +118,15 @@ public class Intake extends SubsystemBase {
}
public void handoff() {
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED);
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
public void handoff2() {
if(intakeforwardLimit.isPressed()) {
intakeMotor.set(-val);
} else {
intakeMotor.set(-val);
}
}
public void stopIntakeMotors() {
@@ -156,7 +162,11 @@ public class Intake extends SubsystemBase {
}
public void resetPostion() {
pivot.getEncoder().setPosition(0);
setPosition(0);
}
public void setPosition(int val) {
pivot.getEncoder().setPosition(val);
}
public void resetPosition1() {
@@ -169,6 +179,10 @@ public class Intake extends SubsystemBase {
return pivot.getEncoder().getPosition();
}
public double getIntakeVelocity() {
return intakeMotor.getEncoder().getVelocity();
}
public void rotateArm() {
}
@@ -181,11 +195,20 @@ public class Intake extends SubsystemBase {
}
}
public void changeIntakeNeutralState() {
if(forwardLimit.isPressed()) {
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Vel Output", getVelocity());
SmartDashboard.putNumber("Position", getPos());
resetPosition1();
changeIntakeNeutralState();
val = SmartDashboard.getNumber("Intake Speed", 0.5);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -33,7 +33,7 @@ public class LED extends SubsystemBase {
if(m_self != null)
return;
m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(130);
m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer);
m_led.start();
src/main/java/frc4388/robot/subsystems/Limelight.java
+23 -146
View File
@@ -3,163 +3,40 @@
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.subsystems;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
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 {
private PhotonCamera cam;
private PhotonPoseEstimator photonPoseEstimator;
private double[] cameraPose;
private Boolean isTag;
private boolean lightOn;
/** Creates a new Limelight. */
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false);
public boolean getIsTag() {
return isTag;
}
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public Pose2d getPose() {
//TODO - Get actual values!
public void toggleLEDs() {
lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
double x = 0;
double y = 0;
double yaw = 0;
public void setDriverMode(boolean driverMode) {
cam.setDriverMode(driverMode);
}
Rotation2d rot = Rotation2d.fromDegrees(yaw);
public void setToLimePipeline() {
cam.setPipelineIndex(1);
setLEDs(true);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
setLEDs(false);
}
public PhotonTrackedTarget getAprilPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget();
}
private List<TargetCorner> getAprilCorners() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget().getDetectedCorners();
}
public double getAprilSkew() {
List<TargetCorner> corners = getAprilCorners();
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
if (bottomSide == null) return 0;
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
return bottomLeft.y - bottomRight.y;
}
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
if (box == null) return null;
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
TargetCorner l1 = new TargetCorner(-1, -1);
TargetCorner l2 = new TargetCorner(-1, -1);
for (TargetCorner c : box) {
if (c.y > l1.y) l1 = c;
}
for (TargetCorner c : box) {
if (c.y == l1.y) continue;
if (c.y > l2.y) l2 = c;
}
bottomSide.add(l1);
bottomSide.add(l2);
return bottomSide;
}
public double getDistanceToApril() {
PhotonTrackedTarget aprilPoint = getAprilPoint();
if (aprilPoint == null) return -1;
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + aprilPoint.getPitch();
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
}
return lowest;
}
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
return new Pose2d(x, y, rot);
}
@Override
public void periodic() {}
}
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]);
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+23 -4
View File
@@ -5,6 +5,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ShooterConstants;
@@ -29,13 +30,31 @@ public class Shooter extends SubsystemBase {
rightShooter.setNeutralMode(NeutralModeValue.Coast);
}
public Shooter(TalonFX leftShooter) {
this.leftShooter = leftShooter;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
}
public void singleSpin() {
leftShooter.set(1.0);
}
public void singleSpin(double speed) {
leftShooter.set(speed);
}
public void spin() {
spin(ShooterConstants.SHOOTER_SPEED);
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(speed);
leftShooter.set(speed);
rightShooter.set(-speed);
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
}
public void stop() {
@@ -49,8 +68,8 @@ public class Shooter extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2 -2
View File
@@ -58,7 +58,7 @@ public class SwerveDrive extends SubsystemBase {
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
@@ -66,7 +66,7 @@ public class SwerveDrive extends SubsystemBase {
stopped = true;
}
SmartDashboard.putBoolean("drift correction", true);
// SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;