I didn’t really test the code before I posted.
change newH to:
newH = currHpr[0] + closestDestAngle(currHpr[0], newHpr[0])
I think that should fix it, but I haven’t tested this either.
If it still acts funny, use some print statements to help figure out what is going on.