Sweeper test code

Coded new sweeper code (did not test with robot)