it works
update 12/5/13

*adapted code to new robot