Changed operate-fork function

Changed the operate fork function so that It goes up a bit and then down
on the first iteration and on every other iteration, it will only go
down
Added TeleOp_FuntionLib.h

We added a function library for our TeleOp, so now the code looks
prettier.