diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 752f6eb2..71bd304b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -373,7 +373,9 @@ public static final class Limelightc { 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 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 deleted file mode 100644 index 1f34b700..00000000 --- a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.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.Limelight; -import edu.wpi.first.wpilibj2.command.Command; - -public class AimArmSpeaker extends Command { - private final Arm arm; - private final Limelight ll; //Shooter 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 arm.armAtSetpoint(); - } -} 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; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java b/src/main/java/org/carlmontrobotics/commands/AutoMATICALLYGetNote.java index 6a07a900..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; @@ -47,8 +50,8 @@ public void initialize() { @Override public void execute() { - double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(INTAKE_LL_NAME)); - double forwardDistErrMeters = ll.getDistanceToNoteMeters(); + double angleErrRad = Units.degreesToRadians(LimelightHelpers.getTX(Limelightc.INTAKE_LL_NAME)); + double forwardDistErrMeters = ll.getDistanceToNoteMeters(); double strafeDistErrMeters = forwardDistErrMeters * Math.tan(angleErrRad); // dt.drive(0,0,0); dt.drive(Math.max(forwardDistErrMeters * 2, MIN_MOVEMENT_METERSPSEC), diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java new file mode 100644 index 00000000..81a4dc7a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java @@ -0,0 +1,59 @@ +// 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.Constants.Limelightc; +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; +import org.carlmontrobotics.subsystems.LimelightHelpers; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class MoveToNote extends Command { + private final Drivetrain dt; + private final Limelight ll; + 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); + this.ll = ll; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + originalFieldOrientation = dt.getFieldOriented(); + timer.reset(); + timer.start(); + dt.setFieldOriented(false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + 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(Math.max(forwardErr*2, .5), 0, 0); + //180deg is about 6.2 rad/sec, min is .5rad/sec + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + dt.setFieldOriented(originalFieldOrientation); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get() >= 0.5; + } +}