Go Down

Topic: Parallel Robot/Stewart Platform Code not working. (Read 2466 times) previous topic - next topic

tesan1

Hello all,

I have recently built a Stewart Platform with 6 mini-linear actuators as the arms. They have a max travel of 30mm. I have written some code to control home position translation,
the three rotations (pitch, yaw, roll)
and z and y translations.
However, it seems that my robot cannot stay level when I attempt to move in the x-direction (left and right when viewed from the front)

I have posted the code. Maybe the numbering is wrong on my actuators? That is the only solution I can think of right now. The math is also included in excel format.

Thanks!

Code: [Select]

#include <Servo.h>
double x = 0;
double y = 0;
double z = 0;
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];

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

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

}

void loop()
{
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;
}
}

void getPulses()
{
for (int i = 0; i < 6; i++)
{
pulse[i] =  map(Len[i],0,30,1000,2000);
}

}
void runservos()
{
for (int i = 0; i < 6; i++)
{
S[i].write(map(pulse[i],1000,2000,45,180)); //MATH
                //S[i].write(45);                  //HOME
}
}

tesan1

Here is the excel table which is all the math.

      (mm)      (DEG)   (RAD)            
   x   0   psi   0   0            
   y   0   theta   0   0            
   z   0   phi   0   0            
                           
                           
      Home      T               
   x   0      0               
   y   0      0               
   z   115.79      115.79               
                           
   p1   p2   p3   p4   p5   p6         
x   27.28   37.28   10   -10   -37.28   -27.28         
y   27.3   9.98   -37.27   -37.27   9.98   27.3         
z   0   0   0   0   0   0         
                           
   b1   b2   b3   b4   b5   b6         
x   51.29   61.29   10   -10   -61.29   -51.29         
y   41.26   23.84   -65   -65   23.84   41.26         
z   0   0   0   0   0   0         
                           
                           
   R Transform Matrix                        
   1   0   0                  
   0   1   0                  
   0   0   1                  
                           
   Rxp                        
   27.28   37.28   10   -10   -37.28   -27.28         
   27.3   9.98   -37.27   -37.27   9.98   27.3         
   0   0   0   0   0   0         
                           
   L=T+Rxp-b                        
   L1   L2   L3   L4   L5   L6         
x   -24.01   -24.01   0   0   24.01   24.01         
y   -13.96   -13.86   27.73   27.73   -13.86   -13.96         
z   115.79   115.79   115.79   115.79   115.79   115.79         
                           
length   119.07   119.06   119.06   119.06   119.06   119.07         
                           
servo out   15.0742869   15.06260454   15.06417177   15.06417177   15.06260454   15.0742869         -0.011682356

Go Up