# Inverse kinematics problems

Hi,
i’m using Arduino by few days ad I don’t know it very well. I’m trying to control three servo motor to move a robotic arm. Some time ago i’m wrritten inverse kinematic algorithm with Matlab to simulate the moving of a robotic arm. Now i’m made a robotic arm and would to move it with Arduino. I’m trying to write an inverse kinematic algorithm using the Arduino language, but i’m encountered some problems and becouse of my inexperience i don’t succeeded in resolve it only using the information founded by google. I report in this message the code that i have written.

``````#include<Servo.h>

Servo M1;
Servo M2;
Servo M3;
int xe[3];
int xd [] = {1,1,1};
int J[2][2];
int q[2];
int dq[2];
int e[2];
int K[]={100,100,100};
int DH[2][3]={{0,90,0,0},
{12,0,0,0},
{6,0,0,0}};
int dt=15;

void setup () {
Serial.begin(9600);
M1.attach(9,500,2500);
M2.attach(10,500,2500);
M3.attach(11,500,2500);
}

void loop () {

T=cindir(DH);
xe={T[0][3],T[1][3],T[2][3]};
J=jacobian(DH);
e=xd-xe;
JT=transpose(J,2,2);
dq=JT*K*e;
q=q1+dq*dt*0.001;

M1.write(q[0]);
M2.write(q[1]);
M3.write(q[2]);

DH[0][3]=q[0];
DH[1][3]=q[1];
DH[2][3]=q[2];

delay(dt)
}

int cindir(DH) {
int T[3][3];
int t1=DH[0][3]*pi/180;
int t2=DH[1][3]*pi/180;
int t3=DH[2][3]*pi/180;
int a2=DH[1][1];
int a3=DH[1][2];

T={ {cos(t1)*cos(t2+t3), -cos(t1)*sin(t2+t3), sin(t1), cos(t1)*(a2*cos(t2)+a3*cos(t2+t3))},
{sin(t1)*cos(t1+t3), -sin(t1)*sin(t2+t3), -cos(t1), sin(t1)*(a2*cos(t2)+a3*cos(t2+t3))},
{sin(t2+t3) , cos(t2+t3), 0 , a2*sin(t2)+a3*sin(t2+t3)},
{0, 0, 0, 1}};

return T;
}

int jacobian(DH) {

int J[2][2];
int t1=DH[0][3]*pi/180;
int t2=DH[1][3]*pi/180;
int t3=DH[2][3]*pi/180;
int a2=DH[1][1];
int a3=DH[1][2];

J = { {-sin(t1)*(a2*cos(t2)+a3*cos(t2+t3)), -cos(t1)*(a2*sin(t2)+a3*sin(t2+t3)), -a3*cos(t1)*sin(t2+t3)},
{cos(t1)*(a2*cos(t2)+a3*cos(t2+t3)), -sin(t1)*(a2*sin(t2)+a3*sin(t2+t3)), -a3*sin(t1)*sin(t2+t3)},
{0, a2*cos(t2)+a3*cos(t2+t3), a3*cos(t2+t3)}};

return J;

}

int transpose (A,dimR,dimC) {

int AT[dimR][dimC];

for(i=0;i<R;i=i+1) {

for(j=0;j<C;j=j+1) {

AT[j]=A[j];

}
}

return AT;

}
``````

When the code is compiled i obtain these errors:

``````sketch_mar07a:7: error: 'DH' was not declared in this scope
sketch_mar07a:8: error: 'DH' was not declared in this scope
sketch_mar07a:9: error: 'A' was not declared in this scope
sketch_mar07a:9: error: 'dimR' was not declared in this scope
sketch_mar07a:9: error: 'dimC' was not declared in this scope
sketch_mar07a:9: error: initializer expression list treated as compound expression
sketch_mar07a:16: error: too many initializers for 'int [3]'
sketch_mar07a:16: error: too many initializers for 'int [3]'
sketch_mar07a:16: error: too many initializers for 'int [2][3]'
sketch_mar07a.ino: In function 'void loop()':
sketch_mar07a:28: error: 'T' was not declared in this scope
sketch_mar07a:28: error: 'cindir' cannot be used as a function
sketch_mar07a:29: error: expected primary-expression before '{' token
sketch_mar07a:29: error: expected `;' before '{' token
sketch_mar07a:30: error: 'jacobian' cannot be used as a function
sketch_mar07a:31: error: incompatible types in assignment of 'int' to 'int [2]'
sketch_mar07a:32: error: 'JT' was not declared in this scope
sketch_mar07a:32: error: 'transpose' cannot be used as a function
sketch_mar07a:34: error: argument of type 'int (Servo:()' does not match 'int'
sketch_mar07a:34: error: argument of type 'int (Servo:()' does not match 'int'
sketch_mar07a:34: error: argument of type 'int (Servo:()' does not match 'int'
sketch_mar07a:35: error: invalid operands of types 'int [2]' and 'int' to binary 'operator*'
sketch_mar07a:46: error: expected `;' before '}' token
sketch_mar07a.ino: At global scope:
sketch_mar07a:48: error: redefinition of 'int cindir'
sketch_mar07a:7: error: 'int cindir' previously defined here
``````

I’m starting now to use Arduino and so i’m very naive about the “Arduino’s World”.
Could you give me some infomation about where can i find some materials that could help me?
Thaks

If you made a mistake in the first one, edit it. Don't post a brand new thread.

``````int DH[2][3]={{0,90,0,0},
{12,0,0,0},
{6,0,0,0}};
``````

Has your ability to count failed you? Three rows with four values each will not fit in a 2 by 3 array, as the compiler is trying to tell you. Since this declaration is rejected, all references to DH will fail.

The value in the brackets is the number of elements, not the upper index or the temperature in Detroit on Christmas.

``````int cindir(DH) {
...
``````

You have not specified a type for the formal argument DH. Similarly for all your other function argument declarations: you need to specify the type and the name for each argument.