Skip to content

Commit 08951be

Browse files
author
RorschachUK
committed
Initial commit
0 parents  commit 08951be

File tree

9 files changed

+489
-0
lines changed

9 files changed

+489
-0
lines changed

‎examples/IKTest/IKTest.ino

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
/* meArm IK test - York Hackspace May 2014
2+
* Test applying Nick Moriarty's Inverse Kinematics solver
3+
* to Phenoptix' meArm robot arm, to walk a specified path.
4+
*
5+
* Pins:
6+
* Arduino Base Shoulder Elbow Gripper
7+
* GND Brown Brown Brown Brown
8+
* 5V Red Red Red Red
9+
* 11 Yellow
10+
* 10 Yellow
11+
* 9 Yellow
12+
* 6 Yellow
13+
*/
14+
#include "meArm.h"
15+
#include <Servo.h>
16+
17+
int basePin = 11;
18+
int shoulderPin = 10;
19+
int elbowPin = 9;
20+
int gripperPin = 6;
21+
22+
meArm arm;
23+
24+
void setup() {
25+
arm.begin(basePin, shoulderPin, elbowPin, gripperPin);
26+
}
27+
28+
void loop() {
29+
//Clap - so it's obvious we're at this part of the routine
30+
arm.openGripper();
31+
arm.closeGripper();
32+
arm.openGripper();
33+
arm.closeGripper();
34+
arm.openGripper();
35+
delay(500);
36+
//Go up and left to grab something
37+
arm.gotoPoint(-80,100,140);
38+
arm.closeGripper();
39+
//Go down, forward and right to drop it
40+
arm.gotoPoint(70,200,10);
41+
arm.openGripper();
42+
//Back to start position
43+
arm.gotoPoint(0,100,50);
44+
delay(2000);
45+
}

‎examples/JoysticksIK/JoysticksIK.ino

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
/* meArm IK joysticks - York Hackspace May 2014
2+
* Using inverse kinematics with joysticks
3+
* Uses two analogue joystcks (two pots each)
4+
* First stick moves gripper forwards, backwards, left and right
5+
* Second stick moves gripper up, down, and closes and opens.
6+
*
7+
* I used Sparkfun thumbstick breakout boards, oriented 'upside down'.
8+
*
9+
* Pins:
10+
* Arduino Stick1 Stick2 Base Shoulder Elbow Gripper
11+
* GND GND GND Brown Brown Brown Brown
12+
* 5V VCC VCC Red Red Red Red
13+
* A0 HOR
14+
* A1 VER
15+
* A2 HOR
16+
* A3 VER
17+
* 11 Yellow
18+
* 10 Yellow
19+
* 9 Yellow
20+
* 6 Yellow
21+
*/
22+
#include "meArm.h"
23+
#include <Servo.h>
24+
25+
int basePin = 11;
26+
int shoulderPin = 10;
27+
int elbowPin = 9;
28+
int gripperPin = 6;
29+
30+
int xdirPin = 0;
31+
int ydirPin = 1;
32+
int zdirPin = 3;
33+
int gdirPin = 2;
34+
35+
meArm arm;
36+
37+
void setup() {
38+
arm.begin(basePin, shoulderPin, elbowPin, gripperPin);
39+
}
40+
41+
void loop() {
42+
float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
43+
float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0);
44+
float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
45+
float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);
46+
if (abs(dx) < 1.5) dx = 0;
47+
if (abs(dy) < 1.5) dy = 0;
48+
if (abs(dz) < 1.5) dz = 0;
49+
50+
if (!(dx == 0 && dy == 0 && dz == 0))
51+
arm.goDirectlyTo(arm.getX() + dx, arm.getY() + dy, arm.getZ() + dz);
52+
53+
if (dg < -3.0)
54+
arm.closeGripper();
55+
else if (dg > 3.0)
56+
arm.openGripper();
57+
delay(50);
58+
}

‎fk.cpp

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
/* Forward kinetics, Nick Moriarty May 2014
2+
This code is provided under the terms of the MIT license.
3+
4+
The MIT License (MIT)
5+
6+
Copyright (c) 2014 Nick Moriarty
7+
8+
Permission is hereby granted, free of charge, to any person obtaining a copy
9+
of this software and associated documentation files (the "Software"), to deal
10+
in the Software without restriction, including without limitation the rights
11+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12+
copies of the Software, and to permit persons to whom the Software is
13+
furnished to do so, subject to the following conditions:
14+
15+
The above copyright notice and this permission notice shall be included in
16+
all copies or substantial portions of the Software.
17+
18+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
SOFTWARE.
25+
*/
26+
#include "math.h"
27+
#include "fk.h"
28+
#include "ik.h"
29+
30+
void polar2cart(float r, float theta, float& a, float& b)
31+
{
32+
a = r * cos(theta);
33+
b = r * sin(theta);
34+
}
35+
36+
void unsolve(float a0, float a1, float a2, float& x, float& y, float& z)
37+
{
38+
// Calculate u,v coords for arm
39+
float u01, v01, u12, v12;
40+
polar2cart(L1, a1, u01, v01);
41+
polar2cart(L2, a2, u12, v12);
42+
43+
// Add vectors
44+
float u, v;
45+
u = u01 + u12 + L3;
46+
v = v01 + v12;
47+
48+
// Calculate in 3D space - note x/y reversal!
49+
polar2cart(u, a0, y, x);
50+
z = v;
51+
}
52+
53+
float distance(float x1, float y1, float z1, float x2, float y2, float z2)
54+
{
55+
float dx = x2-x1;
56+
float dy = y2-y1;
57+
float dz = z2-z1;
58+
59+
return dx*dx + dy*dy + dz*dz;
60+
}

‎fk.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
#ifndef FK_H_INCLUDED
2+
#define FK_H_INCLUDED
3+
4+
void polar2cart(float r, float theta, float& a, float& b);
5+
6+
void unsolve(float a0, float a1, float a2, float& x, float& y, float& z);
7+
8+
float distance(float x1, float y1, float z1, float x2, float y2, float z2);
9+
10+
#endif // FK_H_INCLUDED

‎ik.cpp

Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
/* Inverse kinetics, Nick Moriarty May 2014
2+
This code is provided under the terms of the MIT license.
3+
4+
The MIT License (MIT)
5+
6+
Copyright (c) 2014 Nick Moriarty
7+
8+
Permission is hereby granted, free of charge, to any person obtaining a copy
9+
of this software and associated documentation files (the "Software"), to deal
10+
in the Software without restriction, including without limitation the rights
11+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12+
copies of the Software, and to permit persons to whom the Software is
13+
furnished to do so, subject to the following conditions:
14+
15+
The above copyright notice and this permission notice shall be included in
16+
all copies or substantial portions of the Software.
17+
18+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24+
SOFTWARE.
25+
*/
26+
#include "math.h"
27+
#include "ik.h"
28+
29+
const float PI=3.14159265359;
30+
float L1=80; //Shoulder to elbow length
31+
float L2=80; //Elbow to wrise length
32+
float L3=68; //Length from wrist to hand PLUS base centre to shoulder
33+
34+
// Get polar coords from cartesian ones
35+
void cart2polar(float a, float b, float& r, float& theta)
36+
{
37+
// Determine magnitude of cartesian coords
38+
r = sqrt(a*a + b*b);
39+
40+
// Don't try to calculate zero-magnitude vectors' angles
41+
if(r == 0) return;
42+
43+
float c = a / r;
44+
float s = b / r;
45+
46+
// Safety!
47+
if(s > 1) s = 1;
48+
if(c > 1) c = 1;
49+
if(s < -1) s = -1;
50+
if(c < -1) c = -1;
51+
52+
// Calculate angle in 0..PI
53+
theta = acos(c);
54+
55+
// Convert to full range
56+
if(s < 0) theta *= -1;
57+
}
58+
59+
// Get angle from a triangle using cosine rule
60+
bool cosangle(float opp, float adj1, float adj2, float& theta)
61+
{
62+
// Cosine rule:
63+
// C^2 = A^2 + B^2 - 2*A*B*cos(angle_AB)
64+
// cos(angle_AB) = (A^2 + B^2 - C^2)/(2*A*B)
65+
// C is opposite
66+
// A, B are adjacent
67+
float den = 2*adj1*adj2;
68+
69+
if(den==0) return false;
70+
float c = (adj1*adj1 + adj2*adj2 - opp*opp)/den;
71+
72+
if(c>1 || c<-1) return false;
73+
74+
theta = acos(c);
75+
76+
return true;
77+
}
78+
79+
// Solve angles!
80+
bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
81+
{
82+
// Solve top-down view
83+
float r, th0;
84+
cart2polar(y, x, r, th0);
85+
86+
// Account for the wrist length!
87+
r -= L3;
88+
89+
// In arm plane, convert to polar
90+
float ang_P, R;
91+
cart2polar(r, z, R, ang_P);
92+
93+
// Solve arm inner angles as required
94+
float B, C;
95+
if(!cosangle(L2, L1, R, B)) return false;
96+
if(!cosangle(R, L1, L2, C)) return false;
97+
98+
// Solve for servo angles from horizontal
99+
a0 = th0;
100+
a1 = ang_P + B;
101+
a2 = C + a1 - PI;
102+
103+
return true;
104+
}

‎ik.h

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
/* meArmIK - York Hackspace May 2014
2+
* Inverse Kinematics solver for three degrees of freedom
3+
* created for Phenoptix' meArm robot arm
4+
*/
5+
#ifndef IK_H_INCLUDED
6+
#define IK_H_INCLUDED
7+
8+
extern float L1, L2, L3;
9+
10+
// Get polar coords from cartesian ones
11+
void cart2polar(float a, float b, float& r, float& theta);
12+
13+
// Get angle from a triangle using cosine rule
14+
bool cosangle(float opp, float adj1, float adj2, float& theta);
15+
16+
// Solve angles!
17+
bool solve(float x, float y, float z, float& a0, float& a1, float& a2);
18+
19+
#endif // IK_H_INCLUDED

‎keywords.txt

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#######################################
2+
# Syntax Coloring Map For meArm library
3+
#######################################
4+
5+
#######################################
6+
# Datatypes (KEYWORD1)
7+
#######################################
8+
9+
meArm KEYWORD1
10+
11+
#######################################
12+
# Methods and Functions (KEYWORD2)
13+
#######################################
14+
15+
begin KEYWORD2
16+
openGripper KEYWORD2
17+
closeGripper KEYWORD2
18+
gotoPoint KEYWORD2
19+
goDirectlyTo KEYWORD2
20+
getX KEYWORD2
21+
getY KEYWORD2
22+
getZ KEYWORD2

0 commit comments

Comments
 (0)