Before we move on, let’s create a command which causes the robot to turn to face a specific target.
The basic idea is that when we create the command we will specify the Apriltag number of the target we want to face. We can then get the position of that target by calling the ApriltagLocations.findTag function. This will return an ApriltagLocation and from that and knowing the current location of the robot we can compute the angle we need to face.
We should create a function to perform this calculation. Since it seems like this could be very useful, we should add the function to the DriveSubystem so it is available in multiple commands that we might make:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
/* * Return the angle to the specified Apriltags target */ public double getTargetAngle(int target) { ApriltagLocation location = ApriltagLocations.findTag(target); if (location == null) { throw new RuntimeException(String.format("Invalid target: %d", target)); } Pose2d pose = getPose2d(); double dx = location.m_xMeters - pose.getX(); double dy = location.m_yMeters - pose.getY(); return Math.toDegrees(Math.atan2(dy, dx)); } |
Right now if the target number we specify does not exist, I am throwing an exception. This should not happen unless we have made a mistake so I throw the exception to bring it to our attention. Note, however, if this code was for an FRC competition robot we should do something other than throw an exception because having your program crash during a match is very undesirable.
Now, once we have the desired angle, we need to command the robot to turn to that angle. The basic idea is to measure the difference between the robot’s current position and the desired position and set the turn rate to be proportional to that distance.
To actually make the robot turn, we will use the drive function that we created in the DriveSubsystem. In the command’s execute we would do something like:
|
1 2 3 4 5 6 7 |
public void execute() { Logger.log("AutoAimCommand", -1, "execute()"); ... m_subsystem.drive(0, 0, turnRate, true, ((TimedRobot) RobotCoreBase.getInstance()).getPeriod()); } |
where we will calculated the turnRate (in radians/sec) in the manner described above. We could do the calculation in the execute function but it is likely to be useful elsewhere so add it to the DriveSubsystem:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
/* * Compute the rotation rate needed to perform a turn to target maneuver */ public double computeAutoAim(double targetAngleInDegrees) { Pose2d pos = getPose2d(); double da = Gyro.normalizeAngle(pos.getRotation().getDegrees() - targetAngleInDegrees); if (Math.abs(da) > k_turnDeadZone) { return -k_turnP * da; } return 0; // On target } |
Where we have created a k_turnDeadZone within which we stop turning. Outside the dead zone we compute a turn rate which is proportional to the amount of deviation of the current heading and the target heading where k_turnP is the proportional term.
Our execute function would now look like:
|
1 2 3 4 5 6 7 8 |
public void execute() { Logger.log("AutoAimCommand", -1, "execute()"); double targetAngle = m_subsystem.getTargetAngle(m_targetNo); double turnRate = m_subsystem.computeAutoAim(targetAngle); m_subsystem.drive(0, 0, turnRate, true, ((TimedRobot) RobotCoreBase.getInstance()).getPeriod()); } |
Now tie this command to a button (use whileTrue) and adjust the k_turnP like you would any normal PID P term, making it as large as possible without making the robot unstable.
Now it would be useful to have this command automatically end when it is within the dead zone for the turn. In the execute function we know that computeAutoAim will return zero when in the dead zone, so if it is zero we can set a flag so that we can return true from the isFinished. However you will probably find that the turn may overshoot a little the first time it reaches the dead zone so you should probably only stop when it has been in the dead zone for a period of time.