SHARE
TWEET

HEW Robot Arm Code (Analog sliders)

AverageMan May 30th, 2016 155 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/python
  2.  
  3. # Imports
  4. from ABE_ServoPi import PWM
  5. import time
  6. from ABE_helpers import ABEHelpers
  7. from gpiozero import MCP3008
  8.  
  9. # Set up Analog Zero channel names
  10. adc0 = MCP3008(channel=0)
  11. adc1 = MCP3008(channel=1)
  12. adc2 = MCP3008(channel=2)
  13. adc3 = MCP3008(channel=3)
  14. adc4 = MCP3008(channel=4)
  15. adc5 = MCP3008(channel=5)
  16.  
  17. # Set up some bits for the servo board
  18. i2c_helper = ABEHelpers()
  19. bus = i2c_helper.get_smbus()
  20. pwm = PWM(bus, 0x40)
  21.  
  22. # Set PWM frequency to 50 Hz
  23. pwm.set_pwm_freq(50)
  24. pwm.output_enable()
  25.  
  26. # SERVO NUMBERING GUIDE
  27. # baserotation = 0
  28. # basearm = 1
  29. # elbow = 2
  30. # headangle = 3
  31. # headrotate = 4
  32. # pincer = 5
  33.  
  34. # Set an initial start positon range
  35. currentpos = [400, 450, 200, 325, 340, 300]
  36.  
  37. # Move the arm to the start position
  38. pwm.set_pwm(0, 0, 400)
  39. pwm.set_pwm(1, 0, 450)
  40. pwm.set_pwm(2, 0, 200)
  41. pwm.set_pwm(3, 0, 325)
  42. pwm.set_pwm(4, 0, 340)
  43. pwm.set_pwm(5, 0, 300)
  44.  
  45. # Module for arm movement (Thanks to Stuart Weenig for the help)
  46. 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
  47.    global currentpos # grab the current position variable in a RW capability so that the final position can be stored back
  48.    startpos = currentpos[servo] # get the current position of the selected servo
  49.    
  50.    if stoppos - startpos == 0: # if the stop and start position are both 0, do nothing
  51.       pass # do nothing
  52.      
  53.    else: # otherwise...
  54.       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
  55.       for i in range(startpos, stoppos, direction * speed): # loop through all the positions, moving to each position in small increments
  56.          pwm.set_pwm(servo, 0, i) # move the arm
  57.          currentpos[servo] = stoppos # store the final position
  58.          time.sleep(0.01) # wait
  59.  
  60. while (True): # Run forever in a loop
  61.    
  62.     time.sleep(0.01)
  63.     speed = 5 # set the speed
  64.    
  65.     adc_0 = round(adc0.value, 2) # round the ADC output to 2 decimals
  66.    
  67.     # Following if/elif statements detemin what happens when each slider is witin a set range
  68.     # Ineficient way of doing things but it works!
  69.     # Each block has a header with (a) and (b) - this is the PWM perating range of each servo
  70.    
  71.     ###############################################################
  72.     # BASE ROTATION # a = 275 b = 525 (right - left)
  73.     ###############################################################
  74.  
  75.     ### CENTRAL ###
  76.     if 0.48 <= adc_0 <= 0.50:
  77.         MoveArm(0, 435, speed)
  78.    
  79.     ### UPWARDS ###
  80.     elif 0.51 <= adc_0 <= 0.59:
  81.         MoveArm(0, 425, speed)
  82.        
  83.     elif 0.60 <= adc_0 <= 0.69:
  84.         MoveArm(0, 400, speed)
  85.        
  86.     elif 0.70 <= adc_0 <= 0.79:
  87.         MoveArm(0, 375, speed)
  88.        
  89.     elif 0.80 <= adc_0 <= 0.89:
  90.         MoveArm(0, 350, speed)
  91.        
  92.     elif 0.90 <= adc_0 <= 1.00:
  93.         MoveArm(0, 325, speed)
  94.        
  95.     ### DOWNWARDS ###
  96.    
  97.     elif 0.40 <= adc_0 <= 0.47:
  98.         MoveArm(0, 450, speed)
  99.    
  100.     elif 0.30 <= adc_0 <= 0.39:
  101.         MoveArm(0, 475, speed)
  102.        
  103.     elif 0.20 <= adc_0 <= 0.29:
  104.         MoveArm(0, 500, speed)
  105.        
  106.     elif 0.10<= adc_0 <= 0.19:
  107.         MoveArm(0, 525, speed)
  108.        
  109.     elif 0.00 <= adc_0 <= 0.09:
  110.         MoveArm(0, 550, speed)
  111.    
  112.     ###############################################################
  113.     # BASE ARM     a = 325 b = 550 (away - back)
  114.     ###############################################################
  115.  
  116.     ### CENTRAL ###
  117.     if 0.485000000000 <= adc1.value <= 0.500000000000:
  118.         MoveArm(1, 435, speed)
  119.    
  120.     ### UPWARDS ###
  121.     elif 0.500000000001 <= adc1.value <= 0.599999999999:
  122.         MoveArm(1, 425, speed)
  123.        
  124.     elif 0.600000000000 <= adc1.value <= 0.699999999999:
  125.         MoveArm(1, 400, speed)
  126.        
  127.     elif 0.700000000000 <= adc1.value <= 0.799999999999:
  128.         MoveArm(1, 375, speed)
  129.        
  130.     elif 0.800000000000 <= adc1.value <= 0.899999999999:
  131.         MoveArm(1, 350, speed)
  132.        
  133.     elif 0.900000000000 <= adc1.value <= 1.000000000000:
  134.         MoveArm(1, 325, speed)
  135.        
  136.     ### DOWNWARDS ###
  137.    
  138.     elif 0.400000000000 <= adc1.value <= 0.484999999999:
  139.         MoveArm(1, 450, speed)
  140.    
  141.     elif 0.300000000000 <= adc1.value <= 0.399999999999:
  142.         MoveArm(1, 475, speed)
  143.        
  144.     elif 0.200000000000 <= adc1.value <= 0.299999999999:
  145.         MoveArm(1, 500, speed)
  146.        
  147.     elif 0.100000000000 <= adc1.value <= 0.199999999999:
  148.         MoveArm(1, 525, speed)
  149.        
  150.     elif 0.000000000000 <= adc1.value <= 0.099999999999:
  151.         MoveArm(1, 550, speed)
  152.        
  153.     ###############################################################
  154.     # ELBOW     a = 200 b = 525 (up - down)
  155.     ###############################################################
  156.  
  157.     ### CENTRAL ###
  158.     if 0.485000000000 <= adc2.value <= 0.500000000000:
  159.         MoveArm(2, 362, speed)
  160.    
  161.     ### UPWARDS ###
  162.     elif 0.500000000001 <= adc2.value <= 0.599999999999:
  163.         MoveArm(2, 330, speed)
  164.        
  165.     elif 0.600000000000 <= adc2.value <= 0.699999999999:
  166.         MoveArm(2, 300, speed)
  167.        
  168.     elif 0.700000000000 <= adc2.value <= 0.799999999999:
  169.         MoveArm(2, 270, speed)
  170.        
  171.     elif 0.800000000000 <= adc2.value <= 0.899999999999:
  172.         MoveArm(2, 230, speed)
  173.        
  174.     elif 0.900000000000 <= adc2.value <= 1.000000000000:
  175.         MoveArm(2, 200, speed)
  176.        
  177.     ### DOWNWARDS ###
  178.    
  179.     elif 0.400000000000 <= adc2.value <= 0.484999999999:
  180.         MoveArm(2, 390, speed)
  181.    
  182.     elif 0.300000000000 <= adc2.value <= 0.399999999999:
  183.         MoveArm(2, 420, speed)
  184.        
  185.     elif 0.200000000000 <= adc2.value <= 0.299999999999:
  186.         MoveArm(2, 450, speed)
  187.        
  188.     elif 0.100000000000 <= adc2.value <= 0.199999999999:
  189.         MoveArm(2, 490, speed)
  190.        
  191.     elif 0.000000000000 <= adc2.value <= 0.099999999999:
  192.         MoveArm(2, 525, speed)
  193.        
  194.     ###############################################################
  195.     # HEAD ANGLE    a = 200 b = 450 (lower - higher)
  196.     ###############################################################
  197.  
  198.     ### CENTRAL ###
  199.     if 0.485000000000 <= adc3.value <= 0.500000000000:
  200.         MoveArm(3, 325, speed)
  201.    
  202.     ### UPWARDS ###
  203.     elif 0.500000000001 <= adc3.value <= 0.599999999999:
  204.         MoveArm(3, 350, speed)
  205.        
  206.     elif 0.600000000000 <= adc3.value <= 0.699999999999:
  207.         MoveArm(3, 375, speed)
  208.        
  209.     elif 0.700000000000 <= adc3.value <= 0.799999999999:
  210.         MoveArm(3, 400, speed)
  211.        
  212.     elif 0.800000000000 <= adc3.value <= 0.899999999999:
  213.         MoveArm(3, 425, speed)
  214.        
  215.     elif 0.900000000000 <= adc3.value <= 1.000000000000:
  216.         MoveArm(3, 450, speed)
  217.        
  218.     ### DOWNWARDS ###
  219.    
  220.     elif 0.400000000000 <= adc3.value <= 0.484999999999:
  221.         MoveArm(3, 300, speed)
  222.    
  223.     elif 0.300000000000 <= adc3.value <= 0.399999999999:
  224.         MoveArm(3, 275, speed)
  225.        
  226.     elif 0.200000000000 <= adc3.value <= 0.299999999999:
  227.         MoveArm(3, 250, speed)
  228.        
  229.     elif 0.100000000000 <= adc3.value <= 0.199999999999:
  230.         MoveArm(3, 225, speed)
  231.        
  232.     elif 0.000000000000 <= adc3.value <= 0.099999999999:
  233.         MoveArm(3, 200, speed)
  234.  
  235.     ###############################################################
  236.     # HEAD ROTATE    a = 175 b = 475 (anticlockwise - clockwise)
  237.     ###############################################################
  238.  
  239.     ### CENTRAL ###
  240.     if 0.485000000000 <= adc4.value <= 0.500000000000:
  241.         MoveArm(4, 325, speed)
  242.    
  243.     ### UPWARDS ###
  244.     elif 0.500000000001 <= adc4.value <= 0.599999999999:
  245.         MoveArm(4, 355, speed)
  246.        
  247.     elif 0.600000000000 <= adc4.value <= 0.699999999999:
  248.         MoveArm(4, 385, speed)
  249.        
  250.     elif 0.700000000000 <= adc4.value <= 0.799999999999:
  251.         MoveArm(4, 415, speed)
  252.        
  253.     elif 0.800000000000 <= adc4.value <= 0.899999999999:
  254.         MoveArm(4, 445, speed)
  255.        
  256.     elif 0.900000000000 <= adc4.value <= 1.000000000000:
  257.         MoveArm(4, 475, speed)
  258.        
  259.     ### DOWNWARDS ###
  260.    
  261.     elif 0.400000000000 <= adc4.value <= 0.484999999999:
  262.         MoveArm(4, 295, speed)
  263.    
  264.     elif 0.300000000000 <= adc4.value <= 0.399999999999:
  265.         MoveArm(4, 265, speed)
  266.        
  267.     elif 0.200000000000 <= adc4.value <= 0.299999999999:
  268.         MoveArm(4, 235, speed)
  269.        
  270.     elif 0.100000000000 <= adc4.value <= 0.199999999999:
  271.         MoveArm(4, 205, speed)
  272.        
  273.     elif 0.000000000000 <= adc4.value <= 0.099999999999:
  274.         MoveArm(4, 175, speed)
  275.        
  276.     ###############################################################
  277.     # PINCER    a = 300 b = 475 (open - nearly close)
  278.     ###############################################################
  279.  
  280.     ### CENTRAL ###
  281.     if 0.485000000000 <= adc5.value <= 0.500000000000:
  282.         MoveArm(5, 385, speed)
  283.    
  284.     ### UPWARDS ###
  285.     elif 0.500000000001 <= adc5.value <= 0.599999999999:
  286.         MoveArm(5, 403, speed)
  287.        
  288.     elif 0.600000000000 <= adc5.value <= 0.699999999999:
  289.         MoveArm(5, 430, speed)
  290.        
  291.     elif 0.700000000000 <= adc5.value <= 0.799999999999:
  292.         MoveArm(5, 465, speed)
  293.        
  294.     elif 0.800000000000 <= adc5.value <= 0.899999999999:
  295.         MoveArm(5, 490, speed)
  296.        
  297.     elif 0.900000000000 <= adc5.value <= 1.000000000000:
  298.         MoveArm(5, 518, speed)
  299.        
  300.     ### DOWNWARDS ###
  301.    
  302.     elif 0.400000000000 <= adc5.value <= 0.484999999999:
  303.         MoveArm(5, 367, speed)
  304.    
  305.     elif 0.300000000000 <= adc5.value <= 0.399999999999:
  306.         MoveArm(5, 349, speed)
  307.        
  308.     elif 0.200000000000 <= adc5.value <= 0.299999999999:
  309.         MoveArm(5, 331, speed)
  310.        
  311.     elif 0.100000000000 <= adc5.value <= 0.199999999999:
  312.         MoveArm(5, 313, speed)
  313.        
  314.     elif 0.000000000000 <= adc5.value <= 0.099999999999:
  315.         MoveArm(5, 300, speed)
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top