Create controller.cpp

Carrot Controller code for lab 2. Clean the code a little bit to make it more readable and ease troubleshooting.