Changed AtPosition to be in lib

Robot2024/src/main/java/com/team2813/commands/AutoAimCommand.java

@@ -6,24 +6,30 @@
 import com.team2813.subsystems.Shooter;
 import com.team2813.subsystems.ShooterPivot;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AutoAimCommand extends Command {
-  private static final double offset = Math.toRadians(65);
+  // Math.PI - 1.330223 = angle from plate to top hard stop
+  // other angle is from top plate of shooter to the output of shooter`
+  private static final double top_rad = Math.PI - 1.330223 - 0.851438245792;
+
   private final Shooter shooter;
   private final ShooterPivot shooterPivot;
   private final Magazine mag;
   private final Drive drive;
   private final Limelight limelight;
   private boolean done;
   double magStart = 0;
+  double shooterStart = 0;
 
   // speaker for red
   private static final Pose3d redSpeakerPos = new Pose3d(7.846862, 1.455030, 2.364370, new Rotation3d());
@@ -44,7 +50,8 @@ public boolean isBlue() {
   }
 
   private void useDistance(double distance) {
-	// shooter.run(distance * 10);
+	shooterStart = Timer.getFPGATimestamp();
+	shooter.run(distance * 10);
   }
 
   private void useRotationAngle(Rotation2d rotation) {
@@ -53,9 +60,19 @@ private void useRotationAngle(Rotation2d rotation) {
   }
 
   private void useShootingAngle(double angle) {
-	// double posRad = angle - offset;
-	// shooterPivot.setSetpoint(posRad / (Math.PI * 2));
-	// shooterPivot.enable();
+	SmartDashboard.putNumber("Auto aim theta", angle);
+	double posRad = top_rad - angle;
+	SmartDashboard.putNumber("Auto Aim pivot (radians)", posRad);
+	double posRotations = posRad / (Math.PI * 2);
+	SmartDashboard.putNumber("Auto Aim pivot (rotations) ", posRotations);
+	posRotations = 
+		MathUtil.clamp(
+			posRotations,
+			ShooterPivot.Position.TOP_HARD_STOP.getPos(),
+			ShooterPivot.Position.BOTTOM_HARD_STOP.getPos()
+		);
+	shooterPivot.setSetpoint(posRotations);
+	shooterPivot.enable();
   }
 
   Rotation2d rotation;
@@ -70,21 +87,24 @@ public void initialize() {
 	speakerPos = isBlue() ? blueSpeakerPos : redSpeakerPos;
 	done = false;
 	Pose3d pose = getPose();
-	Transform3d diff = speakerPos.minus(pose);
+	Transform3d diff = pose.minus(speakerPos).plus(new Transform3d(0, 0, -0.266586, new Rotation3d()));
+	SmartDashboard.putNumber("diffX", diff.getX());
+	SmartDashboard.putNumber("diffY", diff.getY());
+	SmartDashboard.putNumber("diffZ", diff.getZ());
 	useRotationAngle(new Rotation2d(Math.atan2(diff.getY(), diff.getX())));
 	double flatDistance = Math.hypot(diff.getX(), diff.getY());
 	useDistance(Math.hypot(diff.getZ(), flatDistance));
-	useShootingAngle(Math.atan2(diff.getZ(), flatDistance));
+	useShootingAngle(Math.atan2(-diff.getZ(), flatDistance));
   }
 
   private boolean atRotation() {
-	return Math.abs(getPose().getRotation().toRotation2d()
-		.minus(rotation).getDegrees()) < 2;
+	return Math.abs(drive.getPose().getRotation()
+		.minus(rotation).getDegrees()) <= 3;
   }
 
   @Override
   public void execute() {
-	if (atRotation() && !done) {
+	if (!done && atRotation() && shooterPivot.atPosition() && Timer.getFPGATimestamp() - shooterStart >= 0.5) {
 		mag.runMagKicker();
 		done = true;
 		magStart = Timer.getFPGATimestamp();

Robot2024/src/main/java/com/team2813/subsystems/Drive.java

@@ -87,7 +87,7 @@ public Drive() {
 			.withKS(0).withKV(1.5).withKA(0);
 		// tune
 		Slot0Configs driveGains = new Slot0Configs()
-			.withKP(3).withKI(0).withKD(0)
+			.withKP(2.5).withKI(0).withKD(0)
 			.withKS(0).withKV(0).withKA(0);
 
 		SwerveDrivetrainConstants drivetrainConstants = new SwerveDrivetrainConstants()
@@ -160,8 +160,10 @@ public Drive() {
 		tab.addDouble("back right", () -> getPosition(3));
 
 		facingRequest = new SwerveRequest.FieldCentricFacingAngle()
+		.withDriveRequestType(DriveRequestType.Velocity)
 		.withSteerRequestType(SteerRequestType.MotionMagic);
-		facingRequest.HeadingController = new PhoenixPIDController(Math.toRadians(45), 0, 0);
+		facingRequest.HeadingController = new PhoenixPIDController(3.5, 0, 1.2);
+		Shuffleboard.getTab("swerve").add("rotation PID", facingRequest.HeadingController);
 	}
 
 	private void setLimits(int module) {

Robot2024/src/main/java/com/team2813/subsystems/Intake.java

@@ -10,8 +10,8 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 public class Intake extends SubsystemBase {
     private final Motor intakeMotor;
-    private static final double INTAKE_SPEED = 0.5;
-    private static final double OUTTAKE_SPEED = -0.5;
+    private static final double INTAKE_SPEED = 0.6;
+    private static final double OUTTAKE_SPEED = -0.6;
 
 	public Intake() {
 		intakeMotor = new TalonFXWrapper(INTAKE, InvertType.COUNTER_CLOCKWISE);

Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java

@@ -11,8 +11,6 @@
 import com.team2813.lib2813.control.motors.TalonFXWrapper;
 import com.team2813.lib2813.subsystems.MotorSubsystem;
 public class IntakePivot extends MotorSubsystem<IntakePivot.Rotations> {
-	private static final double error = 0.5;
-    private Rotations currentPosition;
 
     Motor intakePivotMotor; 
     Encoder intakePivotEncoder;
@@ -24,7 +22,7 @@ public IntakePivot() {
 			new CancoderWrapper(INTAKE_ENCODER)
 			)
 			.PID(0.315, 0, 0)
-			.acceptableError(error)
+			.acceptableError(0.5)
 			.startingPosition(Rotations.INTAKE_UP));
 
         intakePivotMotor = new TalonFXWrapper(INTAKE_PIVOT, InvertType.CLOCKWISE);
@@ -36,16 +34,6 @@ private static PIDMotor pivotMotor() {
 		return pivotMotor;
 	}
 
-    @Override
-    public void setSetpoint(Rotations setPoint) {
-        super.setSetpoint(setPoint);
-        currentPosition = setPoint;
-    }
-
-    public boolean positionReached() {
-        return Math.abs(currentPosition.getPos() - getMeasurement()) < 0.05;
-    }
-
     public static enum Rotations implements MotorSubsystem.Position {
 		INTAKE_DOWN(-0.782471),
         INTAKE_UP(-0.000732);

Robot2024/src/main/java/com/team2813/subsystems/Magazine.java

@@ -32,10 +32,10 @@ public void stop() {
 	//Running just Magazine Motor
 	public void runOnlyMag() {
 		kickerMotor.set(ControlMode.DUTY_CYCLE, -0.1);
-		magMotor.set(ControlMode.DUTY_CYCLE, 0.3);
+		magMotor.set(ControlMode.DUTY_CYCLE, 0.6);
 	}
 
 	public void reverseMag() {
-		magMotor.set(ControlMode.DUTY_CYCLE, -0.5);
+		magMotor.set(ControlMode.DUTY_CYCLE, -0.6);
 	}
 }

Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java

@@ -15,11 +15,10 @@
 import com.team2813.lib2813.util.ConfigUtils;
 
 public class ShooterPivot extends MotorSubsystem<ShooterPivot.Position> {
-	private static final double ERROR = 0.01;
 
 	public ShooterPivot() {
 		super(new MotorSubsystemConfiguration(
-				pivotMotor()).acceptableError(ERROR)
+				pivotMotor()).acceptableError(0.01)
 				.PID(0.2, 0, 0));
 	}
 
@@ -42,10 +41,6 @@ private static PIDMotor pivotMotor() {
 		return result;
 	}
 
-	public boolean atPosition() {
-		return Math.abs(getMeasurement() - getSetpoint()) <= ERROR;
-	}
-
 	public static enum Position implements MotorSubsystem.Position {
 		TOP_HARD_STOP(0),
 		SUBWOOFER_FRONT(0),