PD tuning

PD controller needs further tuning. Gains can be set at the top of
Actions.cpp