SHARE
TWEET

Crane/Robotic arm code

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