Fix main program

fix bug in fwd and bwd that didn’t stop the motors.
also fix claw cube down.
and added debug messages. Good luck guys.