Made main TeleOp code compile

Made function macros

TODO: rename stupid max array functions