Go Down

### Topic: 6DOF Stewart Platform (Read 3141 times)previous topic - next topic

##### May 05, 2016, 08:25 pm
Hello guys
Does anyone have the controlling COD for Stewart Platform?!

Thanks

#### TomGeorge

#1
##### May 06, 2016, 05:18 am
Hi,

You will be surprised.

Tom....
Everything runs on smoke, let the smoke out, it stops running....

#### PaulS

#2
##### May 10, 2016, 10:53 am
Hi,

You will be surprised.

Tom....
I did. No fish in sight. I still need the COD for it.

#3
##### May 12, 2016, 07:13 pm
actually he was right, u can find a number of cods online but unfortunately non of them worked properly

dear PaulS i'm trying to troubleshoot two of them
if u r interested we can work together on that
i will post them and tell u what is the problem in each

1. for this cod the platform doesnt react at all

#include <Servo.h>
/*
*******************************************************************************
feedback   - in fractions of (1/x),
higher x --> reach target location at SLOWER speed but LESS oscillation.
lower x  --> reach target location at FASTER speed but MORE oscillation.

YPR_max    - in degrees,
limits the maximum Yaw-Pitch-Roll angle of the moving platform

servo_mult - constant
converts output angle (in radians) to microsecond pulses e.g.
pi/3 radians * servo_mult = 600uS
-pi/4 radians * servo_mult = -450uS
*******************************************************************************
*/
const float pi = radians(180), theta_s[] = {  pi, pi, pi/3, pi/3, -pi/3, -pi/3},
L1 = 16, L2 = 98.3, z_home = 110.0,
servo_mult = (1800/pi), ADC_mult = (YPR_max/512),

// p = location of servo rotation points in base frame [x/y][1-6]
p[2][6] = {{-52.3993, 50.8007, 90.3895, 39.5875, -38.2649, -90.3679}  ,
{74.8623, 74.8623, 6.2712, -80.5588, -80.4696, 5.8204}},
// re = location of attachment points in end effector frame [x/y][1-6]
re[3][6] = {{-12.0000,  12.0000,  70.2820,  58.2820,  -58.2820,  -70.2820},
{74.2260,  74.2260,  -26.7210,  -47.5060,  -47.5060,  -26.7210},
{-21.075,  -21.075,  -21.075,   -21.075,   -21.075,   -21.075}};

/*
theta_r = angle between attachment points
theta_p = angle between rotation points
theta_s = orientation of the servos
RD = distance to end effector attachment points
PD = distance to servo rotation points
L1 = servo arm length
L2 = connecting arm length
z_home = default z height with servo arms horizontal
servo_min = lower limit for servo arm angle
servo_max = upper limit for servo arm angle
servo_mult = multiplier to convert to milliseconds
re = location of attachment points in end effector frame [x/y][1-6]
pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)
theta_a = angle of the servo arm
servo_pos = value written to each servo
q = position of lower mounting point of connecting link [x,y,x][1-6]
r = position of upper mounting point of connecting link
dl = difference between x,y,z coordinates of q and r
dl2 = distance between q and r

*******************************************************************************
P1 = translation in X direction (sway)          S* = Sin(theta_*)
P2 = translation in Y direction (surge)         C* = Cos(theta_*)
P3 = translation in Z direction (heave)         e.g. C1 = Cos(theta_1)

General Matrix for Yaw-Pitch-Roll-Sway-Surge-Heave Transformation
|   C2C3     S1S2C3 - C1S3    C1S2C3 + S1S3    P1  |     |  X  |    |  q[0]  |
|   C2S3     S1S2S3 + C1C3    C1S2S3 - S1C3    P2  |  x  |  Y  |  = |  q[1]  |
|   -S2          S1C2              C1C2        P3  |     |  Z  |    |  q[2]  |
|    0             0                0           1  |     |  1  |    |  1     |
where:
X = L1          theta_1 = 0
Y = 0           theta_2 = theta_a
Z = 0           theta_3 = theta_s
*******************************************************************************
*/

const int servo_pin[] = {9,3, 5, 11, 6, 10},
servo_zero[6] = {1500, 1500, 1500, 1500, 1500, 1500};
//      servo_pin = servo pin assignments,
//      servo_zero = zero angles for each servo (horizontal)

Servo servo[6];
//      Servos 0, 2, 4: reversed (+ = down, - = up)
//      Servos 1, 3, 5: normal   (+ = up,   - = down)

void setup()
{
//Serial.begin(9600);
for(int i = 0; i < 6; i++)
{
servo.attach(servo_pin);
servo.writeMicroseconds(servo_zero);
}
delay(1000);
}

void loop()
{

//----------------------------------------------------------------
//      Serial.println("SERVO");
//----------------------------------------------------------------
for(int i = 0; i < 6; i++)
{
/*    theta_1=0, Y=0, Z=0, P3=0
|   C2C3    0    0    P1  |     |  X  |    |  q[0]  |    | X(C2C3) + P1 |
|   C2S3    0    0    P2  |  x  |  0  |  = |  q[1]  | =  | X(C2S3) + P2 |
|   -S2     0    0     0  |     |  0  |    |  q[2]  |    | X(-S2)       |
|    0      0    0     1  |     |  1  |    |  1     |    |    1         |
*/
// theta_a = angle of the servo arm
// theta_s = orientation of the servos
if(i%2 == 0)
{ q[0] = L1*cos(-theta_a)*cos(theta_s) + p[0];  //EVEN [0,2,4]
q[1] = L1*cos(-theta_a)*sin(theta_s) + p[1];
q[2] = -L1*sin(-theta_a);
}
else
{ q[0] = -L1*cos(theta_a)*cos(theta_s) + p[0];  //ODD  [1,3,5]
q[1] = -L1*cos(theta_a)*sin(theta_s) + p[1];
q[2] = L1*sin(theta_a);
}

// Z = 0 to simplify equation
// r = position of upper mounting point of connecting link
// re = location of attachment points in end effector frame [x/y][1-6]
// pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)
r[0] = re[0] * cos(pe[4]) * cos(pe[5]) + re[1] * (sin(pe[3]) * sin(pe[4]) * cos(pe[5]) - cos(pe[3]) * sin(pe[5])) + re[2] * ((cos(pe[3]) * sin(pe[4]) * cos(pe[5])) + sin(pe[3]) * sin(pe[5])) + pe[0];
r[1] = re[0] * cos(pe[4]) * sin(pe[5]) + re[1] * (cos(pe[3]) * cos(pe[5]) + sin(pe[3]) * sin(pe[4]) * sin(pe[5])) + re[2] * ((cos(pe[3]) * sin(pe[4]) * cos(pe[5])) - sin(pe[3]) * cos(pe[5])) + pe[1];
r[2] = -re[0] * sin(pe[4]) + re[1] * sin(pe[3]) * cos(pe[4]) + re[2] * (cos(pe[3]) * cos(pe[4])) + z_home + pe[2];
// dl = difference between x,y,z coordinates of q and r
dl[0] = q[0] - r[0];
dl[1] = q[1] - r[1];
dl[2] = q[2] - r[2];

// dl2 = distance between q and r
// L2 = connecting arm length
dl2 = sqrt(dl[0]*dl[0] + dl[1]*dl[1] + dl[2]*dl[2]) - L2;

// feedback in fractions of (1/x),
// higher x --> reach target location at SLOWER speed but LESS oscillation.
//  lower x --> reach target location at FASTER speed but MORE oscillation.
theta_a += (dl2*feedback);

//----------------------------------------------------------------
//     Serial.println(dl2);
//----------------------------------------------------------------

// theta_a = angle of the servo arm in radians
theta_a = constrain(theta_a, servo_min, servo_max);

if(i%2 == 0) servo_pos = servo_zero + theta_a*servo_mult;    //EVEN - normal
else servo_pos = servo_zero - theta_a*servo_mult;            //ODD - reversed
//   if(i%2 == 1) servo_pos = servo_zero + theta_a*servo_mult;
//   else servo_pos = servo_zero - theta_a*servo_mult;
}

for(int i = 0; i < 6; i++)
{
servo.writeMicroseconds(servo_pos);
}
//delay(500);
}

#4
##### May 12, 2016, 07:21 pm
2. in this cod the platform react but not as it should

#include <Servo.h>
double x = 0;  // 70 i/p max
double y = 0;  // 70 i/p max
double z = 0; //31 i/p max
double psi = 0;               // yaw nothing more than .4  // z-axis
double theta = 0;           //roll nothing more than .2 // y-axis
double phi = 0;              //pitch nothing more than .2 // x-axis

double pi = 3.1415;

double M[3][3];
double rxp[3][6];
double L[3][6];
double T[3];
double H[3] = {0,0,115.79};

double Len[6];

int pulse[6];
int pp;

Servo S[6];
int pins[6] = {3,5,6,9,10,11};  //3,6,10 Reversed

void setup()
{
Serial.begin(9600);
Serial.println("test");
for (int i = 0; i < 6; i++)
{
S.attach(pins);
}

}

void loop()
{

while (Serial.available()>0)
{
phi=Serial.parseInt(); //in here im changing the variables to get the movement but the platform doesn't react properly
}
getT();

getmatrix();
getrxp();
getL();
getLen();
getPulses();
runservos();
//Serial.println("==================");
//for (int i = 0; i < 3; i++)
//{
for (int j = 0; j < 6; j++)
{
//Serial.println(map(pulse[j],1000,2000,0,180));

}
//Serial.println();
//}
//Serial.println("==================");
delay(100);
}

{
return a*pi/180;
}

void getmatrix()
{
M[0][0] = cos(psi)*cos(theta);
M[0][1] = -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi);
M[0][2] = sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta);
M[1][0] = sin(psi)*cos(theta);
M[1][1] = cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi);
M[1][2] = -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);
M[2][0] = -sin(theta);
M[2][1] = cos(theta)*sin(phi);
M[2][2] = cos(theta)*cos(phi);
}

void getrxp()
{
rxp[0][0] = M[0][0]*(27.28)+M[0][1]*(27.3)+M[0][2]*0;
rxp[1][0] = M[1][0]*(27.28)+M[1][1]*(27.3)+M[1][2]*0;
rxp[2][0] = M[2][0]*(27.28)+M[2][1]*(27.3)+M[2][2]*0;

rxp[0][1] = M[0][0]*(37.28)+M[0][1]*(9.98)+M[0][2]*0;
rxp[1][1] = M[1][0]*(37.28)+M[1][1]*(9.98)+M[1][2]*0;
rxp[2][1] = M[2][0]*(37.28)+M[2][1]*(9.98)+M[2][2]*0;

rxp[0][2] = M[0][0]*(10)+M[0][1]*(-37.27)+M[0][2]*0;
rxp[1][2] = M[1][0]*(10)+M[1][1]*(-37.27)+M[1][2]*0;
rxp[2][2] = M[2][0]*(10)+M[2][1]*(-37.27)+M[2][2]*0;

rxp[0][3] = M[0][0]*(-10)+M[0][1]*(-37.27)+M[0][2]*0;
rxp[1][3] = M[1][0]*(-10)+M[1][1]*(-37.27)+M[1][2]*0;
rxp[2][3] = M[2][0]*(-10)+M[2][1]*(-37.27)+M[2][2]*0;

rxp[0][4] = M[0][0]*(-37.28)+M[0][1]*(9.98)+M[0][2]*0;
rxp[1][4] = M[1][0]*(-37.28)+M[1][1]*(9.98)+M[1][2]*0;
rxp[2][4] = M[2][0]*(-37.28)+M[2][1]*(9.98)+M[2][2]*0;

rxp[0][5] = M[0][0]*(-27.28)+M[0][1]*(27.3)+M[0][2]*0;
rxp[1][5] = M[1][0]*(-27.28)+M[1][1]*(27.3)+M[1][2]*0;
rxp[2][5] = M[2][0]*(-27.28)+M[2][1]*(27.3)+M[2][2]*0;
}

void getT()
{
T[0] = x+H[0];
T[1] = y+H[1];
T[2] = z+H[2];
}

void getL()
{
L[0][0] = T[0]+rxp[0][0]-(51.29);
L[1][0] = T[1]+rxp[1][0]-(41.26);
L[2][0] = T[2]+rxp[2][0]-(0);

L[0][1] = T[0]+rxp[0][1]-(61.29);
L[1][1] = T[1]+rxp[1][1]-(23.84);
L[2][1] = T[2]+rxp[2][1]-(0);

L[0][2] = T[0]+rxp[0][2]-(10);
L[1][2] = T[1]+rxp[1][2]-(-65);
L[2][2] = T[2]+rxp[2][2]-(0);

L[0][3] = T[0]+rxp[0][3]-(-10);
L[1][3] = T[1]+rxp[1][3]-(-65);
L[2][3] = T[2]+rxp[2][3]-(0);

L[0][4] = T[0]+rxp[0][4]-(-61.29);
L[1][4] = T[1]+rxp[1][4]-(23.84);
L[2][4] = T[2]+rxp[2][4]-(0);

L[0][5] = T[0]+rxp[0][5]-(-51.29);
L[1][5] = T[1]+rxp[1][5]-(41.26);
L[2][5] = T[2]+rxp[2][5]-(0);

}

void getLen()
{
int initLen = 119; // must be 119
for (int i = 0; i < 6; i++)
{
Len = sqrt(L[0]*L[0]+L[1]*L[1]+L[2]*L[2])-initLen;
}
}

void getPulses()
{
for (int i = 0; i < 6; i++)
{
pulse =  map(Len,0,30,1500,2500);
}

}
void runservos()
{
for (int i = 0; i < 6; i++)
{
pp=map(pulse,1500,2500,0,90);
Serial.print(pp);
Serial.println();

if(i%2==0) {
S.write(180-pp); //MATH
//S.write(45);                  //HOME
}
else {  S.write(pp); }

}
}

#### TomGeorge

#5
##### May 12, 2016, 11:24 pm
Hi,
Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html
then look down to item #7 about how to post your code.
It will be formatted in a scrolling window that makes it easier to read.

Then come back to your posts and edit it (lower right of your post screen) add the code tags and make us happy.

Thanks.. Tom..
Everything runs on smoke, let the smoke out, it stops running....

#### ClemsonTigre

#6
##### Dec 26, 2017, 03:10 am

I did make some corrections to the second (2.) version of the code you posted so it would compile and it seems to track. To be concise, S, pp, pulse and Len need to be doubles and arrays throughout the code.
Also, I'm using a Simblee processor so all the serial functions are commented out for my build.

A bit late for your post but maybe it will help someone else.

Code: [Select]
`#include <Servo.h>//#include <SimbleeForMobile.h>double x = 0;  // 70 i/p maxdouble y = 0;  // 70 i/p maxdouble z = 0; //31 i/p maxdouble psi = 0;               // yaw nothing more than .4  // z-axisdouble theta = 0;           //roll nothing more than .2 // y-axisdouble phi = 0;              //pitch nothing more than .2 // x-axisdouble pi = 3.1415;double M[3][3];double rxp[3][6];double L[3][6];double T[3];double H[3] = {0,0,115.79};double Len[6];double pulse[6];// changed to a doubledouble pp[6];// changed to a double and an arrayServo S[6];//int pins[6] = {3,5,6,9,10,11};  //3,6,10 Reversedint pins[6] = {2, 3, 4, 5, 1, 6};  //3,5,6 Reversed for my servo pins// int servo_zero[6] = {1710, 1280, 1700, 1300, 1680, 1300};void setup(){//Serial.begin(9600);//Serial.println("test");for (int i = 0; i < 6; i++){ S[i].attach(pins[i]);}}void loop(){while (Serial.available()>0){ phi=Serial.parseInt(); //in here im changing the variables to get the movement but the platform doesn't react properly } getT();getmatrix();getrxp();getL();getLen();getPulses();     runservos();//Serial.println("==================");//for (int i = 0; i < 3; i++)//{ for (int j = 0; j < 6; j++) { //Serial.println(map(pulse[j],1000,2000,0,180));    } //Serial.println();//}//Serial.println("==================");delay(100);}double rad(double a){return a*pi/180;}void getmatrix(){M[0][0] = cos(psi)*cos(theta);M[0][1] = -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi);M[0][2] = sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta);M[1][0] = sin(psi)*cos(theta);M[1][1] = cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi);M[1][2] = -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);M[2][0] = -sin(theta);M[2][1] = cos(theta)*sin(phi);M[2][2] = cos(theta)*cos(phi);}void getrxp(){rxp[0][0] = M[0][0]*(27.28)+M[0][1]*(27.3)+M[0][2]*0;rxp[1][0] = M[1][0]*(27.28)+M[1][1]*(27.3)+M[1][2]*0;rxp[2][0] = M[2][0]*(27.28)+M[2][1]*(27.3)+M[2][2]*0;rxp[0][1] = M[0][0]*(37.28)+M[0][1]*(9.98)+M[0][2]*0;rxp[1][1] = M[1][0]*(37.28)+M[1][1]*(9.98)+M[1][2]*0;rxp[2][1] = M[2][0]*(37.28)+M[2][1]*(9.98)+M[2][2]*0;rxp[0][2] = M[0][0]*(10)+M[0][1]*(-37.27)+M[0][2]*0;rxp[1][2] = M[1][0]*(10)+M[1][1]*(-37.27)+M[1][2]*0;rxp[2][2] = M[2][0]*(10)+M[2][1]*(-37.27)+M[2][2]*0;rxp[0][3] = M[0][0]*(-10)+M[0][1]*(-37.27)+M[0][2]*0;rxp[1][3] = M[1][0]*(-10)+M[1][1]*(-37.27)+M[1][2]*0;rxp[2][3] = M[2][0]*(-10)+M[2][1]*(-37.27)+M[2][2]*0;rxp[0][4] = M[0][0]*(-37.28)+M[0][1]*(9.98)+M[0][2]*0;rxp[1][4] = M[1][0]*(-37.28)+M[1][1]*(9.98)+M[1][2]*0;rxp[2][4] = M[2][0]*(-37.28)+M[2][1]*(9.98)+M[2][2]*0;rxp[0][5] = M[0][0]*(-27.28)+M[0][1]*(27.3)+M[0][2]*0;rxp[1][5] = M[1][0]*(-27.28)+M[1][1]*(27.3)+M[1][2]*0;rxp[2][5] = M[2][0]*(-27.28)+M[2][1]*(27.3)+M[2][2]*0;}void getT(){T[0] = x+H[0];T[1] = y+H[1];T[2] = z+H[2];}void getL(){L[0][0] = T[0]+rxp[0][0]-(51.29);L[1][0] = T[1]+rxp[1][0]-(41.26);L[2][0] = T[2]+rxp[2][0]-(0);L[0][1] = T[0]+rxp[0][1]-(61.29);L[1][1] = T[1]+rxp[1][1]-(23.84);L[2][1] = T[2]+rxp[2][1]-(0);L[0][2] = T[0]+rxp[0][2]-(10);L[1][2] = T[1]+rxp[1][2]-(-65);L[2][2] = T[2]+rxp[2][2]-(0);L[0][3] = T[0]+rxp[0][3]-(-10);L[1][3] = T[1]+rxp[1][3]-(-65);L[2][3] = T[2]+rxp[2][3]-(0);L[0][4] = T[0]+rxp[0][4]-(-61.29);L[1][4] = T[1]+rxp[1][4]-(23.84);L[2][4] = T[2]+rxp[2][4]-(0);L[0][5] = T[0]+rxp[0][5]-(-51.29);L[1][5] = T[1]+rxp[1][5]-(41.26);L[2][5] = T[2]+rxp[2][5]-(0);}void getLen(){int initLen = 119; // must be 119for (int i = 0; i < 6; i++){ Len[i] = sqrt(L[0][i]*L[0][i]+L[1][i]*L[1][i]+L[2][i]*L[2][i])-initLen; //changed to array} }void getPulses(){for (int i = 0; i < 6; i++){ pulse[i] =  map(Len[i],0,30,1500,2500);// changed to array}}void runservos(){for (int i = 0; i < 6; i++){ pp[i]=map(pulse[i],1200,1700,0,90);// changed pp and pulse to arrays Serial.print(pp[i]); //changed pp to an array Serial.println(); if(i%2==0) { S[i].write(180-pp[i]); //MATH changed S and pp to an array             //S.write(45);                  //HOME } else {  S[i].write(pp[i]); }// changed S to an array             }}`

#### ClemsonTigre

#7
##### Dec 26, 2017, 11:34 pm
Here is the corrected code for Sadi_Toom's first example.
I'm using SG90S servos and Simblee processors with the Arduino IDE so there are differences in the zeroes and I've commented out the Simblee include.

Code: [Select]
`// #include <SimbleeForMobile.h>#include <Servo.h>/*   *******************************************************************************     feedback   - in fractions of (1/x),         higher x --> reach target location at SLOWER speed but LESS oscillation.         lower x  --> reach target location at FASTER speed but MORE oscillation.     YPR_max    - in degrees,         limits the maximum Yaw-Pitch-Roll angle of the moving platform     servo_mult - constant         converts output angle (in radians) to microsecond pulses e.g.         pi/3 radians * servo_mult = 600uS         -pi/4 radians * servo_mult = -450uS   ********************************************************************************/const float pi = radians(180), theta_s[6] = {  pi, pi, pi/3, pi/3, -pi/3, -pi/3},           L1 = 0.666, L2 = 6, z_home = 110.0,           YPR_max = radians(20), servo_min = radians(-60), servo_max = radians(60), feedback = (1/50),           servo_mult = (1800/pi), ADC_mult = (YPR_max/512),             // p = location of servo rotation points in base frame [x/y][1-6]             p[2][6] = {{-52.3993, 50.8007, 90.3895, 39.5875, -38.2649, -90.3679}  ,                        {74.8623, 74.8623, 6.2712, -80.5588, -80.4696, 5.8204}},             // re = location of attachment points in end effector frame [x/y][1-6]             re[3][6] = {{-12.0000,  12.0000,  70.2820,  58.2820,  -58.2820,  -70.2820},                        {74.2260,  74.2260,  -26.7210,  -47.5060,  -47.5060,  -26.7210},                        {-21.075,  -21.075,  -21.075,   -21.075,   -21.075,   -21.075}};/*     theta_r = angle between attachment points     theta_p = angle between rotation points     theta_s = orientation of the servos     RD = distance to end effector attachment points     PD = distance to servo rotation points     L1 = servo arm length     L2 = connecting arm length     z_home = default z height with servo arms horizontal     servo_min = lower limit for servo arm angle     servo_max = upper limit for servo arm angle     servo_mult = multiplier to convert to milliseconds     re = location of attachment points in end effector frame [x/y][1-6]     pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)     theta_a = angle of the servo arm     servo_pos = value written to each servo     q = position of lower mounting point of connecting link [x,y,x][1-6]     r = position of upper mounting point of connecting link     dl = difference between x,y,z coordinates of q and r     dl2 = distance between q and r     *******************************************************************************    P1 = translation in X direction (sway)          S* = Sin(theta_*)    P2 = translation in Y direction (surge)         C* = Cos(theta_*)    P3 = translation in Z direction (heave)         e.g. C1 = Cos(theta_1)        General Matrix for Yaw-Pitch-Roll-Sway-Surge-Heave Transformation    |   C2C3     S1S2C3 - C1S3    C1S2C3 + S1S3    P1  |     |  X  |    |  q[0]  |    |   C2S3     S1S2S3 + C1C3    C1S2S3 - S1C3    P2  |  x  |  Y  |  = |  q[1]  |    |   -S2          S1C2              C1C2        P3  |     |  Z  |    |  q[2]  |    |    0             0                0           1  |     |  1  |    |  1     |    where:    X = L1          theta_1 = 0    Y = 0           theta_2 = theta_a    Z = 0           theta_3 = theta_s    ********************************************************************************/const int servo_pin[6] = {2, 3, 4, 5, 1, 6};  //3,6,10 Reversedconst int servo_zero[6] = {1710, 1280, 1700, 1300, 1680, 1300};   //const int servo_pin[] = {9,3, 5, 11, 6, 10},//servo_zero[6] = {1500, 1500, 1500, 1500, 1500, 1500};//      servo_pin = servo pin assignments,//      servo_zero = zero angles for each servo (horizontal)Servo servo[6];//      Servos 0, 2, 4: reversed (+ = down, - = up)//      Servos 1, 3, 5: normal   (+ = up,   - = down)void setup(){ //Serial.begin(9600); for(int i = 0; i < 6; i++) {   servo[i].attach(servo_pin[i]);   servo[i].writeMicroseconds(servo_zero[i]); } delay(1000);}void loop(){  float pe[6] = {0,0,0,radians(0),radians(0),radians(0)  }, theta_a[6], servo_pos[6],q[3][6], r[3][6], dl[3][6], dl2[6]; pe[0] = (analogRead(0)-512)/51.2; pe[1] = (analogRead(1)-512)/51.2; pe[2] = (analogRead(2)-512)/51.2; pe[3] = (analogRead(3)-512)*ADC_mult; pe[4] = (analogRead(4)-512)*ADC_mult; pe[5] = (analogRead(5)-512)*ADC_mult;   //----------------------------------------------------------------   //      Serial.println("SERVO");   //---------------------------------------------------------------- for(int i = 0; i < 6; i++) {/*    theta_1=0, Y=0, Z=0, P3=0     |   C2C3    0    0    P1  |     |  X  |    |  q[0]  |    | X(C2C3) + P1 |     |   C2S3    0    0    P2  |  x  |  0  |  = |  q[1]  | =  | X(C2S3) + P2 |     |   -S2     0    0     0  |     |  0  |    |  q[2]  |    | X(-S2)       |     |    0      0    0     1  |     |  1  |    |  1     |    |    1         |*/     // theta_a = angle of the servo arm     // theta_s = orientation of the servos           if(i%2 == 0)             { q[0][i] = L1*cos(-theta_a[i])*cos(theta_s[i]) + p[0][i];  //EVEN [0,2,4]               q[1][i] = L1*cos(-theta_a[i])*sin(theta_s[i]) + p[1][i];               q[2][i] = -L1*sin(-theta_a[i]);             }           else             { q[0][i] = -L1*cos(theta_a[i])*cos(theta_s[i]) + p[0][i];  //ODD  [1,3,5]               q[1][i] = -L1*cos(theta_a[i])*sin(theta_s[i]) + p[1][i];               q[2][i] = L1*sin(theta_a[i]);             }   // Z = 0 to simplify equation   // r = position of upper mounting point of connecting link   // re = location of attachment points in end effector frame [x/y][1-6]   // pe = location and orientation of end effector frame relative to the base frame [sway, surge, heave, pitch, roll, yaw)           r[0][i] = re[0][i] * cos(pe[4]) * cos(pe[5]) + re[1][i] * (sin(pe[3]) * sin(pe[4]) * cos(pe[5]) - cos(pe[3]) * sin(pe[5])) + re[2][i] * ((cos(pe[3]) * sin(pe[4]) * cos(pe[5])) + sin(pe[3]) * sin(pe[5])) + pe[0];           r[1][i] = re[0][i] * cos(pe[4]) * sin(pe[5]) + re[1][i] * (cos(pe[3]) * cos(pe[5]) + sin(pe[3]) * sin(pe[4]) * sin(pe[5])) + re[2][i] * ((cos(pe[3]) * sin(pe[4]) * cos(pe[5])) - sin(pe[3]) * cos(pe[5])) + pe[1];           r[2][i] = -re[0][i] * sin(pe[4]) + re[1][i] * sin(pe[3]) * cos(pe[4]) + re[2][i] * (cos(pe[3]) * cos(pe[4])) + z_home + pe[2];   // dl = difference between x,y,z coordinates of q and r           dl[0][i] = q[0][i] - r[0][i];           dl[1][i] = q[1][i] - r[1][i];           dl[2][i] = q[2][i] - r[2][i];   // dl2 = distance between q and r   // L2 = connecting arm length           dl2[i] = sqrt(dl[0][i]*dl[0][i] + dl[1][i]*dl[1][i] + dl[2][i]*dl[2][i]) - L2;   // feedback in fractions of (1/x),   // higher x --> reach target location at SLOWER speed but LESS oscillation.   //  lower x --> reach target location at FASTER speed but MORE oscillation.           theta_a[i] += (dl2[i]*feedback);   //----------------------------------------------------------------    //     Serial.println(dl2);   //----------------------------------------------------------------       // theta_a = angle of the servo arm in radians    theta_a[i] = constrain(theta_a[i], servo_min, servo_max);              if(i%2 == 0) servo_pos[i] = servo_zero[i] + theta_a[i]*servo_mult;    //EVEN - normal   else servo_pos[i] = servo_zero[i] - theta_a[i]*servo_mult;            //ODD - reversed   //   if(i%2 == 1) servo_pos = servo_zero + theta_a*servo_mult;   //   else servo_pos = servo_zero - theta_a*servo_mult; }    for(int i = 0; i < 6; i++) {   servo[i].writeMicroseconds(servo_pos[i]); } //delay(500);}`

#8
##### Feb 03, 2018, 04:29 pm
Hello Friends,
In my over enthusiasm I have embarked upon to make a stewart platform. The hardware is all done; picked up some sketch from Instructables. I need some help , please, to get started with the codes. At present I simply want this platform to:
1. Acquire position in heave, surge, sway, roll, pitch and Yaw i.e. input the variables for the mentioned platform positions.
I shall be greatly obliged if someone can get me started.

Sincerely,

#9
##### Mar 03, 2018, 11:10 am
Dear ClemsonTigre / friends,

Would you please lend me help with the code for my stewart platform?
I picked up the same from github / Thomas KNR.

Sincerely,

#10
##### Apr 07, 2018, 11:58 am
ClemsonTigre !

Friend I mistakenly overlooked the code you had put up. Thanks. i have gone thru it but need some clarifications please. Could you kindly brief me over the following routines;

{
phi=Serial.parseInt(); //in here im changing the variables to get the movement but the platform doesn't react properly
}

{
for (int j = 0; j < 6; j++)
{
//Serial.println(map(pulse[j],1000,2000,0,180));

}

{
for (int i = 0; i < 6; i++)
{
pulse =  map(Len,0,30,1500,2500);// changed to array
}

}
void runservos()
{
for (int i = 0; i < 6; i++)
{
pp=map(pulse,1200,1700,0,90);// changed pp and pulse to arrays
Serial.print(pp); //changed pp to an array
Serial.println();

if(i%2==0) {
S.write(180-pp); //MATH changed S and pp to an array
//S.write(45);                  //HOME
}
else {  S.write(pp); }// changed S to an array

}
}

I am new to C / Arduino programming.

All help shall be appreciated.

Sincerely,

#11
##### Apr 07, 2018, 12:18 pm
ClemsonTigre !

Friend I mistakenly overlooked the code you had put up. Thanks. i have gone thru it but need some clarifications please. Could you kindly brief me over the following routines;

{
phi=Serial.parseInt(); //in here im changing the variables to get the movement but the platform doesn't react properly
}

{
for (int j = 0; j < 6; j++)
{
//Serial.println(map(pulse[j],1000,2000,0,180));

}

{
for (int i = 0; i < 6; i++)
{
pulse =  map(Len,0,30,1500,2500);// changed to array
}

}
void runservos()
{
for (int i = 0; i < 6; i++)
{
pp=map(pulse,1200,1700,0,90);// changed pp and pulse to arrays
Serial.print(pp); //changed pp to an array
Serial.println();

if(i%2==0) {
S.write(180-pp); //MATH changed S and pp to an array
//S.write(45);                  //HOME
}
else {  S.write(pp); }// changed S to an array

}
}

I am new to C / Arduino programming.

All help shall be appreciated.

Sincerely,

#### TomGeorge

#12
##### Apr 08, 2018, 03:27 am
Hi,
Welcome to the forum.

Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html then look down to item #7 about how to post your code.
It will be formatted in a scrolling window that makes it easier to read.

This is an old thread that has been resurrected a few times.
You may be better of to start a new thread of your own, as it seems the original posters are not answering.

Tom...
Everything runs on smoke, let the smoke out, it stops running....

Go Up