[SOFTWARE] Add init motor 2 and modify init motor1
[SOFTWARE] Add L298 driver source