├── README.md ├── requirements.txt ├── robotcontrol ├── Adafruit_I2C.py ├── Adafruit_PWM_Servo_Driver.py ├── RobotServoExtents.xls ├── Sequences.csv ├── Teachpoints.csv ├── __init__.py ├── __main__.py ├── dataposter.py ├── dataposter_testharness.py └── robotcontrol.py └── setup.py /README.md: -------------------------------------------------------------------------------- 1 | # RobotControl 2 | This is a codebase that provides menu driven control of a 6axis servo driven robotic arm. It allows for basic manipulation of manually entered positions, go to teachpoint functionality, and the ability to loop the arm in a sequence of teachpoints. 3 | 4 | The arm is driven using a raspberry pi, affixed w/ the adafruit PWM/Servo shield, a Rot2U robotic arm using MG996R servos. With that said, it should work for any PWM addressable setup. Full build details are available here: 5 | 6 | # Install instructions 7 | On the raspberry pi: 8 | 9 | git clone https://github.com/mechiris/RobotControl.git 10 | sudo apt-get install python-pandas #faster than compiling from scratch 11 | cd RobotControl 12 | pip install -r requirements.txt 13 | python setup.py develop 14 | 15 | # Usage 16 | 17 | ## Run 18 | To run the program, enter the following shell command: 19 | `robotcontrol` 20 | This will pull up a menu with prompts on setting servos directly, going to predefined teachpoints, or sequences that can be setup and saved in csv files. All menu commanders are text entry followed by return. 21 | 22 | ## Menu overview 23 | ### 3. Go To Servo Position 24 | Typical usage would be to use the menu option set individual servos until you get the robot to a position of interest. Enter the servo number, followed by the position of interest. You can also nudge using the up and down keys. Once the robot is positioned, you can go to the main menu and list out the servo positions. From there you can create a teachpoint. 25 | 26 | ### 2. Go To TeachPoint 27 | You can create a teachpoint by editting /robotcontrol/Teachpoints.csv. This file is structured with the following columns: 28 | * Position,delay,s0,s1,s2,s3,s4,s5 29 | * Position: the human readable name of the teachpoint, referenced from the menu 30 | * delay is how long it takes to move (the class uses some basic smoothing so it doesn't jerk the robot right into position) 31 | * s0:s5 are the servo positions. 32 | 33 | Once you save the teachpoint in Teachpoints.csv, you can restart your robotcontrol program it and should be available from the menu. 34 | 35 | Select 2. Go To TeachPoint, and l will list the available teachpoints. You can then type in the name, and hit enter. The robot will move to the teachpoint of interest. 36 | 37 | ### 1. Run Sequence 38 | Once you have teachpoints created, you can create sequences of these teachpoints. A sequence includes one or more teachpoints in a list, which can be either run once or run continuously. 39 | 40 | The robotcontrol/Sequences.csv file has the following structure: 41 | * sequence;teachpoints;delays;loop 42 | * Sequence: The name of the sequence, referenced from the menu 43 | * teachpoints: a comma seperated list of teachpoints to be run 44 | * delays: a comma seperated list of delays associated with each teachpoint. Should be the same length as teachpoints. 45 | * loop: True if you want the sequence to repeat until user interaction. 46 | 47 | The columns use a semicolon delimiter, as some of the contents of the cells can have multiple values seperated by a comma. 48 | Once you save your robotocontrol/Sequences.csv, you can restart the robotcontrol program and reference from the menu: 49 | Select 1. Run Sequence, and l will list the available sequences. Type in the sequence name of interest. If the sequence has loop=True, it will run until you hit return again. 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | RPi.GPIO 2 | pandas 3 | numpy -------------------------------------------------------------------------------- /robotcontrol/Adafruit_I2C.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import re 3 | import smbus 4 | 5 | # =========================================================================== 6 | # Adafruit_I2C Class 7 | # =========================================================================== 8 | 9 | class Adafruit_I2C(object): 10 | 11 | @staticmethod 12 | def getPiRevision(): 13 | "Gets the version number of the Raspberry Pi board" 14 | # Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History 15 | try: 16 | with open('/proc/cpuinfo', 'r') as infile: 17 | for line in infile: 18 | # Match a line of the form "Revision : 0002" while ignoring extra 19 | # info in front of the revsion (like 1000 when the Pi was over-volted). 20 | match = re.match('Revision\s+:\s+.*(\w{4})$', line) 21 | if match and match.group(1) in ['0000', '0002', '0003']: 22 | # Return revision 1 if revision ends with 0000, 0002 or 0003. 23 | return 1 24 | elif match: 25 | # Assume revision 2 if revision ends with any other 4 chars. 26 | return 2 27 | # Couldn't find the revision, assume revision 0 like older code for compatibility. 28 | return 0 29 | except: 30 | return 0 31 | 32 | @staticmethod 33 | def getPiI2CBusNumber(): 34 | # Gets the I2C bus number /dev/i2c# 35 | return 1 if Adafruit_I2C.getPiRevision() > 1 else 0 36 | 37 | def __init__(self, address, busnum=-1, debug=False): 38 | self.address = address 39 | # By default, the correct I2C bus is auto-detected using /proc/cpuinfo 40 | # Alternatively, you can hard-code the bus version below: 41 | # self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's) 42 | # self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's) 43 | self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber()) 44 | self.debug = debug 45 | 46 | def reverseByteOrder(self, data): 47 | "Reverses the byte order of an int (16-bit) or long (32-bit) value" 48 | # Courtesy Vishal Sapre 49 | byteCount = len(hex(data)[2:].replace('L','')[::2]) 50 | val = 0 51 | for i in range(byteCount): 52 | val = (val << 8) | (data & 0xff) 53 | data >>= 8 54 | return val 55 | 56 | def errMsg(self): 57 | print "Error accessing 0x%02X: Check your I2C address" % self.address 58 | return -1 59 | 60 | def write8(self, reg, value): 61 | "Writes an 8-bit value to the specified register/address" 62 | try: 63 | self.bus.write_byte_data(self.address, reg, value) 64 | if self.debug: 65 | print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg) 66 | except IOError, err: 67 | return self.errMsg() 68 | 69 | def write16(self, reg, value): 70 | "Writes a 16-bit value to the specified register/address pair" 71 | try: 72 | self.bus.write_word_data(self.address, reg, value) 73 | if self.debug: 74 | print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" % 75 | (value, reg, reg+1)) 76 | except IOError, err: 77 | return self.errMsg() 78 | 79 | def writeRaw8(self, value): 80 | "Writes an 8-bit value on the bus" 81 | try: 82 | self.bus.write_byte(self.address, value) 83 | if self.debug: 84 | print "I2C: Wrote 0x%02X" % value 85 | except IOError, err: 86 | return self.errMsg() 87 | 88 | def writeList(self, reg, list): 89 | "Writes an array of bytes using I2C format" 90 | try: 91 | if self.debug: 92 | print "I2C: Writing list to register 0x%02X:" % reg 93 | print list 94 | self.bus.write_i2c_block_data(self.address, reg, list) 95 | except IOError, err: 96 | return self.errMsg() 97 | 98 | def readList(self, reg, length): 99 | "Read a list of bytes from the I2C device" 100 | try: 101 | results = self.bus.read_i2c_block_data(self.address, reg, length) 102 | if self.debug: 103 | print ("I2C: Device 0x%02X returned the following from reg 0x%02X" % 104 | (self.address, reg)) 105 | print results 106 | return results 107 | except IOError, err: 108 | return self.errMsg() 109 | 110 | def readU8(self, reg): 111 | "Read an unsigned byte from the I2C device" 112 | try: 113 | result = self.bus.read_byte_data(self.address, reg) 114 | if self.debug: 115 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 116 | (self.address, result & 0xFF, reg)) 117 | return result 118 | except IOError, err: 119 | return self.errMsg() 120 | 121 | def readS8(self, reg): 122 | "Reads a signed byte from the I2C device" 123 | try: 124 | result = self.bus.read_byte_data(self.address, reg) 125 | if result > 127: result -= 256 126 | if self.debug: 127 | print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" % 128 | (self.address, result & 0xFF, reg)) 129 | return result 130 | except IOError, err: 131 | return self.errMsg() 132 | 133 | def readU16(self, reg, little_endian=True): 134 | "Reads an unsigned 16-bit value from the I2C device" 135 | try: 136 | result = self.bus.read_word_data(self.address,reg) 137 | # Swap bytes if using big endian because read_word_data assumes little 138 | # endian on ARM (little endian) systems. 139 | if not little_endian: 140 | result = ((result << 8) & 0xFF00) + (result >> 8) 141 | if (self.debug): 142 | print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg) 143 | return result 144 | except IOError, err: 145 | return self.errMsg() 146 | 147 | def readS16(self, reg, little_endian=True): 148 | "Reads a signed 16-bit value from the I2C device" 149 | try: 150 | result = self.readU16(reg,little_endian) 151 | if result > 32767: result -= 65536 152 | return result 153 | except IOError, err: 154 | return self.errMsg() 155 | 156 | if __name__ == '__main__': 157 | try: 158 | bus = Adafruit_I2C(address=0) 159 | print "Default I2C bus is accessible" 160 | except: 161 | print "Error accessing default I2C bus" 162 | -------------------------------------------------------------------------------- /robotcontrol/Adafruit_PWM_Servo_Driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import time 4 | import math 5 | from Adafruit_I2C import Adafruit_I2C 6 | 7 | # ============================================================================ 8 | # Adafruit PCA9685 16-Channel PWM Servo Driver 9 | # ============================================================================ 10 | 11 | class PWM : 12 | # Registers/etc. 13 | __MODE1 = 0x00 14 | __MODE2 = 0x01 15 | __SUBADR1 = 0x02 16 | __SUBADR2 = 0x03 17 | __SUBADR3 = 0x04 18 | __PRESCALE = 0xFE 19 | __LED0_ON_L = 0x06 20 | __LED0_ON_H = 0x07 21 | __LED0_OFF_L = 0x08 22 | __LED0_OFF_H = 0x09 23 | __ALL_LED_ON_L = 0xFA 24 | __ALL_LED_ON_H = 0xFB 25 | __ALL_LED_OFF_L = 0xFC 26 | __ALL_LED_OFF_H = 0xFD 27 | 28 | # Bits 29 | __RESTART = 0x80 30 | __SLEEP = 0x10 31 | __ALLCALL = 0x01 32 | __INVRT = 0x10 33 | __OUTDRV = 0x04 34 | 35 | general_call_i2c = Adafruit_I2C(0x00) 36 | 37 | @classmethod 38 | def softwareReset(cls): 39 | "Sends a software reset (SWRST) command to all the servo drivers on the bus" 40 | cls.general_call_i2c.writeRaw8(0x06) # SWRST 41 | 42 | def __init__(self, address=0x40, debug=False): 43 | self.i2c = Adafruit_I2C(address) 44 | self.i2c.debug = debug 45 | self.address = address 46 | self.debug = debug 47 | if (self.debug): 48 | print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2" 49 | self.setAllPWM(0, 0) 50 | self.i2c.write8(self.__MODE2, self.__OUTDRV) 51 | self.i2c.write8(self.__MODE1, self.__ALLCALL) 52 | time.sleep(0.005) # wait for oscillator 53 | 54 | mode1 = self.i2c.readU8(self.__MODE1) 55 | mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep) 56 | self.i2c.write8(self.__MODE1, mode1) 57 | time.sleep(0.005) # wait for oscillator 58 | 59 | def setPWMFreq(self, freq): 60 | "Sets the PWM frequency" 61 | prescaleval = 25000000.0 # 25MHz 62 | prescaleval /= 4096.0 # 12-bit 63 | prescaleval /= float(freq) 64 | prescaleval -= 1.0 65 | if (self.debug): 66 | print "Setting PWM frequency to %d Hz" % freq 67 | print "Estimated pre-scale: %d" % prescaleval 68 | prescale = math.floor(prescaleval + 0.5) 69 | if (self.debug): 70 | print "Final pre-scale: %d" % prescale 71 | 72 | oldmode = self.i2c.readU8(self.__MODE1); 73 | newmode = (oldmode & 0x7F) | 0x10 # sleep 74 | self.i2c.write8(self.__MODE1, newmode) # go to sleep 75 | self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) 76 | self.i2c.write8(self.__MODE1, oldmode) 77 | time.sleep(0.005) 78 | self.i2c.write8(self.__MODE1, oldmode | 0x80) 79 | 80 | def setPWM(self, channel, on, off): 81 | "Sets a single PWM channel" 82 | self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) 83 | self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) 84 | self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) 85 | self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) 86 | 87 | def setAllPWM(self, on, off): 88 | "Sets a all PWM channels" 89 | self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF) 90 | self.i2c.write8(self.__ALL_LED_ON_H, on >> 8) 91 | self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF) 92 | self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8) 93 | -------------------------------------------------------------------------------- /robotcontrol/RobotServoExtents.xls: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mechiris/RobotControl/095738f313b8a33ee470401675bf334541869209/robotcontrol/RobotServoExtents.xls -------------------------------------------------------------------------------- /robotcontrol/Sequences.csv: -------------------------------------------------------------------------------- 1 | sequence;teachpoints;delays;loop 2 | Pick_Place_cups;safety, left_pick_hover, left_pick, grip_closed, left_pick_hover, right_pick_hover, grip_open, right_pick;1,1,1,1,1,1,1,1;True 3 | Spinmove;safety, left_pick_hover, left_pick, grip_closed, left_pick_hover, right_pick_hover, grip_open, right_pick;1,1,1,1,1,1,1,1;False 4 | Standard;left_pick_hover, left_pick, grip_pick_closed, left_pick_hover, left_pick, grip_open;1,1,1,1,1,1;True 5 | Shotglass;safety,left_pick, grip_closed, left_pick_hover, rotate_glass, left_pick_hover, left_pick, grip_open, safety;1,1,1,1,1,1,1,1,1;True 6 | -------------------------------------------------------------------------------- /robotcontrol/Teachpoints.csv: -------------------------------------------------------------------------------- 1 | Position,delay,s0,s1,s2,s3,s4,s5 2 | right_dump,,,,,,, 3 | right_pick_hover,,400.0,300.0,450.0,525.0,340.0, 4 | right_pick,,400.0,400.0,450.0,610.0,340.0, 5 | left_dump,,,,,,, 6 | left_pick_hover,,575.0,300.0,450.0,525.0,340.0, 7 | left_pick_stage,,575.0,400.0,450.0,610.0,340.0, 8 | left_pick,,575.0,380.0,360.0,675.0,, 9 | grip_open,,,,,,,260.0 10 | grip_closed,,,,,,,425.0 11 | grip_pick_closed,,,,,,,325.0 12 | safety,1.0,500.0,210.0,350.0,525.0,340.0,300.0 13 | rest,,500.0,150.0,340.0,700.0,340.0,300.0 14 | shotlift,,575.0,310.0,390.0,595.0,340.0, 15 | rotate_glass,,,,,,550, 16 | -------------------------------------------------------------------------------- /robotcontrol/__init__.py: -------------------------------------------------------------------------------- 1 | #init placeholder 2 | -------------------------------------------------------------------------------- /robotcontrol/__main__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from robotcontrol import RobotControl 3 | 4 | def main(args=None): 5 | """The main routine.""" 6 | if args is None: 7 | args = sys.argv[1:] 8 | 9 | RC = RobotControl() 10 | RC.start() 11 | # Do argument parsing here (eg. with argparse) and anything else 12 | # you want your project to do. 13 | 14 | if __name__ == "__main__": 15 | main() 16 | -------------------------------------------------------------------------------- /robotcontrol/dataposter.py: -------------------------------------------------------------------------------- 1 | from multiprocessing import Process, Pipe, Manager, Value 2 | import datetime, time 3 | import pandas as pd 4 | import numpy as np 5 | import subprocess as sbp 6 | import logging 7 | import os 8 | import glob 9 | import json 10 | import csv 11 | import copy 12 | 13 | class DataPoster(): 14 | 15 | def generateTrajectory(self, sequence): 16 | trajectory = pd.DataFrame(columns=self.teachpoints.columns) 17 | cur_seq = self.sequences[self.sequences['sequence'] == sequence].iloc[0] 18 | 19 | pts = [str(p).strip() for p in cur_seq['teachpoints'].split(',')] 20 | delays = cur_seq['delays'].split(',') 21 | 22 | for x,p in enumerate(pts): 23 | tp = self.teachpoints[self.teachpoints['Position'] == p].iloc[0] 24 | tp['delay']= delays[x] 25 | trajectory = trajectory.append(tp) 26 | trajectory.index = np.arange(0,trajectory.shape[0]) 27 | trajectory = trajectory.fillna(method='pad') 28 | fulltrajectory = self.interpolateFullTrajectory(trajectory) 29 | return fulltrajectory 30 | 31 | def interpolateFullTrajectory(self,trajectory, pollrate = 0.1): 32 | fulltrajectory = pd.DataFrame(columns=list(self.teachpoints.columns) + ['Time']) 33 | cur_time = 0 34 | for x,row in enumerate(trajectory.iterrows()): 35 | if x >= trajectory.shape[0]-1: 36 | nextrow = trajectory.iloc[0] 37 | else: 38 | nextrow = trajectory.iloc[x+1] 39 | cur_move = pd.DataFrame(columns=fulltrajectory.columns) 40 | cur_move['Time'] = np.arange(cur_time,cur_time+float(row[1]['delay']),pollrate) 41 | for col in row[1].keys()[2:]: 42 | cur_move[col] = np.linspace(row[1][col],nextrow[col],cur_move.shape[0]) 43 | 44 | cur_move['Position'] = row[1]['Position'] 45 | cur_move.loc[0,'Position':'s5']= row[1] 46 | fulltrajectory = fulltrajectory.append(cur_move) 47 | cur_time += float(row[1]['delay']) 48 | #cur_move[.iloc[0] 49 | fulltrajectory.index = np.arange(0,fulltrajectory.shape[0]) 50 | fulltrajectory['counter'] = self.counter 51 | return fulltrajectory 52 | 53 | 54 | def saveTrajectory(self,fulltrajectory): 55 | outfile = self.path+time.strftime("%Y%m%d-%H%M%S") + '.csv' 56 | fulltrajectory.to_csv(outfile) 57 | sleeptime = fulltrajectory['Time'].iloc[-1] - fulltrajectory['Time'].iloc[0] 58 | print('Saving {}s trajectory to: {}'.format(sleeptime,outfile)) 59 | time.sleep(sleeptime) 60 | 61 | def postSequence(self, initstate, sequence='Pick_Place_cups'): 62 | if not sequence in self.trajectories: 63 | logging.info('Trajectory not found, generating') 64 | self.trajectories[sequence] = self.generateTrajectory(sequence) 65 | 66 | while(True): 67 | cur_trajectory = self.trajectories[sequence].copy() 68 | ## add noise 69 | cur_trajectory = self.addNoise(cur_trajectory) 70 | cur_trajectory['counter'] = self.counter 71 | cur_trajectory['timestamp'] = 0 72 | for x in np.arange(0,cur_trajectory.shape[0]): 73 | cur_trajectory.loc[x,'timestamp'] = datetime.datetime.utcnow() + datetime.timedelta(0,cur_trajectory.loc[x,'Time']) 74 | # cur_trajectory['timestamp'].iloc[x] = datetime.datetime.utcnow() + datetime.timedelta(0,cur_trajectory['Time'].iloc[x]) 75 | self.saveTrajectory(cur_trajectory) 76 | if self.state['state'] != initstate: 77 | break; 78 | self.counter += 1 79 | self.changeState() 80 | 81 | def postStopped(self,initstate): 82 | while(True): 83 | print('Posting stopped') 84 | if self.state['state'] != initstate: 85 | break; 86 | time.sleep(1) 87 | self.changeState() 88 | 89 | def holdingPattern(self,initstate): 90 | while(True): 91 | print('holdingPattern') 92 | time.sleep(1) 93 | if self.state['state'] != initstate: 94 | break; 95 | self.changeState() 96 | 97 | def addNoise(self,fulltrajectory): 98 | #adds noise to a dataframe with servo trajectory values 99 | for x,row in enumerate(fulltrajectory.iterrows()): 100 | row[1][2:-2] *= 1+np.random.rand()/100 101 | fulltrajectory.loc[x,2:-2] = row[1][2:-2] 102 | return fulltrajectory 103 | 104 | def shutdown(self): 105 | print('Shutting down') 106 | return 107 | 108 | def changeState(self): 109 | options = { 110 | 'Initializing' : None, 111 | 'mainMenu' : self.postStopped, 112 | 'runSequence' : self.postSequence, 113 | 'goToTeachPoint' : None, 114 | 'goToServoPosition' : None, 115 | 'shutdown' : self.shutdown, 116 | } 117 | newState = options[self.state['state']] 118 | if newState: 119 | newState(self.state['state']) 120 | else: 121 | self.holdingPattern(self.state['state']) #wait for another state change 122 | 123 | def initialize(self,teachpoints,sequences,state,path='/opt/sightmachine/data/rpi/'): 124 | self.teachpoints = teachpoints 125 | self.sequences = sequences 126 | self.state = state 127 | self.trajectories = {} #dict of trajectories to save for sslog collection 128 | self.keySequence = '' 129 | self.path = path 130 | 131 | def __init__(self): 132 | ### Configureable parameters ### 133 | # Breathing Motion 134 | print('created poster class') 135 | self.counter = 0 136 | -------------------------------------------------------------------------------- /robotcontrol/dataposter_testharness.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | from dataposter import DataPoster 3 | from multiprocessing import Process, Pipe, Manager, Value 4 | import os 5 | 6 | teachpoints = pd.read_csv('Teachpoints.csv') 7 | sequences = pd.read_csv('Sequences.csv',delimiter=';') 8 | state = Manager().dict() #multiprocessing thread safe value passing 9 | state['state'] = 'runSequence' 10 | 11 | directory = '/opt/sightmachine/data/demorobotdata/' 12 | if not os.path.exists(directory): 13 | os.makedirs(directory) 14 | 15 | 16 | dp = DataPoster() 17 | dp.initialize(teachpoints,sequences,state,directory) 18 | 19 | dp.postSequence(state['state']) 20 | -------------------------------------------------------------------------------- /robotcontrol/robotcontrol.py: -------------------------------------------------------------------------------- 1 | from multiprocessing import Process, Pipe, Manager, Value 2 | from Adafruit_PWM_Servo_Driver import PWM 3 | from dataposter import DataPoster 4 | import RPi.GPIO as GPIO 5 | import datetime, time 6 | import pandas as pd 7 | import numpy as np 8 | import subprocess as sbp 9 | import logging 10 | import os 11 | import glob 12 | import json 13 | import csv 14 | import copy 15 | import thread 16 | 17 | 18 | 19 | class RobotControl(): 20 | def setServoPulse(self,channel, pulse): 21 | pulseLength = 1000000 # 1,000,000 us per second 22 | pulseLength /= 60 # 60 Hz 23 | pulseLength /= 4096 # 12 bits of resolution 24 | pulse *= 1000 25 | pulse /= pulseLength 26 | self.pwm.setPWM(channel, 0, pulse) 27 | 28 | def dataPoster(self, state): 29 | print('TODO: Flesh this out') 30 | 31 | def start(self): 32 | logging.info('Run started') 33 | 34 | # self.dataPoster = DataPoster() 35 | # self.dataPoster.initialize(self.teachpoints,self.sequences,self.state) 36 | # self.parent_conn, self.child_conn = Pipe() 37 | # p = Process(target=self.dataPoster.changeState) 38 | # p.start() 39 | 40 | self.mainMenu() 41 | 42 | # p.join() 43 | 44 | 45 | def goToTeachPoint(self,teachpoint,delay=1,steps=25): 46 | #this simply jerks then does a linear ramp over the move delay. For longer moves, it is best practice to ramp at accel_max, move at vel_max, then decel. The throw of the ROT2U is negligable for this, YMMV. 47 | incdelay = 1.0*delay/steps 48 | tp = self.teachpoints[self.teachpoints['Position'] == teachpoint] 49 | if tp.shape<1: 50 | logging.info('Teachpoint not found') 51 | return 52 | movesarr = np.zeros((6,steps)) 53 | for channel,col in enumerate(tp.columns[2:]): 54 | position = tp.loc[tp.index[0],col] 55 | if ~pd.isnull(position): 56 | movesarr[channel] = np.linspace(self.servoPositions[channel],position,steps) 57 | else: 58 | movesarr[channel] = np.ones(steps) * self.servoPositions[channel] #dont move this servo 59 | movesarr = np.round(movesarr) 60 | for x in np.arange(0,steps): 61 | cur_positions = movesarr[:,x] 62 | for channel,value in enumerate(cur_positions): 63 | self.pwm.setPWM(channel,0,int(value)) 64 | self.servoPositions[channel] = int(value) 65 | time.sleep(incdelay) 66 | 67 | def goToServoPositionSmooth(self,channel,position,delay=1,steps=25): 68 | #this simply jerks then does a linear ramp over the move delay. For longer moves, it is best practice to ramp at accel_max, move at vel_max, then decel. The throw of the ROT2U is negligable for this, YMMV. 69 | incdelay = 1.0*delay/steps 70 | start_position = self.servoPositions[channel] 71 | inc_step = 1.0*(position - start_position)/steps 72 | new_pos = start_position 73 | 74 | while(True): 75 | arrived = False 76 | new_pos = int(new_pos + inc_step) 77 | 78 | if (start_position > position): 79 | if (new_pos<=position): 80 | arrived = True 81 | new_pos = position 82 | else: 83 | if (new_pos >= position): 84 | arrived = True 85 | new_pos = position 86 | self.pwm.setPWM(channel,0,new_pos) 87 | self.servoPositions[channel] = new_pos 88 | if arrived: 89 | break 90 | print('moved {}, sleeping for {}'.format(new_pos,incdelay)) 91 | time.sleep(incdelay) 92 | 93 | 94 | 95 | 96 | self.pwm.setPWM(channel,0,position) 97 | self.servoPositions[channel] = position #update current log of positions 98 | 99 | 100 | def goToServoPosition(self,channel,position): 101 | self.pwm.setPWM(channel,0,position) 102 | self.servoPositions[channel] = position #update current log of positions 103 | 104 | def printCurrentServoPositions(self): 105 | for x,pos in enumerate(self.servoPositions): 106 | print(str(x)+ ": " + str(pos)) 107 | self.changeState(0) 108 | 109 | 110 | def goToTeachPointMenu(self): 111 | while (True): 112 | print('\n') 113 | teachPoint = raw_input("Please input a teachpoint to go to. Enter l to list available teachpoints, q to return to the main menu: ") 114 | if teachPoint == 'l': 115 | for pos in self.teachpoints['Position'].unique(): 116 | print(pos) 117 | elif teachPoint == 'q': 118 | break; 119 | else: 120 | self.goToTeachPoint(teachPoint) 121 | self.changeState(0) 122 | 123 | 124 | def runSequenceMenu(self): 125 | while (True): 126 | print('\n') 127 | sequence = raw_input("Please input a sequence to run. Enter l to list available sequences, q to return to the main menu: ") 128 | if sequence == 'l': 129 | for seq in self.sequences['sequence'].unique(): 130 | print(seq) 131 | elif sequence == 'q': 132 | break; 133 | else: 134 | self.runSequence(sequence) 135 | self.changeState(0) 136 | 137 | # def goToTeachPoint(self,teachpoint,delay=0): 138 | # tp = self.teachpoints[self.teachpoints['Position'] == teachpoint] 139 | # if tp.shape<1: 140 | # logging.info('Teachpoint not found') 141 | # return 142 | # for channel,col in enumerate(tp.columns[2:]): 143 | # position = tp.loc[tp.index[0],col] 144 | # if ~pd.isnull(position): 145 | # self.goToServoPosition(int(channel),int(position)) 146 | # if delay>0: 147 | # time.sleep(delay) 148 | 149 | def goToServoPositionMenu(self): 150 | servoextent = '' 151 | arrowDelta = 10 #delta to move when using arrow keys 152 | while (True): 153 | if servoextent == 'q': 154 | break; 155 | servoChannel = raw_input("Please input a servo channel (0-5). Enter q to return to the main menu: ") 156 | if servoChannel == 'q': 157 | break; 158 | while(True): 159 | servoextent = raw_input("Please input a servo extent from 0 to 4096, up or down arrows to nudge, or s to switch servo channel, q to return to the main menu") 160 | ## TODO: error input handling 161 | if servoextent == "s": 162 | logging.info('Switching channels') 163 | break; 164 | elif servoextent == "q": 165 | logging.info('Return to main menu') 166 | break; 167 | elif servoextent == '\x1b[A': #up 168 | self.goToServoPosition(int(servoChannel),int(self.servoPositions[int(servoChannel)]+arrowDelta)) 169 | elif servoextent == '\x1b[B': 170 | self.goToServoPosition(int(servoChannel),int(self.servoPositions[int(servoChannel)]-arrowDelta)) 171 | else: 172 | self.goToServoPosition(int(servoChannel), int(servoextent)) 173 | self.changeState(0) 174 | 175 | def input_thread(self, a_list): 176 | raw_input("Press enter to stop sequence") 177 | a_list.append(True) 178 | print('Stopping loop after this sequence run completes') 179 | 180 | def runSequence(self, sequence): 181 | try: 182 | cur_seq = self.sequences[self.sequences['sequence'] == sequence].iloc[0] 183 | pts = [str(p).strip() for p in cur_seq['teachpoints'].split(',')] 184 | delays = cur_seq['delays'].split(',') 185 | except: 186 | logging.info('No sequence ') 187 | if len(delays) != len(pts): 188 | logging.info('Delays array is a different length than teachpoints. Generating delays') 189 | delays = np.ones(len(pts)) 190 | 191 | print('Running sequence') 192 | 193 | a_list = [] 194 | if cur_seq['loop']: 195 | thread.start_new_thread(self.input_thread, (a_list,)) 196 | while not a_list: 197 | for x,pt in enumerate(pts): 198 | #print(pt) 199 | self.goToTeachPoint(pt,int(delays[x])) 200 | if not cur_seq['loop']: 201 | break 202 | 203 | def shutdown(self): 204 | logging.info('Shutting down system') 205 | self.goToTeachPoint('safety') 206 | time.sleep(0.5) 207 | self.goToTeachPoint('rest') 208 | 209 | def testSequence(self): 210 | while(True): 211 | self.goToTeachPoint('safety') 212 | time.sleep(1) 213 | self.goToTeachPoint('left_pick_hover') 214 | time.sleep(1) 215 | self.goToTeachPoint('left_pick') 216 | time.sleep(1) 217 | self.goToTeachPoint('grip_closed') 218 | time.sleep(1) 219 | self.goToTeachPoint('left_pick_hover') 220 | time.sleep(1) 221 | self.goToTeachPoint('safety') 222 | time.sleep(1) 223 | self.goToTeachPoint('right_pick_hover') 224 | time.sleep(1) 225 | self.goToTeachPoint('grip_open') 226 | time.sleep(1) 227 | self.goToTeachPoint('right_pick') 228 | time.sleep(1) 229 | self.goToTeachPoint('grip_closed') 230 | time.sleep(1) 231 | self.goToTeachPoint('right_pick_hover') 232 | time.sleep(1) 233 | 234 | def changeState(self, newState): 235 | try: 236 | states = { 237 | 0 : 'mainMenu', 238 | 1 : 'runSequence', 239 | 2 : 'goToTeachPoint', 240 | 3 : 'goToServoPosition', 241 | 4 : 'printCurrentServoPositions', 242 | 5 : 'shutdown', 243 | } 244 | self.state['state'] = states[newState] 245 | 246 | options = { 247 | 0 : self.mainMenu, 248 | 1 : self.runSequenceMenu, 249 | 2 : self.goToTeachPointMenu, 250 | 3 : self.goToServoPositionMenu, 251 | 4 : self.printCurrentServoPositions, 252 | 5 : self.shutdown, 253 | } 254 | options[newState]() 255 | except: 256 | print('Invalid option, returning to main menu') 257 | self.changeState(0) 258 | 259 | 260 | 261 | def mainMenu(self): 262 | print('\n\n\n') 263 | print('Main Menu:____________________________________________________________________________________') 264 | print('Please enter a number to select one of the following options:') 265 | print('1. Run Sequence') 266 | print('2. Go To TeachPoint') 267 | print('3. Go To Servo Position') 268 | print('4. List Current Servo Positions') 269 | print('5. Shutdown') 270 | newState = raw_input("Please input a number: ") 271 | self.changeState(int(newState)) 272 | 273 | def __init__(self): 274 | number_of_servos = 6 275 | 276 | installdir = os.path.dirname(__file__) 277 | 278 | logging.basicConfig(filename=installdir + '/robotcontrol.log',level=logging.INFO) 279 | self.teachpoints = pd.read_csv(installdir + '/Teachpoints.csv') 280 | self.sequences = pd.read_csv(installdir + '/Sequences.csv',delimiter=';') 281 | 282 | self.pwm = PWM(0x40) 283 | self.pwm.setPWMFreq(60) # Set frequency to 60 Hz 284 | self.servoPositions = self.teachpoints.loc[self.teachpoints['Position']=='rest'].iloc[:,2:].values[0] 285 | self.goToTeachPoint('safety') 286 | self.state = Manager().dict() #multiprocessing thread safe value passing 287 | self.state['state'] = 'Initializing' 288 | 289 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup(name='RobotControl', 4 | version='0.9.0', 5 | packages=['robotcontrol'], 6 | entry_points={ 7 | 'console_scripts': [ 8 | 'robotcontrol = robotcontrol.__main__:main' 9 | ] 10 | }, 11 | ) 12 | --------------------------------------------------------------------------------