turn

from sense_hat import SenseHat
from gpiozero import Robot
from time import sleep
sense = SenseHat()
robin = Robot(left=(6, 5), right=(11, 9))
TURN_POWER = 0.65 # power level used to drive motors when turning
sense.set_imu_config(False, False, True)  # Compass only

def rotate_to_direction(new_dir):
    curr_dir = sense.get_compass()

    # Do we turn left or right - which is closer?
    if new_dir > curr_dir:
        diff = new_dir - curr_dir
        if diff < 180:
            clockwise = True
            pass_zero = False
        else:
            clockwise = False
            pass_zero = True # the turn takes us through zero
    else:
        diff = curr_dir - new_dir
        if diff < 180:
            clockwise = False
            pass_zero = False
        else:
            clockwise = True
            pass_zero = True

    # Now we turn
    if clockwise:
        robin.right(TURN_POWER)
        if pass_zero:
            while sense.get_compass() > 10: # ideally should stop at 0.01
                continue
        while sense.get_compass() < new_dir:
            continue
    else: # anti-clockwise
        robin.left(TURN_POWER)
        if pass_zero:
            while sense.get_compass() < 350: # ideally should stop at 359.99
                continue
        while sense.get_compass() >= new_dir:
            continue
    robin.stop()

print(f"Start position {sense.get_compass()}")
print("Intended final position 18")
rotate_to_direction(18)
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())
print(sense.get_compass())