PSoC: Various fixes, VelCtrl tuned-ish

Working version
PSoC: VelCtrl tuning (again), resets on watchdog, all ASCII functions implemented