Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/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)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement