PID controls

calcMotorSignals and PWMcontrol
Current testing parameters: K_P = 15, K_I = 15, K_D = 10