Fixed bugs in WIPCoordSystem.c

I fixed bugs, added functions, so it can track locations if implemented.
Needs to be further tested.