End of feb 13 work

Still debugging.  Got it to kind of start converging, so there may be
hope for this routine.  Need to check all my angle definitions and sign
conventions.  Also figure out how I'm going to alter the controls.