diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7c2b0cc..48e6982 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -12,6 +12,7 @@ import java.io.File; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; @@ -121,7 +122,7 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0))), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 39164ab..d7c0687 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -9,7 +9,6 @@ import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; -import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.structs.Gains; public class AutoAlign extends Command { @@ -20,16 +19,17 @@ public class AutoAlign extends Command { SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { this.swerveDrive = swerveDrive; this.vision = vision; + this.targetpos = targetpos; addRequirements(swerveDrive); } @Override public void initialize() { rotPID.initialize(); - this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); + //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); }