Bug fixes. Main Problem description in commen below.

Changed energy calculation to be more modulare (recycle code, dont
write twice) and fixed some bugs about it.
Main Problem:
The velocity of the pendulum can change without it moving, due to the
position grid not havin sufficient resolution.