Fixes servo steering

Servo steering fixed, variable names optimized, PCA ISR optimized.