PID works with out robot, but fails when robot is attached?
Fixed the setup to use compfilter to generate ref angle, and corrected some type conversion problems with spd. Set spd expects a float now