Added PID Control for drivetrain