Python code to control XR-2 Robotic Arm over a serial port using keyboard

XR2.control.py

#!/usr/bin/python
#Todo: smoother control, need to find optimal rate of movement

import serial
import math
import time
import curses

#User constants
ROBOT_STEP_AMOUNT = 25;
POLL_RATE = 0.075;
COM_PORT = '/dev/ttyUSB0'
CARRIAGE_RETURN = '\r\n';
GRIP_STATE = False;

stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)

stdscr.addstr(0, 0, 'Robot arm control program')

stdscr.addstr(1, 0, 'Connecting to robot arm on port ' + COM_PORT + '...')
ser = serial.Serial(COM_PORT, 9600, serial.SEVENBITS, serial.PARITY_EVEN, serial.STOPBITS_TWO)
stdscr.addstr(2, 0, 'Connected.')

ROBOT_BASE = 'b'

ROBOT_SHOULDER = 'e';
# Link 2 - G
ROBOT_ELBOW = 'g';
# Link 3 - C
ROBOT_FOREARM = 'c';
# Wrist Rotate - B
ROBOT_WRIST = 'b';
# Gripper - F
ROBOT_GRIPPER = 'f';

stdscr = curses.initscr()
curses.cbreak()
stdscr.keypad(1)

stdscr.addstr(3, 0, 'Hit "q" to quit')
stdscr.refresh()

key = ''
while key != ord('q'):

    key = stdscr.getch()
    stdscr.addch(7, 0, key)
    stdscr.refresh()

    if key == curses.KEY_UP: 
        ser.write(ROBOT_SHOULDER + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == curses.KEY_DOWN: 
        ser.write(ROBOT_SHOULDER + '-' + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == curses.KEY_LEFT:
    	ser.write(ROBOT_BASE + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == curses.KEY_RIGHT:
    	ser.write(ROBOT_BASE + '-' + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == ord('w'):
    	ser.write(ROBOT_FOREARM + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == ord('s'):
    	ser.write(ROBOT_FOREARM + '-' + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == ord('a'):
    	ser.write(ROBOT_ELBOW + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == ord('d'):
    	ser.write(ROBOT_ELBOW + '-' + str(ROBOT_STEP_AMOUNT) + CARRIAGE_RETURN)

    elif key == ord(' '):
    	if GRIP_STATE:
    		ser.write(ROBOT_GRIPPER + str(ROBOT_STEP_AMOUNT * 5) + CARRIAGE_RETURN)
    		time.sleep(3)
    		GRIP_STATE = False
    	else:
    		ser.write(ROBOT_GRIPPER + '-' + str(ROBOT_STEP_AMOUNT * 5) + CARRIAGE_RETURN)
    		time.sleep(3)
    		#ser.write(ROBOT_GRIPPER + '-' + str(ROBOT_STEP_AMOUNT * 10) + CARRIAGE_RETURN)
    		GRIP_STATE = True

    time.sleep(POLL_RATE)
    curses.flushinp()

curses.endwin()