Final code for Lab 2 in Lab_2_T_Control