From 6d9ceacca0842fabed237b2c0713c6e85924388d Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Mon, 1 Apr 2024 09:53:06 -0700 Subject: [PATCH 1/5] autoaim arm to shoot --- .../java/org/carlmontrobotics/Constants.java | 2 + .../commands/AimArmSpeaker.java | 47 +++++++++++++++++++ .../commands/AutoMATICALLYGetNote.java | 2 +- .../carlmontrobotics/commands/MoveToNote.java | 2 +- .../org/carlmontrobotics/subsystems/Arm.java | 17 +++++++ .../subsystems/Limelight.java | 14 ++++-- 6 files changed, 78 insertions(+), 6 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Arm.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ba923009..e12c0aa5 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -183,6 +183,8 @@ public static final class Apriltag { public static final int RED_SPEAKER_CENTER_TAG_ID = 4; public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125 + public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); + public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); } } diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java new file mode 100644 index 00000000..f38b4789 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Arm; +import org.carlmontrobotics.subsystems.Limelight; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class AimArmSpeaker extends Command { + private final Arm arm; + private final Limelight ll; + + /** Creates a new AimOuttakeSpeaker. */ + public AimArmSpeaker(Arm arm, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.arm = arm); + addRequirements(this.ll = ll); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double goal = ll.getArmAngleToShootSpeakerRad(); + //arm.setArmTarget(goal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { +} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + //return arm.armAtSetpoint(); + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index d3361099..b843d542 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -47,7 +47,7 @@ public void initialize() { @Override public void execute() { double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); - double forwardDistErrMeters = ll.getDistanceToNote(); + double forwardDistErrMeters = ll.getDistanceToNoteMeters(); double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad); // dt.drive(0,0,0); dt.drive(Math.max(forwardDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(strafeDistErrMeters*2, MIN_MOVEMENT_METERSPSEC), Math.max(angleErrRad*2,MIN_MOVEMENT_RADSPSEC)); diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java index 67a6abb3..eb7236b8 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java +++ b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java @@ -40,7 +40,7 @@ public void initialize() { @Override public void execute() { double radErr = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); - double distErr = ll.getDistanceToNote(); //meters + double distErr = ll.getDistanceToNoteMeters(); //meters double forwardErr = distErr * Math.cos(radErr); // dt.drive(0,0,0); dt.drive(Math.max(forwardErr*2, .5), 0, 0); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java new file mode 100644 index 00000000..c6e78058 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Arm.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + /** Creates a new Arm. */ + public Arm() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 3b636e1f..2ff86e20 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -37,9 +37,9 @@ public Limelight(Drivetrain drivetrain) { public void periodic() { poseEstimator.update(Rotation2d.fromDegrees(drivetrain.getHeading()), drivetrain.getModulePositions()); updateBotPose3d(); - getDistanceToTargetSpeaker(); + getDistanceToSpeakerMeters(); getCurrentPose(); - getDistanceToNote(); + getDistanceToNoteMeters(); } public void updateBotPose3d(){ @@ -56,7 +56,7 @@ public Pose2d getCurrentPose(){ } - public double getDistanceToTargetSpeaker(){ + public double getDistanceToSpeakerMeters(){ if (LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID || LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID){ Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER).plus(Rotation2d.fromDegrees(LimelightHelpers.getTY(SHOOTER_LL_NAME))); double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) / angleToGoal.getTan(); @@ -70,7 +70,7 @@ public double getDistanceToTargetSpeaker(){ } } - public double getDistanceToNote(){ + public double getDistanceToNoteMeters(){ Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE).plus(Rotation2d.fromDegrees(LimelightHelpers.getTY(INTAKE_LL_NAME))); if (angleToGoal.getDegrees() <= 0) { double distance = (HEIGHT_FROM_GROUND_METERS_INTAKE - NOTE_HEIGHT) / Math.tan(Math.abs(angleToGoal.getRadians()));; @@ -83,6 +83,12 @@ public double getDistanceToNote(){ } } + public double getArmAngleToShootSpeakerRad(){ + double armRestingHeightToSubwoofer = HEIGHT_FROM_BOTTOM_TO_ARM_RESTING + HEIGHT_FROM_BOTTOM_TO_SUBWOOFER; + double horizontalDistanceMeters = getDistanceToSpeakerMeters(); + return Math.atan(armRestingHeightToSubwoofer/horizontalDistanceMeters); + } + // public Pose3d getTargetPose() { // double[] poseArray = LimelightHelpers.getLimelightNTDoubleArray(SHOOTER_LL_NAME, "targetpose_robotspace"); // return LimelightHelpers.toPose3D(poseArray); From 3c5dcd9148eef07d671935e81a9e76a6cad5781b Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Mon, 22 Apr 2024 13:37:37 -0700 Subject: [PATCH 2/5] other merge stuff --- .../org/carlmontrobotics/commands/AutoMATICALLYGetNote.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index 0c128e79..4890f606 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java +++ b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java @@ -5,6 +5,9 @@ package org.carlmontrobotics.commands; import static org.carlmontrobotics.Constants.Limelightc.*; + +import org.carlmontrobotics.Constants.Limelightc; + import static org.carlmontrobotics.Constants.Effectorc.*; import org.carlmontrobotics.subsystems.Drivetrain; From 5a914e339dcf456ae9b8c13a26a581509649cece Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Mon, 22 Apr 2024 19:17:36 -0700 Subject: [PATCH 3/5] aiming arm fixes --- .../java/org/carlmontrobotics/commands/AimArmSpeaker.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java index f38b4789..ba2fe607 100644 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -19,14 +19,14 @@ public class AimArmSpeaker extends Command { public AimArmSpeaker(Arm arm, Limelight ll) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(this.arm = arm); - addRequirements(this.ll = ll); + this.ll = ll; } // Called when the command is initially scheduled. @Override public void initialize() { double goal = ll.getArmAngleToShootSpeakerRad(); - //arm.setArmTarget(goal); + arm.setArmTarget(goal); } // Called every time the scheduler runs while the command is scheduled. @@ -41,7 +41,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - //return arm.armAtSetpoint(); return false; } } From fcb4fab843c8abc1ac903a900fa2cbeacb745d76 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Mon, 22 Apr 2024 19:17:45 -0700 Subject: [PATCH 4/5] moving to note fixes --- .../org/carlmontrobotics/commands/MoveToNote.java | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java index eb7236b8..81a4dc7a 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java +++ b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java @@ -17,22 +17,21 @@ public class MoveToNote extends Command { private final Drivetrain dt; private final Limelight ll; - private Timer timer = new Timer(); + private Timer timer = new Timer(); + private boolean originalFieldOrientation; /** Creates a new MoveToNote. */ public MoveToNote(Drivetrain dt, Limelight ll) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(this.dt=dt); - addRequirements(this.ll=ll); + this.ll = ll; } // Called when the command is initially scheduled. @Override public void initialize() { - new AlignToNote(dt); + originalFieldOrientation = dt.getFieldOriented(); timer.reset(); timer.start(); - //new Intake().finallyDo(()->{this.end(false);}); - SmartDashboard.putBoolean("end", false); dt.setFieldOriented(false); } @@ -42,7 +41,6 @@ public void execute() { double radErr = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); double distErr = ll.getDistanceToNoteMeters(); //meters double forwardErr = distErr * Math.cos(radErr); - // dt.drive(0,0,0); dt.drive(Math.max(forwardErr*2, .5), 0, 0); //180deg is about 6.2 rad/sec, min is .5rad/sec } @@ -50,7 +48,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - dt.setFieldOriented(true); + dt.setFieldOriented(originalFieldOrientation); } // Returns true when the command should end. From 16d602af63c2527114da653c01c2baa9910eec95 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Tue, 23 Apr 2024 10:51:04 -0700 Subject: [PATCH 5/5] deleted unwanted files --- .../commands/AimArmSpeaker.java | 46 ------------------- .../commands/AimAtSpeaker.java | 43 ----------------- 2 files changed, 89 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java deleted file mode 100644 index ba2fe607..00000000 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.subsystems.Arm; -import org.carlmontrobotics.subsystems.Limelight; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class AimArmSpeaker extends Command { - private final Arm arm; - private final Limelight ll; - - /** Creates a new AimOuttakeSpeaker. */ - public AimArmSpeaker(Arm arm, Limelight ll) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(this.arm = arm); - this.ll = ll; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double goal = ll.getArmAngleToShootSpeakerRad(); - arm.setArmTarget(goal); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { -} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java deleted file mode 100644 index 1966c2c3..00000000 --- a/src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.subsystems.Arm; -import org.carlmontrobotics.subsystems.Drivetrain; -import org.carlmontrobotics.subsystems.Limelight; - -import edu.wpi.first.wpilibj2.command.Command; - -public class AimAtSpeaker extends Command { - private Arm arm; - private Drivetrain dt; - private Limelight ll; //shooter limelight - - /** Creates a new AimAtSpeaker. */ - public AimAtSpeaker(Arm arm, Drivetrain dt, Limelight ll) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(this.arm = arm); - addRequirements(this.dt = dt); - this.ll = ll; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -}