When we want to shoot, we need to set a point for the shot and set the action to TRAVEL (in auto, the wheels should spin in the TRAVEL state). Once this is done, we need to create another point with an identical Pose2d, but the action should be set to PODIUM.
Develop a logic that interrupts the array of points during shooting and resumes it only when readyToShoot() is true (probably needs to have delay).
if(error.getNorm() < Constants.AutonomousConstants.distanceToletrance &&
error.getAngle().getRadians() < Constants.AutonomousConstants.angleToletrance &&
(!points[currentPointIndex].getAction().equals(RobotState.PODIUM) || readyToShoot())){
//continue point index
}
the above means if we are in place, and we are ready to shoot or the robot's state isn't podium shooting, continue to the next point.
When we want to shoot, we need to set a point for the shot and set the action to TRAVEL (in auto, the wheels should spin in the TRAVEL state). Once this is done, we need to create another point with an identical Pose2d, but the action should be set to PODIUM.
Develop a logic that interrupts the array of points during shooting and resumes it only when readyToShoot() is true (probably needs to have delay).
the above means if we are in place, and we are ready to shoot or the robot's state isn't podium shooting, continue to the next point.