Working control system

Smart Bang Bang control system works well, though there's lots of gyro
drift