Working test bench for the left motor, simple PWM implemented, need to add FD,SD modes, forwards and reverse logic. Cleaned up the firmware class a bit (function names, order etc) added methods to control the motors.