Code for AER201 Robot Winter 2014