Add variables and functions. Define functions
Add Timer to PIControl, so test runs oproperly
Add Interrupt to main.c