Tested code on arduino, fixed bugs and now it works :) I plan to make a processing gui for this soon :)