Started the Firmware

I wrote some code to test the motors and the encoders. So far, the motors spin as they should, but I’m having some difficulty with the encoders. The encoders are needed for a PID loop, which is crucial for the robot as it needs to follow a straight path.

Next steps

Debug the code for the encoders and add UART input.