Go Down

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

Sadi_Toom

Hello guys
Does anyone have the controlling COD for Stewart Platform?!

Thanks

TomGeorge

Hi,

google      arduino stewart platform

You will be surprised.

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

PaulS

Hi,

google      arduino stewart platform

You will be surprised.

Tom.... :)
I did. No fish in sight. I still need the COD for it.
The art of getting good answers lies in asking good questions.

Sadi_Toom

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,
           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[] = {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()
{
  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] = 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);
}





Sadi_Toom

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);
}

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 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

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

Hi Sadi_Toom,

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 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];

double pulse[6];// changed to a double
double pp[6];// changed to a double and an array

Servo S[6];
//int pins[6] = {3,5,6,9,10,11};  //3,6,10 Reversed
int 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 119
for (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

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 Reversed
const 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);
}

jawadmalik

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,

jawadmalik

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,

jawadmalik

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,


jawadmalik

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

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