From 480e4f5e6280626bd2815b8e5717b00e7e8d8ae2 Mon Sep 17 00:00:00 2001 From: stwiggy <144397102+stwiggy@users.noreply.github.com> Date: Tue, 23 Apr 2024 10:49:19 -0700 Subject: [PATCH 1/3] deleted unwanted files --- .../commands/AimAtSpeaker.java | 43 -------------- .../carlmontrobotics/commands/MoveToNote.java | 59 ------------------- 2 files changed, 102 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/AimAtSpeaker.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/MoveToNote.java 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/MoveToNote.java b/src/main/java/org/carlmontrobotics/commands/MoveToNote.java deleted file mode 100644 index 81a4dc7a..00000000 --- a/src/main/java/org/carlmontrobotics/commands/MoveToNote.java +++ /dev/null @@ -1,59 +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.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; - } -} From 091f2ac153da0d08a068d36df85890395618c72a Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 23 Apr 2024 12:23:35 -0700 Subject: [PATCH 2/3] Restore AimArmSpeaker.java removed in earlier PR. --- .../commands/AimArmSpeaker.java | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java 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..ba2fe607 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java @@ -0,0 +1,46 @@ +// 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; + } +} From b80adcd21f2073f24d445df0b9f11e178d347634 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 23 Apr 2024 12:27:40 -0700 Subject: [PATCH 3/3] Restore MoveToNote.java removed by previous commit. --- .../carlmontrobotics/commands/MoveToNote.java | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/MoveToNote.java 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; + } +}