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!
#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
}
}