Crane/Robotic arm code

# Made by Dylan Hewitt(MazeHorizonTech)
# Credit to multiple sources for parts of code. Not all code here is written
# by me.
# Majority was referenced online, and some copied to keep formatting.
# This is code for a robotic arm which has a shoulder servo, elbow servo, wrist
# rotation servo, and 2 servos functioning as the fingers on a 2 finger gripper.
# This is mounted on a stepper motor for rotation, which is commanded by a L298N H-Bridge.
# Tested with a raspberry Pi 3.
# Build 2.7 written June 13, 2017

import RPi.GPIO as GPIO
import pigpio
GPIO.setmode(GPIO.BCM)
from time import sleep
import sys, tty, termios, time
import time

GPIO.setwarnings(False)

#PiGPIO variables
pi = pigpio.pi()
user_gpio = 0-31.
pulsewidth = 0, 500-2500

# Variables

delay = 0.02
steps = 200
refresh_period = 0.02

# Enable servo pins and variables
global spwm
global epwm
global wpwm
global g1pwm
global g2pwm

spwm = 1950
epwm = 1900
wpwm = 1500
g1pwm = 1200
g2pwm = 1800

global nspwm
global nepwm
global nwpwm
global ng1pwm
global ng2pwm

nspwm = 1950
nepwm = 1900
nwpwm = 1500
ng1pwm = 1200
ng2pwm = 1800
#____________________________

shoulderpin = 18
elbowpin = 12
wristpin = 16
grip1pin = 20
grip2pin = 21

GPIO.setup(shoulderpin, GPIO.OUT)
GPIO.setup(elbowpin, GPIO.OUT)
GPIO.setup(wristpin, GPIO.OUT)
GPIO.setup(grip1pin, GPIO.OUT)
GPIO.setup(grip2pin, GPIO.OUT)

# Enable pins for IN1-4 to control step sequence in stepper motor

coil_1_pin = 23
coil_2_pin = 24
coil_3_pin = 22
coil_4_pin = 17

# Set pin states for stepper motor

GPIO.setup(coil_1_pin, GPIO.OUT)
GPIO.setup(coil_2_pin, GPIO.OUT)
GPIO.setup(coil_3_pin, GPIO.OUT)
GPIO.setup(coil_4_pin, GPIO.OUT)


# The getch method can determine which key has been pressed
# by the user on the keyboard by accessing the system files
# It will then return the pressed key as a variable
def getch():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch

# This section of code defines the methods used to move
# the stepper turning motor and servos used for manipulating
# the arm. These definitions are called later by keypress events.


def turn_right():       #turns stepper motor right
    GPIO.output(23, 1)
    sleep(0.02)
    GPIO.output(23, 0)
    GPIO.output(24, 1)
    sleep(0.02)
    GPIO.output(24, 0)
    GPIO.output(22, 1)
    sleep(0.02)
    GPIO.output(22, 0)
    GPIO.output(17, 1)
    sleep(0.02)
    GPIO.output(17, 0)

def turn_left():        #turns stepper motor left
    GPIO.output(17, 1)
    sleep(0.02)
    GPIO.output(17, 0)
    GPIO.output(22, 1)
    sleep(0.02)
    GPIO.output(22, 0)
    GPIO.output(24, 1)
    sleep(0.02)
    GPIO.output(24, 0)
    GPIO.output(23, 1)
    sleep(0.02)
    GPIO.output(23, 0)


def shoulder_up():      #moves shoulder servo up
    global spwm
    global nspwm
    spwm = nspwm
    pi.set_servo_pulsewidth(shoulderpin, spwm)
    nspwm = spwm - 20

def shoulder_down():    #Moves shoulder Servo down
    global spwm
    global nspwm
    spwm = nspwm
    pi.set_servo_pulsewidth(shoulderpin, spwm)
    nspwm = spwm   + 20

def elbow_up():         #Moves elbow servo Up
    global epwm
    global nepwm
    epwm = nepwm
    pi.set_servo_pulsewidth(elbowpin, epwm)
    nepwm = epwm - 20

def elbow_down():       #Moves elbow servo down
    global epwm
    global nepwm
    epwm = nepwm
    pi.set_servo_pulsewidth(elbowpin, epwm)
    nepwm = epwm + 20


def wrist_clockwise():  #Turns wrist servo clockwise
    global wpwm
    global nwpwm
    wpwm = nwpwm
    pi.set_servo_pulsewidth(wristpin, wpwm)
    nwpwm = wpwm - 20

def wrist_counterclockwise():   #Turns wrist servo Counter-Clockwise
    global wpwm
    global nwpwm
    wpwm = nwpwm
    pi.set_servo_pulsewidth(wristpin, wpwm)
    nwpwm = wpwm + 20

def grip_closed():          #Closes gripper
    global g1pwm
    global g2pwm
    global ng1pwm
    global ng2pwm
    g1pwm = ng1pwm
    g2pwm = ng2pwm
    pi.set_servo_pulsewidth(grip1pin, g1pwm)
    pi.set_servo_pulsewidth(grip2pin, g2pwm)
    ng1pwm = g1pwm + 20
    ng2pwm = ng2pwm - 20

def grip_open():            #Opens Gripper
    global g1pwm
    global g2pwm
    global ng1pwm
    global ng2pwm
    g1pwm = ng1pwm
    g2pwm = ng2pwm
    pi.set_servo_pulsewidth(grip1pin, g1pwm)
    pi.set_servo_pulsewidth(grip2pin, g2pwm)
    ng1pwm = g1pwm - 20
    ng2pwm = ng2pwm + 20

# Infinite loop that will not end until the user presses the
# exit key
while True:
    # Keyboard character retrieval method is called and saved
    # into variable
    char = getch()

    # The shoulder will move down when the "w" key is pressed
    if(char == "w"):
        shoulder_down()

    # The shoulder will move up when the "s" key is pressed
    if(char == "s"):
        shoulder_up()

    # The "a" key will turn the arm left
    if(char == "a"):
        turn_left()

    # The "d" key will turn the arm right
    if(char == "d"):
        turn_right()

    # The "r" key will move the elbow down
    if(char == "r"):
        elbow_down()

    # The "f" key will move the elbow up
    if(char == "f"):
        elbow_up()

    # The "q" key will turn the wrist counterclockwise
    if(char == "q"):
        wrist_counterclockwise()

    # The "e" key will turn the wrist clockwise
    if(char == "e"):
        wrist_clockwise()

    # The "o" key will open the gripper
    if(char == "o"):
        grip_open()

    # The "p" key will close the gripper
    if(char == "p"):
        grip_closed()

    # The "x" key will break the loop and exit the program
    if(char == "x"):
        print("Program Ended")
        break

    # At the end of each loop

    # The keyboard character variable will be set to blank, ready
    # to save the next key that is pressed
    char = ""

# Program will cease all GPIO activity before terminating
GPIO.cleanup()