This fixes #7. Accelerometers' data converted to angles. SDK initiali…

…zes from accelerometers and doesn't require a specific pose upon initialization any more (which wasn't feasible and repeatable anyway). Calibration instructions and tool created.

application/fpga/python/DobotDriver.py

@@ -8,7 +8,7 @@
 Author: maxosprojects (March 18 2016)
 Additional Authors: <put your name here>
 
-Version: 0.3.0
+Version: 0.4.0
 
 License: MIT
 """
@@ -17,6 +17,7 @@
 import threading
 import time
 from serial import SerialException
+import math
 
 _max_trys = 1
 
@@ -26,6 +27,9 @@
 CMD_GET_ACCELS = 3
 CMD_SWITCH_TO_ACCEL_REPORT_MODE = 4
 
+piToDegrees = 180.0 / math.pi
+halfPi = math.pi / 2.0
+
 
 class DobotDriver:
 	def __init__(self, comport, rate=115200):
@@ -278,6 +282,15 @@ def stepsToCmdVal(self, steps):
 			return 0x0242f000;
 		return self.reverseBits32(long(500000/steps))
 
+	def accelToAngle(self, val, offset):
+		return self.accelToRadians(val, offset) * piToDegrees
+
+	def accelToRadians(self, val, offset):
+		try:
+			return math.asin(float(val - offset) / 493.56)
+		except ValueError:
+			return halfPi
+
 	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/DobotInverseKinematics.py

@@ -22,11 +22,19 @@
 algorithm
 
 xy plane is paralell to surface dobot is on. z is perpendicular
-1. first get distance, in xy plane, to current point from origin using forward kinematics, known angles, and pythagoreas thm. This is your radius. Your angle can be defined to be theta original. You now have your starting point in polar coordinates.
-2. Ignore the desired z position data for now. Get the polar coordinates for the desired point. The radius is not important now. The angle is though. Subtracting the desired angle from the current angle gives you the number of degrees and direction to rotate the base.
-3. The radius from step 1 (starting radius) gives you your current horizontal position (imagine it as x or y, doesn't matter). You also already know your current z position (potentially from step 1).
-4. The radius from step 2 (desired radius) gives you your desired horizontal position (imagine it as x or y, doesn't matter). Of course, the user has already input the desired z position. This is now a 2D problem with two arms that each rotate (2 degrees of freedom)
-5: use IK, see ik 2d video, to find number of degrees and direction to rotate rear and fore arms. Note that there are often two solutions. One (elbow down) is not possible.
+1. first get distance, in xy plane, to current point from origin using forward kinematics, known angles,
+	and pythagoreas thm. This is your radius. Your angle can be defined to be theta original. You now have your
+	starting point in polar coordinates
+2. Ignore the desired z position data for now. Get the polar coordinates for the desired point. The radius is
+	not important now. The angle is though. Subtracting the desired angle from the current angle gives you the
+	number of degrees and direction to rotate the base
+3. The radius from step 1 (starting radius) gives you your current horizontal position (imagine it as x or y,
+	doesn't matter). You also already know your current z position (potentially from step 1)
+4. The radius from step 2 (desired radius) gives you your desired horizontal position (imagine it as x or y,
+	doesn't matter). Of course, the user has already input the desired z position. This is now a 2D problem with
+	two arms that each rotate (2 degrees of freedom)
+5: use IK, see ik 2d video, to find number of degrees and direction to rotate rear and fore arms. Note that there
+	are often two solutions. One (elbow down) is not possible.
 6. Check that move is valid (e.g. not out of range, etc...)
 7. move
 
@@ -58,7 +66,7 @@ def _debug(self, *args):
 	#cartesian (x,y,z) coordinate
 	#robot dimensions
 	#output:
-	#angles for robot arm base, rear, and fore arms in degree
+	#angles for robot arm base, rear, and fore arms in degrees
 	def convert_cartesian_coordinate_to_arm_angles(self, x, y, z):
 
 		# do a general check to see if even a maximally stretched arm could reach the point
@@ -151,7 +159,8 @@ def check_for_angle_limits_is_valid(self, baseAngle, rearArmAngle, foreArmAngle)
 			ret = False
 
 		# check the foreArmAngle
-		# the valid forearm angle is dependent on the rear arm angle. The real world angle of the forearm (0 degrees = horizontal) needs to be evaluated.
+		# the valid forearm angle is dependent on the rear arm angle. The real world angle of the forearm
+		# (0 degrees = horizontal) needs to be evaluated.
 		# min empirically determined to be around -105 degrees. Using -102.
 		# max empirically determined to be around 21 degrees. Using 18.
 		if (-102 > foreArmAngle > 18):
@@ -161,11 +170,11 @@ def check_for_angle_limits_is_valid(self, baseAngle, rearArmAngle, foreArmAngle)
 		return ret
 
 """
-	#get polar coordinates for the current point
-	#radius is simply given by the base angle
-	# can read the angles from the IMU and empirically determine the radius and angle - I'm using this approach since it should account for any build up in error, assuming accelerometers
-	#are accurate!!!!
-	#alternatively can just use pythagorean thm on theoretical current x,y data
+	# get polar coordinates for the current point
+	# radius is simply given by the base angle
+	# can read the angles from the IMU and empirically determine the radius and angle - I'm using this approach
+	# since it should account for any build up in error, assuming accelerometers are accurate!!!!
+	# alternatively can just use pythagorean thm on theoretical current x,y data
 
 	startRearArmAngle = get_rear_arm_angle
 	startForeArmAngle = get_fore_arm_angle

application/fpga/python/DobotSDK.py

@@ -1,26 +1,26 @@
 """
 open-dobot SDK.
 
-SDK providing high-level functions to control Dobot via the driver to open firmware that controls Dobot FPGA.
+SDK providing high-level functions to control Dobot via the driver to open firmware, which, in turn, 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.
+It is assumed that upon SDK initialization the arms are beetween 0 and 90 degrees - beetween their normal
+horizontal and vertical positions.
+Upon initialization accelerometers are read to figure out current arms' configuration. Accelerometers get confused
+when rear arm leans backwards from the dobot base or when forearm bends towards the base.
+Also, Inverse Kinematics at the moment don't account for when forearm is looking up (higher than it's
+normal horizontal position). So be gentle and give dobot some feasible initial configuration in case it happened
+to be beyond the mentioned limits.
+Refer to docs/images/ to find more about reference frame, arm names and more.
 
-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.
+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 and SDK re-initialized.
 
 Author: maxosprojects (March 18 2016)
 Additional Authors: <put your name here>
 
-Version: 0.3.0
+Version: 0.4.0
 
 License: MIT
 """
@@ -32,6 +32,10 @@
 from DobotDriver import DobotDriver
 from DobotInverseKinematics import DobotInverseKinematics
 import timeit
+import math
+
+# calibration-tool.py for details
+accelOffsets = (1024, 1024)
 
 # The NEMA 17 stepper motors that Dobot uses are 200 steps per revolution.
 stepperMotorStepsPerRevolution = 200.0
@@ -54,15 +58,15 @@ def __init__(self, port, rate=115200, timeout=0.025, debug=False):
 		self._driver = DobotDriver(port, rate)
 		self._driver.Open(timeout)
 		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._foreAngle = 0.0
+		# Initialize arms current configuration from accelerometers
+		accels = self._driver.GetAccelerometers()
+		accelRear = accels[1]
+		accelFore = accels[2]
+		rearAngle = math.pi / 2 - self._driver.accelToRadians(accelRear, accelOffsets[0])
+		foreAngle = self._driver.accelToRadians(accelFore, accelOffsets[1])
 		self._baseSteps = long(0)
-		self._rearSteps = long(0)
-		self._foreSteps = long(0)
+		self._rearSteps = long((rearAngle / math.pi / 2.0) * rearArmActualStepsPerRevolution + 0.5)
+		self._foreSteps = long((foreAngle / math.pi / 2.0) * foreArmActualStepsPerRevolution + 0.5)
 
 	def _debug(self, *args):
 		if self._debugOn:

application/fpga/python/calibration-tool.py

@@ -0,0 +1,49 @@
+#! /usr/bin/env python
+
+"""
+open-dobot Calibration Tool.
+
+This tool continuously reports accelerometers and angles from those.
+
+Use this tool to find offsets for your accelerometers:
+1. Turn off power on the arm and disconnect USB cable
+2. Remove accelerometers from the arm and put them on a flat surface that has no inclination
+3. Connect USB cable
+4. Enable accelerometers reporting mode
+4.1. Push and "sensor Calibration" button
+4.2. Push and release "Reset" button
+4.3. Start this tool (still holding "Sensor Calibration" button)
+4.4. Wait for the accelerometer data to start flowing on your console/whatever_you_use_to_start_this_tool
+4.5. Release the "Sensor Calibration" button
+5 Gently push the accelerometers so that they are evenly on the surface. Don't touch any contacts/leads.
+	You can push them one by one, not necessary to push both at the same time
+6. Note the "Raw" data from accelerometers reported on the console. Those are your accelerometers' offsets
+7. Turn off power on the arm, disconnect USB cable, mount accelerometers back onto the arm 
+
+Author: maxosprojects (March 18 2016)
+Additional Authors: <put your name here>
+
+Version: 0.4.0
+
+License: MIT
+"""
+import math
+
+from DobotDriver import DobotDriver
+
+driver = DobotDriver('/dev/tty.usbmodem1421', 115200)
+driver.Open()
+
+# Offsets must be found using this tool for your Dobot once
+# (rear arm, forearm)
+offsets = (1024, 1024)
+
+def toEndEffectorHeight(rear, fore):
+	return 88.0 - 160.0 * math.sin(fore) + 135.0 * math.sin(rear)
+
+while True:
+	ret = driver.GetAccelerometers()
+	print "Rear arm: {0:10f} | Forearm: {1:10f} | End effector height: {2:10f} | Raw rear arm: {3:4d} | Raw forearm: {4:4d}".format(\
+		driver.accelToAngle(ret[1], offsets[0]), driver.accelToAngle(ret[2], offsets[1]),\
+		toEndEffectorHeight(driver.accelToRadians(ret[1], offsets[0]), driver.accelToRadians(ret[2], offsets[1])),\
+		ret[1], ret[2])

application/fpga/python/example-sdk.py

@@ -19,17 +19,20 @@
 
 dobot = Dobot('/dev/tty.usbmodem1421', debug=True)
 
-dobot.moveTo(160.0, 0.0, 60.0, 1)
+duration = 1
+
+dobot.moveTo(160.0, 0.0, 60.0, duration)
 print '======================================'
 for i in range(3):
-	dobot.moveTo(160.0, 60.0, 60.0, 1)
+	dobot.moveTo(160.0, 60.0, 60.0, duration)
 	print '======================================'
-	dobot.moveTo(220.0, 60.0, 60.0, 1)
+	dobot.moveTo(220.0, 60.0, 60.0, duration)
 	print '======================================'
-	dobot.moveTo(220.0, -60.0, 60.0, 1)
+	dobot.moveTo(220.0, -60.0, 60.0, duration)
 	print '======================================'
-	dobot.moveTo(160.0, -60.0, 60.0, 1)
+	dobot.moveTo(160.0, -60.0, 60.0, duration)
 	print '======================================'
-dobot.moveTo(160.0, 0.0, 60.0, 1)
+dobot.moveTo(160.0, 0.0, 60.0, duration)
+print '======================================'
+dobot.moveTo(160.0, 0.0, 215.0, duration)
 print '======================================'
-dobot.moveTo(160.0, 0.0, 215.0, 1)