696_AmpGetToPosition.java

// 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 team696.frc.robot.commands;

import team696.frc.lib.Camera.BaseCam.AprilTagResult;
import team696.frc.robot.Constants;
import team696.frc.robot.subsystems.Swerve;

import java.util.Optional;

import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;

/** Add your docs here. */
public class AmpGetToPosition extends Command{
    private Pose2d goal;
    PIDController x;
    PIDController y;
    public AmpGetToPosition(){
        goal=new Pose2d(1.83, 7.54, Rotation2d.fromDegrees(90));
        x=new PIDController(0.02, 0, 0);
        y=new PIDController(0.02, 0, 0);
        x.setSetpoint(goal.getX());
        y.setSetpoint(goal.getY());

    }
    @Override
    public void initialize(){

    }
    @Override
    public void execute(){
        Optional<AprilTagResult> new_estimate=Swerve.get().ampCam.getEstimate();
        if(!new_estimate.isEmpty()){
            Pose2d currentPose=new_estimate.get().pose;
            Swerve.get().Drive(new ChassisSpeeds(x.calculate(currentPose.getX()), y.calculate(currentPose.getY()), 0));

        }else{
            Swerve.get().Drive(new ChassisSpeeds(0,0,0));
        }
    }
    @Override
    public void end(boolean interruptred){

    }
}