Also, I've discovered that while manual moves are typically clamped, your initial homing offset may be out of bounds so to speak. Make sure after you've homed that you request a G0 move to a valid point.
atan2with X as the first argument and Y as the second argument, whereas the documentation for this function specifies that the order should be Y, X. Your code ends up producing the right result, but "strictly speaking" the arguments should be swapped and the code that follows adjusted for the differing output. I will be attempting that in my solution, and will post the results.
SCARA_S2 = sqrt(1 - sq(SCARA_C2));