HEW Robot Arm Code (Analog sliders)

#!/usr/bin/python

# Imports
from ABE_ServoPi import PWM
import time
from ABE_helpers import ABEHelpers
from gpiozero import MCP3008

# Set up Analog Zero channel names
adc0 = MCP3008(channel=0)
adc1 = MCP3008(channel=1)
adc2 = MCP3008(channel=2)
adc3 = MCP3008(channel=3)
adc4 = MCP3008(channel=4)
adc5 = MCP3008(channel=5)

# Set up some bits for the servo board
i2c_helper = ABEHelpers()
bus = i2c_helper.get_smbus()
pwm = PWM(bus, 0x40)

# Set PWM frequency to 50 Hz
pwm.set_pwm_freq(50)
pwm.output_enable()

# SERVO NUMBERING GUIDE
# baserotation = 0
# basearm = 1
# elbow = 2
# headangle = 3
# headrotate = 4
# pincer = 5

# Set an initial start positon range
currentpos = [400, 450, 200, 325, 340, 300]

# Move the arm to the start position
pwm.set_pwm(0, 0, 400)
pwm.set_pwm(1, 0, 450)
pwm.set_pwm(2, 0, 200)
pwm.set_pwm(3, 0, 325)
pwm.set_pwm(4, 0, 340)
pwm.set_pwm(5, 0, 300)

# Module for arm movement (Thanks to Stuart Weenig for the help)
def MoveArm(servo, stoppos, speed): # servo is the number of servo to move, stoppos is the position to move to, and speed is the speed with which to move the arm
   global currentpos # grab the current position variable in a RW capability so that the final position can be stored back
   startpos = currentpos[servo] # get the current position of the selected servo

   if stoppos - startpos == 0: # if the stop and start position are both 0, do nothing
      pass # do nothing

   else: # otherwise...
      direction = (stoppos - startpos)/abs(stoppos - startpos) # determine the direction of movement based on current position and requested position; will be -1 for going down and 1 for going up
      for i in range(startpos, stoppos, direction * speed): # loop through all the positions, moving to each position in small increments
         pwm.set_pwm(servo, 0, i) # move the arm
         currentpos[servo] = stoppos # store the final position
         time.sleep(0.01) # wait

while (True): # Run forever in a loop

    time.sleep(0.01)
    speed = 5 # set the speed

    adc_0 = round(adc0.value, 2) # round the ADC output to 2 decimals

    # Following if/elif statements detemin what happens when each slider is witin a set range
    # Ineficient way of doing things but it works!
    # Each block has a header with (a) and (b) - this is the PWM perating range of each servo

    ###############################################################
    # BASE ROTATION # a = 275 b = 525 (right - left)
    ###############################################################

    ### CENTRAL ###
    if 0.48 <= adc_0 <= 0.50:
        MoveArm(0, 435, speed)

    ### UPWARDS ###
    elif 0.51 <= adc_0 <= 0.59:
        MoveArm(0, 425, speed)

    elif 0.60 <= adc_0 <= 0.69:
        MoveArm(0, 400, speed)

    elif 0.70 <= adc_0 <= 0.79:
        MoveArm(0, 375, speed)

    elif 0.80 <= adc_0 <= 0.89:
        MoveArm(0, 350, speed)

    elif 0.90 <= adc_0 <= 1.00:
        MoveArm(0, 325, speed)

    ### DOWNWARDS ###

    elif 0.40 <= adc_0 <= 0.47:
        MoveArm(0, 450, speed)

    elif 0.30 <= adc_0 <= 0.39:
        MoveArm(0, 475, speed)

    elif 0.20 <= adc_0 <= 0.29:
        MoveArm(0, 500, speed)

    elif 0.10<= adc_0 <= 0.19:
        MoveArm(0, 525, speed)

    elif 0.00 <= adc_0 <= 0.09:
        MoveArm(0, 550, speed)

    ###############################################################
    # BASE ARM     a = 325 b = 550 (away - back)
    ###############################################################

    ### CENTRAL ###
    if 0.485000000000 <= adc1.value <= 0.500000000000:
        MoveArm(1, 435, speed)

    ### UPWARDS ###
    elif 0.500000000001 <= adc1.value <= 0.599999999999:
        MoveArm(1, 425, speed)

    elif 0.600000000000 <= adc1.value <= 0.699999999999:
        MoveArm(1, 400, speed)

    elif 0.700000000000 <= adc1.value <= 0.799999999999:
        MoveArm(1, 375, speed)

    elif 0.800000000000 <= adc1.value <= 0.899999999999:
        MoveArm(1, 350, speed)

    elif 0.900000000000 <= adc1.value <= 1.000000000000:
        MoveArm(1, 325, speed)

    ### DOWNWARDS ###

    elif 0.400000000000 <= adc1.value <= 0.484999999999:
        MoveArm(1, 450, speed)

    elif 0.300000000000 <= adc1.value <= 0.399999999999:
        MoveArm(1, 475, speed)

    elif 0.200000000000 <= adc1.value <= 0.299999999999:
        MoveArm(1, 500, speed)

    elif 0.100000000000 <= adc1.value <= 0.199999999999:
        MoveArm(1, 525, speed)

    elif 0.000000000000 <= adc1.value <= 0.099999999999:
        MoveArm(1, 550, speed)

    ###############################################################
    # ELBOW     a = 200 b = 525 (up - down)
    ###############################################################

    ### CENTRAL ###
    if 0.485000000000 <= adc2.value <= 0.500000000000:
        MoveArm(2, 362, speed)

    ### UPWARDS ###
    elif 0.500000000001 <= adc2.value <= 0.599999999999:
        MoveArm(2, 330, speed)

    elif 0.600000000000 <= adc2.value <= 0.699999999999:
        MoveArm(2, 300, speed)

    elif 0.700000000000 <= adc2.value <= 0.799999999999:
        MoveArm(2, 270, speed)

    elif 0.800000000000 <= adc2.value <= 0.899999999999:
        MoveArm(2, 230, speed)

    elif 0.900000000000 <= adc2.value <= 1.000000000000:
        MoveArm(2, 200, speed)

    ### DOWNWARDS ###

    elif 0.400000000000 <= adc2.value <= 0.484999999999:
        MoveArm(2, 390, speed)

    elif 0.300000000000 <= adc2.value <= 0.399999999999:
        MoveArm(2, 420, speed)

    elif 0.200000000000 <= adc2.value <= 0.299999999999:
        MoveArm(2, 450, speed)

    elif 0.100000000000 <= adc2.value <= 0.199999999999:
        MoveArm(2, 490, speed)

    elif 0.000000000000 <= adc2.value <= 0.099999999999:
        MoveArm(2, 525, speed)

    ###############################################################
    # HEAD ANGLE    a = 200 b = 450 (lower - higher)
    ###############################################################

    ### CENTRAL ###
    if 0.485000000000 <= adc3.value <= 0.500000000000:
        MoveArm(3, 325, speed)

    ### UPWARDS ###
    elif 0.500000000001 <= adc3.value <= 0.599999999999:
        MoveArm(3, 350, speed)

    elif 0.600000000000 <= adc3.value <= 0.699999999999:
        MoveArm(3, 375, speed)

    elif 0.700000000000 <= adc3.value <= 0.799999999999:
        MoveArm(3, 400, speed)

    elif 0.800000000000 <= adc3.value <= 0.899999999999:
        MoveArm(3, 425, speed)

    elif 0.900000000000 <= adc3.value <= 1.000000000000:
        MoveArm(3, 450, speed)

    ### DOWNWARDS ###

    elif 0.400000000000 <= adc3.value <= 0.484999999999:
        MoveArm(3, 300, speed)

    elif 0.300000000000 <= adc3.value <= 0.399999999999:
        MoveArm(3, 275, speed)

    elif 0.200000000000 <= adc3.value <= 0.299999999999:
        MoveArm(3, 250, speed)

    elif 0.100000000000 <= adc3.value <= 0.199999999999:
        MoveArm(3, 225, speed)

    elif 0.000000000000 <= adc3.value <= 0.099999999999:
        MoveArm(3, 200, speed)

    ###############################################################
    # HEAD ROTATE    a = 175 b = 475 (anticlockwise - clockwise)
    ###############################################################

    ### CENTRAL ###
    if 0.485000000000 <= adc4.value <= 0.500000000000:
        MoveArm(4, 325, speed)

    ### UPWARDS ###
    elif 0.500000000001 <= adc4.value <= 0.599999999999:
        MoveArm(4, 355, speed)

    elif 0.600000000000 <= adc4.value <= 0.699999999999:
        MoveArm(4, 385, speed)

    elif 0.700000000000 <= adc4.value <= 0.799999999999:
        MoveArm(4, 415, speed)

    elif 0.800000000000 <= adc4.value <= 0.899999999999:
        MoveArm(4, 445, speed)

    elif 0.900000000000 <= adc4.value <= 1.000000000000:
        MoveArm(4, 475, speed)

    ### DOWNWARDS ###

    elif 0.400000000000 <= adc4.value <= 0.484999999999:
        MoveArm(4, 295, speed)

    elif 0.300000000000 <= adc4.value <= 0.399999999999:
        MoveArm(4, 265, speed)

    elif 0.200000000000 <= adc4.value <= 0.299999999999:
        MoveArm(4, 235, speed)

    elif 0.100000000000 <= adc4.value <= 0.199999999999:
        MoveArm(4, 205, speed)

    elif 0.000000000000 <= adc4.value <= 0.099999999999:
        MoveArm(4, 175, speed)

    ###############################################################
    # PINCER    a = 300 b = 475 (open - nearly close)
    ###############################################################

    ### CENTRAL ###
    if 0.485000000000 <= adc5.value <= 0.500000000000:
        MoveArm(5, 385, speed)

    ### UPWARDS ###
    elif 0.500000000001 <= adc5.value <= 0.599999999999:
        MoveArm(5, 403, speed)

    elif 0.600000000000 <= adc5.value <= 0.699999999999:
        MoveArm(5, 430, speed)

    elif 0.700000000000 <= adc5.value <= 0.799999999999:
        MoveArm(5, 465, speed)

    elif 0.800000000000 <= adc5.value <= 0.899999999999:
        MoveArm(5, 490, speed)

    elif 0.900000000000 <= adc5.value <= 1.000000000000:
        MoveArm(5, 518, speed)

    ### DOWNWARDS ###

    elif 0.400000000000 <= adc5.value <= 0.484999999999:
        MoveArm(5, 367, speed)

    elif 0.300000000000 <= adc5.value <= 0.399999999999:
        MoveArm(5, 349, speed)

    elif 0.200000000000 <= adc5.value <= 0.299999999999:
        MoveArm(5, 331, speed)

    elif 0.100000000000 <= adc5.value <= 0.199999999999:
        MoveArm(5, 313, speed)

    elif 0.000000000000 <= adc5.value <= 0.099999999999:
        MoveArm(5, 300, speed)