Final working code (both FPGA and Keil) v1.0