pybricks_heading_tracker

from pybricks.hubs import InventorHub
from pybricks.parameters import Color, Axis, Port, Direction
from pybricks.tools import wait, StopWatch, run_task, multitask
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase


hub = InventorHub()
time = StopWatch()
left = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right = Motor(Port.B, Direction.CLOCKWISE)
drive = DriveBase(left, right, 55, 125)
def time_s():
    return time.time()/1000



def calc_head_with_bias(angular_velocity, time_delta, bias, current_heading = 0):
    corrected_velocoty = angular_velocity - bias

    heading_change = corrected_velocoty * time_delta
    new_heading = current_heading + heading_change
    return new_heading
class track_heading:
    def __init__(self):
        self.value = 0.0

    async def start(self, bias = 0.0, start_heading=0.0):
        self.value = start_heading
        last_time = time_s()
        hub.light.on(Color.GREEN)
        while True:
            current_time = time_s()
            time_delta = current_time - last_time
            angular_velocity = hub.imu.angular_velocity(-Axis.Z)
            self.value = calc_head_with_bias(angular_velocity, time_delta, bias, self.value)
            last_time = current_time
            hub.display.number(self.value)
            await wait(10)

    def current(self):
        return self.value


head = track_heading()
async def track(bias = 0.0, start_heading = 0.0):
    hub.light.on(Color.GREEN)
    await head.start(bias, start_heading)



async def main():
    print("hi")
    track()
    wait(100)
    drive.straight(1000)
run_task(main())