Gripper/arm ID translation in comm is broken

[dfarrell07]

When Planner.py calls

self.scPlanner.gripperOpen(armId)

in pickUpBlock, it passes an armId of 0 or 2. The function in comm is:

  def gripperOpen(self, arm):
    gripper = grippers[arm]
    return self.gripperSetAngle(gripper, gripper_angles[gripper][0])

The relevant globals are:

left_arm = 0
right_arm = 2
grippers = { left_arm: 1, right_arm: 3 }
gripper_angles = { grippers[left_arm]: (200, 400),
                   grippers[right_arm]: (200, 400) }  # gripper: (open, close)

So, gripperOpen takes 0 or 2, then translates those to 1 or 3, then passes the translated value and 200 (arrived at by a series of convoluted translations) to self.gripperSetAngle, which is:

  def gripperSetAngle(self, arm, angle, ramp=default_servo_ramp):
    response = self.runCommand("servo {channel} {ramp} {angle}".format(channel=grippers[arm], ramp=ramp, angle=angle))
    sleep(servo_delay)  # wait here for servo to reach angle
    return response.get('result', False)

This function takes the already translated arm value and then attempts to translate it again in the same way, which obviously fails with a KeyError.

In my opinion, this process is incredibly obtuse and needlessly complicated. I could fix this myself, but it really desperately just needs to be refactored completely, which I wouldn't want to do to someone else's package.