Add STM32_Init and fix some bug