Reorganize, add setPoint functions