This closes #4. Initial version of SDK implemented providing high-lev…

…el functions to move arm to specific coordinates. SDK example provided. FPGA SPI protocol decoded.

application/fpga/python/DobotDriver.py

@@ -1,3 +1,18 @@
+"""
+open-dobot driver.
+
+Implements driver to open firmware that controls Dobot FPGA.
+Abstracts communication protocol, CCITT CRC and commands sent to FPGA.
+Find firmware and SDK at https://github.com/maxosprojects/open-dobot
+
+Author: maxosprojects (March 18 2016)
+Additional Authors: <put your name here>
+
+Version: 0.3.0
+
+License: MIT
+"""
+
 import serial
 import threading
 import time
@@ -13,7 +28,7 @@
 
 
 class DobotDriver:
-	def __init__(self, comport, rate):
+	def __init__(self, comport, rate=115200):
 		self._lock = threading.Lock()
 		self._comport = comport
 		self._rate = rate
@@ -242,6 +257,27 @@ def _write14441read1(self, cmd, val1, val2, val3, val4):
 									(self._writelong, val3),
 									(self._writebyte, val4)])
 
+	def reverseBits32(self, val):
+		# return long(bin(val&0xFFFFFFFF)[:1:-1], 2)
+		return int('{0:032b}'.format(val)[::-1], 2)
+
+	def freqToCmdVal(self, freq):
+		'''
+		Converts stepping frequency into a command value that dobot takes.
+		'''
+		if freq == 0:
+			return 0x0242f000;
+		return self.reverseBits32(long(25000000/freq))
+
+	def stepsToCmdVal(self, steps):
+		'''
+		Converts number of steps for dobot to do in 20ms into a command value that dobot
+		takes to set the stepping frequency.
+		'''
+		if steps == 0:
+			return 0x0242f000;
+		return self.reverseBits32(long(500000/steps))
+
 	def Steps(self, j1, j2, j3, j1dir, j2dir, j3dir, deferred=False):
 		'''
 		Adds a command to the controller's queue to execute on FPGA.

application/fpga/python/DobotSDK.py

@@ -1,3 +1,30 @@
+"""
+open-dobot SDK.
+
+SDK providing high-level functions to control Dobot via the driver to open firmware that controls Dobot FPGA.
+Abstracts specifics of commands sent to FPGA.
+Find firmware and driver at https://github.com/maxosprojects/open-dobot
+
+It is assumed that upon SDK initialization the arms are set as follows:
+- rear arm is set vertically
+- forearm is set horizontally
+Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find more about reference
+frame and arm names.
+SDK keeps track of the current end effector pose, thus in case the arm slips or motors are disabled while
+in move (with the "Laser Adjustment" button) it has to be re-initialized - the arm set to initial configuration
+(end effector to initial pose) and SDK re-initialized.
+
+Proper initialization based on data from accelerometers is under development (planned for 0.4.0). This would
+enable to initialize from almost any end effector pose.
+
+Author: maxosprojects (March 18 2016)
+Additional Authors: <put your name here>
+
+Version: 0.3.0
+
+License: MIT
+"""
+
 import serial
 import threading
 import time
@@ -10,54 +37,60 @@
 stepperMotorStepsPerRevolution = 200.0
 # FPGA board has all stepper drivers' stepping pins set to microstepping.
 baseMicrosteppingMultiplier = 16.0
-upperArmMicrosteppingMultiplier = 16.0
-lowerArmMicrosteppingMultiplier = 16.0
+rearArmMicrosteppingMultiplier = 16.0
+foreArmMicrosteppingMultiplier = 16.0
 # The NEMA 17 stepper motors Dobot uses are connected to a planetary gearbox, the black cylinders
 # with 10:1 reduction ratio
 stepperPlanetaryGearBoxMultiplier = 10.0
 
 # calculate the actual number of steps it takes for each stepper motor to rotate 360 degrees
 baseActualStepsPerRevolution = stepperMotorStepsPerRevolution * baseMicrosteppingMultiplier * stepperPlanetaryGearBoxMultiplier
-upperArmActualStepsPerRevolution = stepperMotorStepsPerRevolution * upperArmMicrosteppingMultiplier * stepperPlanetaryGearBoxMultiplier
-lowerArmActualStepsPerRevolution = stepperMotorStepsPerRevolution * lowerArmMicrosteppingMultiplier * stepperPlanetaryGearBoxMultiplier
+rearArmActualStepsPerRevolution = stepperMotorStepsPerRevolution * rearArmMicrosteppingMultiplier * stepperPlanetaryGearBoxMultiplier
+foreArmActualStepsPerRevolution = stepperMotorStepsPerRevolution * foreArmMicrosteppingMultiplier * stepperPlanetaryGearBoxMultiplier
 
 class Dobot:
-	def __init__(self, port, rate=115200, timeout=0.025):
+	def __init__(self, port, rate=115200, timeout=0.025, debug=False):
+		self._debugOn = debug
 		self._driver = DobotDriver(port, rate)
 		self._driver.Open(timeout)
-		self._ik = DobotInverseKinematics()
+		self._ik = DobotInverseKinematics(debug=debug)
 		self._x = 160.0
 		self._y = 0.0
 		self._z = 215.0
 		self._baseAngle = 0.0
 		self._rearAngle = 90.0
-		self._frontAngle = 0.0
+		self._foreAngle = 0.0
 		self._baseSteps = long(0)
 		self._rearSteps = long(0)
-		self._frontSteps = long(0)
-
-	def _reverseBits32(self, val):
-		# return long(bin(val&0xFFFFFFFF)[:1:-1], 2)
-		return int('{0:032b}'.format(val)[::-1], 2)
+		self._foreSteps = long(0)
 
-	def _freqToCmdVal(self, freq):
-		'''
-		Converts stepping frequency into a command value that dobot takes.
-		'''
-		if freq == 0:
-			return 0x0242f000;
-		return this._reverseBits32(long(25000000/freq))
+	def _debug(self, *args):
+		if self._debugOn:
+			# Since "print" is not a function the expansion (*) cannot be used
+			# as it is not an operator. So this is a workaround.
+			for arg in args:
+				print arg,
+			print
 
-	def _stepsToCmdVal(self, steps):
+	def moveTo(self, x, y, z, duration):
 		'''
-		Converts number of steps for dobot to do in 20ms into a command value that dobot
-		takes to set the stepping frequency.
+		Moves the arm to the location with provided coordinates. Makes sure it takes exactly
+		the amount of time provided in "duration" argument to get end effector to the location.
+		All axes arrive at the same time.
+		SDK keeps track of the current end effector pose.
+		The origin is at the arm's base (the rotating base plate - joint1).
+		Refer to docs/images/arm-description.png and docs/images/reference-frame.png to find
+		more about reference frame and arm names.
+		Current limitations:
+		- rear arm cannot go beyond initial 90 degrees (backwards from initial vertical position),
+		  so plan carefully the coordinates
+		- forearm cannot go above initial horizontal position
+		Not yet implemented:
+		- the move is not linear as motors run the whole path at constant speed
+		- acceleration/deceleration
+		- rear arm cannot go beyond 90 degrees (see current limitations) and there are no checks
+		  for that - it will simply go opposite direction instead
 		'''
-		if steps == 0:
-			return 0x0242f000;
-		return self._reverseBits32(long(500000/steps))
-
-	def moveTo(self, x, y, z):
 		xx = float(x)
 		yy = float(y)
 		zz = float(z)
@@ -68,75 +101,78 @@ def moveTo(self, x, y, z):
 			print 'Unreachable'
 			return
 
-		print 'ik base angle', moveToAngles[0]
-		print 'ik upper angle', moveToAngles[1]
-		print 'ik lower angle', moveToAngles[2]
+		self._debug('ik base angle', moveToAngles[0])
+		self._debug('ik rear angle', moveToAngles[1])
+		self._debug('ik fore angle', moveToAngles[2])
 
-		moveToUpperArmAngleFloat = moveToAngles[1]
-		moveToLowerArmAngleFloat = moveToAngles[2]
+		moveToRearArmAngleFloat = moveToAngles[1]
+		moveToForeArmAngleFloat = moveToAngles[2]
 
-		transformedUpperArmAngle = 90.0 - moveToUpperArmAngleFloat
+		transformedRearArmAngle = 90.0 - moveToRearArmAngleFloat
 		# -90 different from c++ code, accounts for fact that arm starts at the c++ simulation's 90
 		# note that this line is different from the similar line in the move angles function.
-		# Has to do with the inverse kinematics function and the fact that the lower arm angle is
-		# calculated relative to the upper arm angle.
-		transformedLowerArmAngle = 270.0 + transformedUpperArmAngle - moveToLowerArmAngleFloat
-		print 'transformed upper angle', transformedUpperArmAngle
-		print 'transformed lower angle', transformedLowerArmAngle
+		# Has to do with the inverse kinematics function and the fact that the forearm angle is
+		# calculated relative to the rear arm angle.
+		transformedForeArmAngle = 270.0 + transformedRearArmAngle - moveToForeArmAngleFloat
+		self._debug('transformed rear angle', transformedRearArmAngle)
+		self._debug('transformed fore angle', transformedForeArmAngle)
 
 		# check that the final angles are mechanically valid. note that this check only considers final
 		# angles, and not angles while the arm is moving
 		# need to pass in real world angles
-		# real world base and upper arm angles are those returned by the ik function.
-		# real world lower arm angle is -1 * transformedLowerArmAngle
-		if not self._ik.check_for_angle_limits_is_valid(moveToAngles[0], moveToAngles[1], -1 * transformedLowerArmAngle):
+		# real world base and rear arm angles are those returned by the ik function.
+		# real world fore arm angle is -1 * transformedForeArmAngle
+		if not self._ik.check_for_angle_limits_is_valid(moveToAngles[0], moveToAngles[1], -1 * transformedForeArmAngle):
 			return
 
-		self._moveArmToAngles(moveToAngles[0], transformedUpperArmAngle, transformedLowerArmAngle)
+		self._moveArmToAngles(moveToAngles[0], transformedRearArmAngle, transformedForeArmAngle, duration)
 
-	def _moveArmToAngles(self, baseAngle, upperArmAngle, lowerArmAngle):
+	def _moveArmToAngles(self, baseAngle, rearArmAngle, foreArmAngle, duration):
 		self._baseAngle = baseAngle
-		self._rearAngle = upperArmAngle
-		self._frontAngle = lowerArmAngle
+		self._rearAngle = rearArmAngle
+		self._foreAngle = foreArmAngle
+		dur = float(duration)
 
-		baseStepNumber = long((abs(baseAngle) / 360.0) * baseActualStepsPerRevolution + 0.5)
-		upperArmStepNumber = long((abs(upperArmAngle) / 360.0) * upperArmActualStepsPerRevolution + 0.5)
-		lowerArmStepNumber = long((abs(lowerArmAngle) / 360.0) * lowerArmActualStepsPerRevolution + 0.5)
+		baseStepLocation = long((baseAngle / 360.0) * baseActualStepsPerRevolution + 0.5)
+		rearArmStepLocation = long((abs(rearArmAngle) / 360.0) * rearArmActualStepsPerRevolution + 0.5)
+		foreArmStepLocation = long((abs(foreArmAngle) / 360.0) * foreArmActualStepsPerRevolution + 0.5)
 
-		print "Base Step Number", baseStepNumber
-		print "Upper Arm Step Number", upperArmStepNumber
-		print "Lower Arm Step Number", lowerArmStepNumber
+		self._debug("Base Step Location", baseStepLocation)
+		self._debug("Rear Arm Step Location", rearArmStepLocation)
+		self._debug("Forearm Step Location", foreArmStepLocation)
 
-		baseDiff = baseStepNumber - self._baseSteps
-		rearDiff = upperArmStepNumber - self._rearSteps
-		print 'rearDiff', rearDiff
-		frontDiff = lowerArmStepNumber - self._frontSteps
+		baseDiff = baseStepLocation - self._baseSteps
+		rearDiff = rearArmStepLocation - self._rearSteps
+		foreDiff = foreArmStepLocation - self._foreSteps
+		self._debug('baseDiff', baseDiff)
+		self._debug('rearDiff', rearDiff)
+		self._debug('foreDiff', foreDiff)
 
-		self._baseSteps = baseStepNumber
-		self._rearSteps = upperArmStepNumber
-		self._frontSteps = lowerArmStepNumber
+		self._baseSteps = baseStepLocation
+		self._rearSteps = rearArmStepLocation
+		self._foreSteps = foreArmStepLocation
 
 		baseDir = 1
 		rearDir = 1
-		frontDir = 1
+		foreDir = 1
 
 		if (baseDiff < 1):
 			baseDir = 0
 		if (rearDiff < 1):
 			rearDir = 0
-		if (frontDiff > 1):
-			frontDir = 0
+		if (foreDiff > 1):
+			foreDir = 0
 
-		baseSliced = self._sliceStepsToValues(abs(baseDiff), 1)
-		rearSliced = self._sliceStepsToValues(abs(rearDiff), 1)
-		frontSliced = self._sliceStepsToValues(abs(frontDiff), 1)
+		baseSliced = self._sliceStepsToValues(abs(baseDiff), dur)
+		rearSliced = self._sliceStepsToValues(abs(rearDiff), dur)
+		foreSliced = self._sliceStepsToValues(abs(foreDiff), dur)
 
-		for base, upper, lower in zip(baseSliced, rearSliced, frontSliced):
+		for base, rear, fore in zip(baseSliced, rearSliced, foreSliced):
 			ret = [0, 0]
 			# If ret[0] == 0 then command timed out or crc failed.
 			# If ret[1] == 0 then command queue was full.
 			while ret[0] == 0 or ret[1] == 0:
-				ret = self._driver.Steps(base, upper, lower, baseDir, rearDir, frontDir)
+				ret = self._driver.Steps(base, rear, fore, baseDir, rearDir, foreDir)
 
 	def _sliceStepsToValues(self, steps, duration):
 		'''
@@ -147,32 +183,7 @@ def _sliceStepsToValues(self, steps, duration):
 		num = long(duration / 0.02)
 		chunk = (steps / num)
 		for _ in range(num):
-			ret.append(self._stepsToCmdVal(chunk))
+			ret.append(self._driver.stepsToCmdVal(chunk))
 		return ret
 
 
-
-if __name__ == '__main__':
-
-	dobot = Dobot('/dev/tty.usbmodem1421')
-
-	# while True:
-	dobot.moveTo(160.0, 0.0, 150.0)
-	print '======================================'
-	dobot.moveTo(160.0, 60.0, 150.0)
-	print '======================================'
-	dobot.moveTo(220.0, 60.0, 150.0)
-	print '======================================'
-	dobot.moveTo(220.0, 0.0, 150.0)
-	print '======================================'
-	dobot.moveTo(160.0, 0.0, 150.0)
-	print '======================================'
-	dobot.moveTo(160.0, 60.0, 150.0)
-	print '======================================'
-	dobot.moveTo(220.0, 60.0, 150.0)
-	print '======================================'
-	dobot.moveTo(220.0, 0.0, 150.0)
-	print '======================================'
-	dobot.moveTo(160.0, 0.0, 150.0)
-	print '======================================'
-	dobot.moveTo(160.0, 0.0, 215.0)

application/fpga/python/example-accelerometers.py

@@ -3,7 +3,7 @@
 '''
 This example continuously reports accelerometers.
 Refer to _SwitchToAccelerometerReportMode function in DobotDriver.py for details on
-how to enable reporting mode and that way.
+how to enable reporting mode.
 '''
 
 from DobotDriver import DobotDriver