SHARE
TWEET

RPI Robotic Arm

a guest Jun 9th, 2017 120 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import RPi.GPIO as GPIO
  2. import pigpio
  3. GPIO.setmode(GPIO.BCM)
  4. from time import sleep
  5. import sys, tty, termios, time
  6. import time
  7. GPIO.setwarnings(False)
  8.  
  9. pi = pigpio.pi()
  10. user_gpio = 0-31.
  11. pulsewidth = 0, 500-2500
  12.  
  13. # Variables
  14.  
  15. delay = 0.02
  16. steps = 200
  17. refresh_period = 0.02
  18.  
  19. # Enable servo pins and variables
  20. spwm = 1500
  21. epwm = 1500
  22. wpwm = 1500
  23. g1pwm = 1200
  24. g2pwm = 1800
  25.  
  26. nspwm = 1500
  27. nepwm = 1500
  28. nwpwm = 1500
  29. ng1pwm = 1200
  30. ng2pwm = 1800
  31.  
  32. shoulderpin = 18
  33. elbowpin = 12
  34. wristpin = 16
  35. grip1pin = 20
  36. grip2pin = 21
  37.  
  38. GPIO.setup(shoulderpin, GPIO.OUT)
  39. GPIO.setup(elbowpin, GPIO.OUT)
  40. GPIO.setup(wristpin, GPIO.OUT)
  41. GPIO.setup(grip1pin, GPIO.OUT)
  42. GPIO.setup(grip2pin, GPIO.OUT)
  43.  
  44. # Enable pins for IN1-4 to control step sequence in stepper motor
  45.  
  46. coil_1_pin = 23
  47. coil_2_pin = 24
  48. coil_3_pin = 22
  49. coil_4_pin = 17
  50.  
  51. # Set pin states for stepper motor
  52.  
  53. GPIO.setup(coil_1_pin, GPIO.OUT)
  54. GPIO.setup(coil_2_pin, GPIO.OUT)
  55. GPIO.setup(coil_3_pin, GPIO.OUT)
  56. GPIO.setup(coil_4_pin, GPIO.OUT)
  57.  
  58. # The getch method can determine which key has been pressed
  59. # by the user on the keyboard by accessing the system files
  60. # It will then return the pressed key as a variable
  61. def getch():
  62.     fd = sys.stdin.fileno()
  63.     old_settings = termios.tcgetattr(fd)
  64.     try:
  65.         tty.setraw(sys.stdin.fileno())
  66.         ch = sys.stdin.read(1)
  67.     finally:
  68.         termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
  69.     return ch
  70.  
  71. # This section of code defines the methods used to move
  72. # the stepper turning motor and servos used for manipulating
  73. # the arm. These definitions are called later by keypress events.
  74.  
  75.  
  76. def turn_right():
  77.     GPIO.output(23, 1)        
  78.     sleep(0.02)      
  79.     GPIO.output(23, 0)
  80.     GPIO.output(24, 1)        
  81.     sleep(0.02)      
  82.     GPIO.output(24, 0)
  83.     GPIO.output(22, 1)        
  84.     sleep(0.02)      
  85.     GPIO.output(22, 0)
  86.     GPIO.output(17, 1)        
  87.     sleep(0.02)      
  88.     GPIO.output(17, 0)
  89.  
  90. def turn_left():
  91.     GPIO.output(17, 1)        
  92.     sleep(0.02)      
  93.     GPIO.output(17, 0)
  94.     GPIO.output(22, 1)        
  95.     sleep(0.02)      
  96.     GPIO.output(22, 0)
  97.     GPIO.output(24, 1)        
  98.     sleep(0.02)      
  99.     GPIO.output(24, 0)
  100.     GPIO.output(23, 1)        
  101.     sleep(0.02)      
  102.     GPIO.output(23, 0)
  103.  
  104. def shoulder_up(spwm = spwm, nspwm = nspwm):
  105.     for spwm in range(1000, 2000, 20):
  106.         pi.set_servo_pulsewidth(shoulderpin, spwm)
  107.         time.sleep(0.4)
  108.         pi.set_servo_pulsewidth(shoulderpin, 0)  
  109. def shoulder_down(spwm = spwm, nspwm = nspwm):
  110.     spwm = nspwm
  111.     pi.set_servo_pulsewidth(shoulderpin, spwm)
  112.     time.sleep(0.4)
  113.     pi.set_servo_pulsewidth(shoulderpin, 0)    # off
  114.     nspwm = spwm + 20
  115. def elbow_up():
  116.     epwm = epwm + 20
  117.     pi.set_servo_pulsewidth(elbowpin, 1500)
  118.     time.sleep(0.4)
  119.     pi.set_servo_pulsewidth(elbowpin, 0)    # off
  120.  
  121. def elbow_down():
  122.     pi.set_servo_pulsewidth(elbowpin, 2000)
  123.     time.sleep(0.4)
  124.     pi.set_servo_pulsewidth(elbowpin, 0)    # off
  125.  
  126. def wrist_clockwise():
  127.     pi.set_servo_pulsewidth(wristpin, 1000)
  128.     time.sleep(0.4)
  129.     pi.set_servo_pulsewidth(wristpin, 0)    # off
  130.  
  131. def wrist_counterclockwise():
  132.     pi.set_servo_pulsewidth(wristpin, 1200)
  133.     time.sleep(0.4)
  134.     pi.set_servo_pulsewidth(wristpin, 0)    # off
  135.  
  136. def grip_closed():
  137.     pi.set_servo_pulsewidth(grip1pin, 1450)
  138.     pi.set_servo_pulsewidth(grip2pin, 1650)
  139.     time.sleep(0.2)
  140.     pi.set_servo_pulsewidth(grip1pin, 0)    # off
  141.     pi.set_servo_pulsewidth(grip2pin, 0)    # off
  142.  
  143. def grip_open():
  144.     pi.set_servo_pulsewidth(grip1pin, 1800)
  145.     pi.set_servo_pulsewidth(grip2pin, 1200)
  146.     time.sleep(0.2)
  147.     pi.set_servo_pulsewidth(grip1pin, 0)    # off
  148.     pi.set_servo_pulsewidth(grip2pin, 0)    # off
  149.  
  150. # Infinite loop that will not end until the user presses the
  151. # exit key
  152. while True:
  153.     # Keyboard character retrieval method is called and saved
  154.     # into variable
  155.     char = getch()
  156.  
  157.     # The shoulder will move down when the "w" key is pressed
  158.     if(char == "w"):
  159.         shoulder_down()
  160.  
  161.     # The shoulder will move up when the "s" key is pressed
  162.     if(char == "s"):
  163.         shoulder_up()
  164.  
  165.     # The "a" key will turn the arm left
  166.     if(char == "a"):
  167.         turn_left()
  168.  
  169.     # The "d" key will turn the arm right
  170.     if(char == "d"):
  171.         turn_right()
  172.  
  173.     # The "r" key will move the elbow down
  174.     if(char == "r"):
  175.         elbow_down()
  176.  
  177.     # The "f" key will move the elbow up
  178.     if(char == "f"):
  179.         elbow_up()
  180.  
  181.     # The "q" key will turn the wrist counterclockwise
  182.     if(char == "q"):
  183.         wrist_counterclockwise()
  184.  
  185.     # The "e" key will turn the wrist clockwise
  186.     if(char == "e"):
  187.         wrist_clockwise()
  188.  
  189.     # The "o" key will open the gripper
  190.     if(char == "o"):
  191.         grip_open()
  192.  
  193.     # The "p" key will close the gripper
  194.     if(char == "p"):
  195.         grip_closed()
  196.        
  197.     # The "x" key will break the loop and exit the program
  198.     if(char == "x"):
  199.         print("Program Ended")
  200.         break
  201.  
  202.     # At the end of each loop
  203.  
  204.     # The keyboard character variable will be set to blank, ready
  205.     # to save the next key that is pressed
  206.     char = ""
  207.  
  208. # Program will cease all GPIO activity before terminating
  209. GPIO.cleanup()
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