Re-uploaded code from robotics work (hopefully this is better than subversion)

Signed-off-by: Vhaos <kareemdaggash@gmail.com>