Autopilot compass help

[ScaleRobotics]

This is part of the code that does that for the RCAP autopilot. I think it will help you. If you still have problems, let me know.

'b2dest is bearing to destination ie direction to assigned waypoint
'cmg course made good, in this case from a gps, current course direction of vehicle
'offcourse is the difference between b2dest and cmg
'dir_steer : which direction to steer the servo

        IF cmg > b2dest Then    'IF cmg > b2dest Then
            dir_steer = 1
            offcourse = cmg - b2dest        'offcourse = cmg - b2dest
        Else
            dir_steer = 0
            offcourse = b2dest - cmg           'offcourse = b2dest - cmg
        EndIF
                                 
        offcourse = offcourse / 10 ' Get rid of the fraction

        'If more than 180 degrees offcourse change direction 
        'to take the shortest turn to our course
        IF offcourse > 180 Then             
             dir_steer = dir_steer ^ %1    'switch direction to steer servo               
             offcourse = 360 - offcourse
        EndIF