Changed the default gains. Now only discretizes the w value. Results without actually running the robot were very good.