there is no sensor to correct heading

[1-rafael-1]

even with encoders we will still not truly know if we move straight or turn as we wanted, because tracks may slip, the entire robot may get re-oriented by an obstacle, ... that sort of thing.

So next up: add an MPU-6050 sensor IC to obtain data. Then hook that data into the correction of motor speed.
Probably also add calibration at startup, as far as i read the IC will want that to work properly later. So:

Spawn a task that signals "I am calibrating", calibrates and then signals "I am done calibrating" and enters its acquisition loop.
Make the drive task aware that it should await end of calibration before entering its execution loop.