Implement Shooting while driving to Pose (translation)

[danjburke12]

No description provided.

[mpulte]

@danjburke12 Here is a stub for the command.

package com.team1701.robot.commands;

import java.util.Set;
import java.util.function.Supplier;

import com.team1701.lib.util.LoggedTunableNumber;
import com.team1701.robot.Constants;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
import com.team1701.robot.subsystems.indexer.Indexer;
import com.team1701.robot.subsystems.shooter.Shooter;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Subsystem;
import org.littletonrobotics.junction.Logger;

public class ShootAndDriveToPose extends Command {
    private static final String kLoggingPrefix = "Command/RotateToSpeakerAndDriveToPose/";
    private static final TrapezoidProfile.State kZeroState = new TrapezoidProfile.State();

    private static final LoggedTunableNumber kTranslationKp =
            new LoggedTunableNumber(kLoggingPrefix + "TranslationKp", 6.0);
    private static final LoggedTunableNumber kTranslationKi =
            new LoggedTunableNumber(kLoggingPrefix + "TranslationKi", 0.0);
    private static final LoggedTunableNumber kTranslationKd =
            new LoggedTunableNumber(kLoggingPrefix + "TranslationKd", 0.0);

    private final Drive mDrive;
    private final RobotState mRobotState;
    private final Command mShootAndMoveCommand;
    private final Supplier<Pose2d> mTargetPoseSupplier;
    private final PIDController mTranslationController;
    private final TrapezoidProfile mTranslationProfile;
    private final FinishedState mFinishedState;

    private TrapezoidProfile.State mTranslationState = kZeroState;
    private boolean mAtPose = false;

    public ShootAndDriveToPose(
            Drive drive,
            Shooter shooter,
            Indexer indexer,
            RobotState robotState,
            Supplier<Pose2d> targetPoseSupplier,
            FinishedState finishedState) {
        mDrive = drive;
        mRobotState = robotState;
        mTargetPoseSupplier = targetPoseSupplier;
        mFinishedState = finishedState;

        mTranslationController = new PIDController(
                kTranslationKp.get(), kTranslationKi.get(), kTranslationKd.get(), Constants.kLoopPeriodSeconds);
        mTranslationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(
                Constants.Drive.kSlowTrapezoidalKinematicLimits.maxDriveVelocity(),
                Constants.Drive.kSlowTrapezoidalKinematicLimits.maxDriveAcceleration()));

        mShootAndMoveCommand = new ShootAndMove(drive, shooter, indexer, robotState, this::getSpeeds);
        CommandScheduler.getInstance().registerComposedCommands(mShootAndMoveCommand);
    }

    @Override
    public void initialize() {
        mShootAndMoveCommand.initialize();
        mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);

        // TODO: Initialize the translation state
        // TODO: Initialize the translation PID controller
    }

    @Override
    public void execute() {
        mShootAndMoveCommand.execute();
        Logger.recordOutput(kLoggingPrefix + "TranslationError", mTranslationController.getPositionError());
    }

    @Override
    public void end(boolean interrupted) {
        mShootAndMoveCommand.end(interrupted);
    }

    @Override
    public boolean isFinished() {
        // TODO: Return conditionally based on if FinishedState is reached
        // May be easier to move FinishedState to ShootAndMove and implement there
        return mShootAndMoveCommand.isFinished();
    }

    @Override
    public Set<Subsystem> getRequirements() {
        return mShootAndMoveCommand.getRequirements();
    }

    private Translation2d getSpeeds() {
        // TODO: Calculate the translation speeds with TrapezoidProfile
        return new Translation2d();
    }

    public static enum FinishedState {
        NONE,
        END_AFTER_SHOOTING_AND_MOVING,
        END_AFTER_SHOOTING,
        END_AFTER_MOVING
    }
}