DEMO #2 FBL100

Corrected bug in flybarless code. Issue related to wrong input before
calculating the I and D terms. Should be using error, and not something
else. Runs smooth.