Video of new code. Here is a short video of the robot now. The 4 commands I set are:
go 5 5
go -10 3
go 0 0
go 0 1