What programming language do you use? Are there SDKs for interfacing with the sensors and motors?
I could use the routines that the MCU manufacturer provides but when I've tried, I've found that I spend a large amount of time understanding the API and what they are doing and writing the drivers myself is faster.
Take a look at this code base: https://github.com/ukmars/ukmarsbot