Updated Code...

Reconfigured arm reversals, the motors are on the inside now