Skip to main content
. 2019 Dec 12;19(24):5480. doi: 10.3390/s19245480
Algorithm 1 Active Camera Tracking of AprilTag Center.
Input: camera yaw angle ’ϕ’
 Output: servo angle ’Γ’
  • 1:

    KP← Propotional gain depending upon z-axis value.

  • 2:

    KI Integral gain depending upon z-axis value.

  • 3:

    KD← Propotional gain depending upon z-axis value.

  • 4:

    ϵ← Initialize error with zero

  • 5:

    T← 0.05

▹ Servo stopping threshold.
  • 6:

    α← 0.008

▹ Smoothing factor.
  • 7:

    ϵ←ϕ

  • 8:

    ifϵ>Tthen

  • 9:

    Integral ← Integral + ϵ

  • 10:

    else Integral ← 0.00

  • 11:

    end if

  • 12:

    P ←ϵ×KP

  • 13:

    I ← Integral ×KI

  • 14:

    D ← (LastYawAngle − CurrentYawAngle) ×KD

  • 15:

    Drive ← P + I + D

  • 16:

    Drive ← Drive ×α

  • 17:

    if Drive > 90 then

 ▹ To keep camera facing AprilTag
  • 18:

        Drive ← 90

  • 19:

    else Drive < -90

  • 20:

        Drive ← -90

  • 21:

    end if

  • 22:

    Γ← CurrentServoAngle + Drive

  • 23:

    LastYawAngle ← CurrentYawAngle

  • 24:

    CurrentServoAngle ←Γ

  • 25:

    returnΓ