Added step counting for pedometer mode

Seems to work good, hardest part done. Now to design and implement UI.