Added restart command
Changed to compute angle based on delta instead of rate
Compute average delta on startup to adjust for steady state error