Merge branch 'main' of https://github.com/redshiftrobotics/crescendo-…

…2024
redshiftrobotics · Mar 2, 2024 · 25de589 · 25de589
2 parents 04bdb5d + 2940fc5
commit 25de589
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -143,7 +143,7 @@ public static class IntakeShooterConstants {
 		}
 
 		public static final boolean HAS_INTAKE;
-		
+
 		public static final boolean INTAKE_REVERSE = false;
 		public static final boolean FLYWHEEL_REVERSE = false;
 
@@ -309,8 +309,12 @@ public static class AutoConstants {
 
 	public static class VisionConstants {
 
-		public static final Transform3d CAMERA_POSE = new Transform3d(0, 0, 0, new Rotation3d());
-		public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
+		public static final Transform3d CAMERA_POSE = new Transform3d(Units.inchesToMeters(13), 0,
+				Units.inchesToMeters(11), new Rotation3d(0, Units.degreesToRadians(-20), 0));
+		public static final Transform3d ROBOT_TO_FRONT = new Transform3d(Units.inchesToMeters(-13), 0, 0,
+				new Rotation3d());
+		public static final String CAMERA_NAME = "Arducam_OV2311_USB_Camera";
+		// public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
 	}
 
 	public static class LightConstants {

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
 
@@ -75,6 +76,8 @@ public void execute() {
 		Transform3d transform = vision.getTransformToTag(tagID);
 
 		if (transform != null){
+			transform = transform.plus(VisionConstants.ROBOT_TO_FRONT);
+
 			double forward = transform.getX();
 			double left = transform.getY();
 			Rotation2d rotation = new Rotation2d(forward, left);

diff --git a/src/main/java/frc/robot/commands/SpinIntakeFlywheels.java b/src/main/java/frc/robot/commands/SpinIntakeFlywheels.java
@@ -0,0 +1,35 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeShooter;
+
+public class SpinIntakeFlywheels extends Command {
+	private IntakeShooter intakeShooter;
+	private int speed;
+
+	private long startTime;
+
+	public SpinIntakeFlywheels(IntakeShooter intake, int speed) {
+		intakeShooter = intake;
+		this.speed = speed;
+	}
+
+	@Override
+	public void initialize() {
+		startTime = System.currentTimeMillis();
+		intakeShooter.setFlyWheelSpeed(speed);
+	}
+
+	// @Override
+	// public void execute() { }
+
+	@Override
+	public boolean isFinished() {
+		return (System.currentTimeMillis() + 1000) >= startTime;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		// end
+	}
+}