diff --git a/Robot2024/lib2813 b/Robot2024/lib2813 index 57da102..441dedb 160000 --- a/Robot2024/lib2813 +++ b/Robot2024/lib2813 @@ -1 +1 @@ -Subproject commit 57da10272589a15a2ab8d8f53245d85f720a800d +Subproject commit 441dedb31321a05491d10a011d8f0e45852aaab7 diff --git a/Robot2024/src/main/java/com/team2813/commands/AutoAimCommand.java b/Robot2024/src/main/java/com/team2813/commands/AutoAimCommand.java index 65dcdb5..867c0f0 100644 --- a/Robot2024/src/main/java/com/team2813/commands/AutoAimCommand.java +++ b/Robot2024/src/main/java/com/team2813/commands/AutoAimCommand.java @@ -6,17 +6,22 @@ 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; @@ -24,6 +29,7 @@ public class AutoAimCommand extends Command { 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(); diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java index e5880db..45f92c1 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java +++ b/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) { diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Intake.java b/Robot2024/src/main/java/com/team2813/subsystems/Intake.java index 3f3a590..91d6f29 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Intake.java +++ b/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); diff --git a/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java b/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java index 68b939a..d7bad43 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/IntakePivot.java +++ b/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 { - 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); diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java b/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java index 4251227..4988696 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java +++ b/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); } } \ No newline at end of file diff --git a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java index f399fd8..c708c6c 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java @@ -15,11 +15,10 @@ import com.team2813.lib2813.util.ConfigUtils; public class ShooterPivot extends MotorSubsystem { - 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),