changed main to cube any groscope reading so that changes between 0 and 1 radian/second become small and anything larger has a big impact. Also fixed verious logical flaws since last commit.