Almost finished code

Large amounts of the problems have been fixed, including the communication between the hardware module and java program. Added small delays within the code, to prevent code skipping. Along with fixing the calibration phase.