Merging branch C into master

Switching from python to c because of the limited hardware of the pi