Fixed PID bug invovling index, added zero position command.