Bug Fixes and Updates

Works with Ports 2-7 (PORTD) and fixes some bugs