Hello guys
Does anyone have the controlling COD for Stewart Platform?!
Thanks
Hi,
google arduino stewart platform
You will be surprised.
Tom.... 
TomGeorge:
Hi,google arduino stewart platform
You will be surprised.
Tom....
I did. No fish in sight. I still need the COD for it.
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
- 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] = L1cos(-theta_a)cos(theta_s_) + p[0]; //EVEN [0,2,4]
q[1] = L1*cos(-theta_a)*sin(theta_s) + p[1];
q[2] = -L1sin(-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] = L1sin(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 += (dl2feedback);
//----------------------------------------------------------------
// 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_aservo_mult;
// else servo_pos = servo_zero - theta_aservo_mult;
_ }_
* for(int i = 0; i < 6; i++)*
* {*
servo.writeMicroseconds(servo_pos*);
_ }
//delay(500);
}*_
- 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); }*_
}
}
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..
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.
#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
}
}
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.
// #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);
}
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:
- 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,
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,
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,
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,
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...