Updated to working code.
 1. Changed PWM pins because all dont work.
 2. Fixed minor issues.