Initial commit

examples/IKTest/IKTest.ino

@@ -0,0 +1,45 @@
+/* meArm IK test - York Hackspace May 2014
+ * Test applying Nick Moriarty's Inverse Kinematics solver
+ * to Phenoptix' meArm robot arm, to walk a specified path.
+ *
+ * Pins:
+ * Arduino    Base   Shoulder  Elbow    Gripper
+ *    GND    Brown     Brown   Brown     Brown
+ *     5V      Red       Red     Red       Red
+ *     11    Yellow
+ *     10             Yellow
+ *      9                     Yellow
+ *      6                               Yellow
+ */
+#include "meArm.h"
+#include <Servo.h>
+
+int basePin = 11;
+int shoulderPin = 10;
+int elbowPin = 9;
+int gripperPin = 6;
+
+meArm arm;
+
+void setup() {
+  arm.begin(basePin, shoulderPin, elbowPin, gripperPin);
+}
+
+void loop() {
+  //Clap - so it's obvious we're at this part of the routine
+  arm.openGripper();
+  arm.closeGripper();
+  arm.openGripper();
+  arm.closeGripper();
+  arm.openGripper();
+  delay(500);
+  //Go up and left to grab something
+  arm.gotoPoint(-80,100,140); 
+  arm.closeGripper();
+  //Go down, forward and right to drop it
+  arm.gotoPoint(70,200,10);
+  arm.openGripper();
+  //Back to start position
+  arm.gotoPoint(0,100,50);
+  delay(2000);
+}

examples/JoysticksIK/JoysticksIK.ino

@@ -0,0 +1,58 @@
+/* meArm IK joysticks - York Hackspace May 2014
+ * Using inverse kinematics with joysticks
+ * Uses two analogue joystcks (two pots each)
+ * First stick moves gripper forwards, backwards, left and right
+ * Second stick moves gripper up, down, and closes and opens.
+ * 
+ * I used Sparkfun thumbstick breakout boards, oriented 'upside down'.
+ * 
+ * Pins:
+ * Arduino    Stick1    Stick2    Base   Shoulder  Elbow    Gripper
+ *    GND       GND       GND    Brown     Brown   Brown     Brown
+ *     5V       VCC       VCC      Red       Red     Red       Red
+ *     A0       HOR
+ *     A1       VER
+ *     A2                 HOR
+ *     A3                 VER
+ *     11                       Yellow
+ *     10                                 Yellow
+ *      9                                         Yellow
+ *      6                                                   Yellow
+ */
+#include "meArm.h"
+#include <Servo.h>
+
+int basePin = 11;
+int shoulderPin = 10;
+int elbowPin = 9;
+int gripperPin = 6;
+
+int xdirPin = 0;
+int ydirPin = 1;
+int zdirPin = 3;
+int gdirPin = 2;
+
+meArm arm;
+
+void setup() {
+  arm.begin(basePin, shoulderPin, elbowPin, gripperPin);
+}
+
+void loop() {
+  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
+  float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
+  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
+  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);
+  if (abs(dx) < 1.5) dx = 0;
+  if (abs(dy) < 1.5) dy = 0;
+  if (abs(dz) < 1.5) dz = 0;
+
+  if (!(dx == 0 && dy == 0 && dz == 0))
+    arm.goDirectlyTo(arm.getX() + dx, arm.getY() + dy, arm.getZ() + dz);
+
+  if (dg < -3.0)
+    arm.closeGripper();
+  else if (dg > 3.0)
+    arm.openGripper();  
+  delay(50);
+}

fk.cpp

@@ -0,0 +1,60 @@
+/* Forward kinetics, Nick Moriarty May 2014
+This code is provided under the terms of the MIT license.
+
+The MIT License (MIT)
+
+Copyright (c) 2014 Nick Moriarty
+
+Permission is hereby granted, free of charge, to any person obtaining a copy 
+of this software and associated documentation files (the "Software"), to deal 
+in the Software without restriction, including without limitation the rights 
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
+copies of the Software, and to permit persons to whom the Software is 
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in 
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 
+SOFTWARE.
+ */
+#include "math.h"
+#include "fk.h"
+#include "ik.h"
+
+void polar2cart(float r, float theta, float& a, float& b)
+{
+    a = r * cos(theta);
+    b = r * sin(theta);
+}
+
+void unsolve(float a0, float a1, float a2, float& x, float& y, float& z)
+{
+    // Calculate u,v coords for arm
+    float u01, v01, u12, v12;
+    polar2cart(L1, a1, u01, v01);
+    polar2cart(L2, a2, u12, v12);
+
+    // Add vectors
+    float u, v;
+    u = u01 + u12 + L3;
+    v = v01 + v12;
+
+    // Calculate in 3D space - note x/y reversal!
+    polar2cart(u, a0, y, x);
+    z = v;
+}
+
+float distance(float x1, float y1, float z1, float x2, float y2, float z2)
+{
+    float dx = x2-x1;
+    float dy = y2-y1;
+    float dz = z2-z1;
+
+    return dx*dx + dy*dy + dz*dz;
+}

fk.h

@@ -0,0 +1,10 @@
+#ifndef FK_H_INCLUDED
+#define FK_H_INCLUDED
+
+void polar2cart(float r, float theta, float& a, float& b);
+
+void unsolve(float a0, float a1, float a2, float& x, float& y, float& z);
+
+float distance(float x1, float y1, float z1, float x2, float y2, float z2);
+
+#endif // FK_H_INCLUDED

ik.cpp

@@ -0,0 +1,104 @@
+/* Inverse kinetics, Nick Moriarty May 2014
+This code is provided under the terms of the MIT license.
+
+The MIT License (MIT)
+
+Copyright (c) 2014 Nick Moriarty
+
+Permission is hereby granted, free of charge, to any person obtaining a copy 
+of this software and associated documentation files (the "Software"), to deal 
+in the Software without restriction, including without limitation the rights 
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
+copies of the Software, and to permit persons to whom the Software is 
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in 
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 
+SOFTWARE.
+ */
+#include "math.h"
+#include "ik.h"
+
+const float PI=3.14159265359;
+float L1=80; //Shoulder to elbow length
+float L2=80; //Elbow to wrise length
+float L3=68; //Length from wrist to hand PLUS base centre to shoulder
+
+// Get polar coords from cartesian ones
+void cart2polar(float a, float b, float& r, float& theta)
+{
+    // Determine magnitude of cartesian coords
+    r = sqrt(a*a + b*b);
+
+    // Don't try to calculate zero-magnitude vectors' angles
+    if(r == 0) return;
+
+    float c = a / r;
+    float s = b / r;
+
+    // Safety!
+    if(s > 1) s = 1;
+    if(c > 1) c = 1;
+    if(s < -1) s = -1;
+    if(c < -1) c = -1;
+
+    // Calculate angle in 0..PI
+    theta = acos(c);
+
+    // Convert to full range
+    if(s < 0) theta *= -1;
+}
+
+// Get angle from a triangle using cosine rule
+bool cosangle(float opp, float adj1, float adj2, float& theta)
+{
+    // Cosine rule:
+    // C^2 = A^2 + B^2 - 2*A*B*cos(angle_AB)
+    // cos(angle_AB) = (A^2 + B^2 - C^2)/(2*A*B)
+    // C is opposite
+    // A, B are adjacent
+    float den = 2*adj1*adj2;
+
+    if(den==0) return false;
+    float c = (adj1*adj1 + adj2*adj2 - opp*opp)/den;
+
+    if(c>1 || c<-1) return false;
+
+    theta = acos(c);
+
+    return true;
+}
+
+// Solve angles!
+bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
+{
+    // Solve top-down view
+    float r, th0;
+    cart2polar(y, x, r, th0);
+
+    // Account for the wrist length!
+    r -= L3;
+
+    // In arm plane, convert to polar
+    float ang_P, R;
+    cart2polar(r, z, R, ang_P);
+
+    // Solve arm inner angles as required
+    float B, C;
+    if(!cosangle(L2, L1, R, B)) return false;
+    if(!cosangle(R, L1, L2, C)) return false;
+
+    // Solve for servo angles from horizontal
+    a0 = th0;
+    a1 = ang_P + B;
+    a2 = C + a1 - PI;
+
+    return true;
+}

ik.h

@@ -0,0 +1,19 @@
+/* meArmIK - York Hackspace May 2014
+ * Inverse Kinematics solver for three degrees of freedom
+ * created for Phenoptix' meArm robot arm
+ */
+#ifndef IK_H_INCLUDED
+#define IK_H_INCLUDED
+
+extern float L1, L2, L3;
+
+// Get polar coords from cartesian ones
+void cart2polar(float a, float b, float& r, float& theta);
+
+// Get angle from a triangle using cosine rule
+bool cosangle(float opp, float adj1, float adj2, float& theta);
+
+// Solve angles!
+bool solve(float x, float y, float z, float& a0, float& a1, float& a2);
+
+#endif // IK_H_INCLUDED

keywords.txt

@@ -0,0 +1,22 @@
+#######################################
+# Syntax Coloring Map For meArm library
+#######################################
+
+#######################################
+# Datatypes (KEYWORD1)
+#######################################
+
+meArm		KEYWORD1
+
+#######################################
+# Methods and Functions (KEYWORD2)
+#######################################
+
+begin			KEYWORD2
+openGripper  	KEYWORD2
+closeGripper 	KEYWORD2
+gotoPoint		KEYWORD2
+goDirectlyTo	KEYWORD2
+getX			KEYWORD2
+getY			KEYWORD2
+getZ			KEYWORD2