Friends!
I have built a small stewart platform / Instructables , and presently facing difficulty with the coding. I sifted thru ARDUINO forums and found a couple but on account of my little programming knowledge, still stuck up. No progress.
I would welcome if any one could help me edit / correct the attached code by Clemson Tigre or post a working Code for my consumption.
Thanks.
CODE - Stewart Platform.txt (7.69 KB)
and presently facing difficulty with the coding.
But, you have no intention of telling us what the problem is.
Well, then, I have no intention of telling you what the solution is.
Hi jawadmalik,
Your post count indicates that you are newish to the forum.
Read through the sticky post at the top of this and every forum named 'How to use this forum - please read'
It covers a lot of helpful info like using code tags.
When you are asking for help with your code, it is helpful to describe your problems. A good format for that description is:
Edit: A very frequent request for projects using motors or actuators is a wiring diagram showing how things are powered and how they are connected to the arduino. Pencil, paper and a camera are good enough if you include proper detail (like pin numbers). Fritzing may look pretty but is often less readable than pencil and paper drawings.
And I want pics of your project. I like to see mechanical workings. What are you using for actuators? Your attached code looks like it is talking to servos.
const int servo_pin[6] = {2, 3, 4, 5, 1, 6}; //3,6,10 Reversed
Serial.begin(9600);// opens serial port, sets data rate to 9600
Are you using pin 1 for Serial or for the Servo? Both is the wrong answer. So is yes.
void setup()
{
// put your setup code here, to run once:
#include <Servo.h>
#include <stdio.h>
#include <SPI.h>
#include <Wire.h>
#include <String.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)
// */ is a multiline comment
Servo servo[6];
// Servos 0, 2, 4: reversed (+ = down, - = up)
// Servos 1, 3, 5: normal (+ = up, - = down)
void setup()
{
servo[0].attach(3, MIN, MAX);
servo[1].attach(5, MIN, MAX);
servo[2].attach(6, MIN, MAX);
servo[3].attach(9, MIN, MAX);
servo[4].attach(10, MIN, MAX);
servo[5].attach(11, MIN, MAX);
}
Serial.begin(9600);
// opens serial port, sets data rate to 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; // 1023/103.2 for Arduino Mega//
pe[1] = (analogRead(1)-512)/51.2;
pe[2] = (analogRead(2)-512)/51.2;
pe[3] = (analogRead(3)-512)*ADC_mult;//(analogRead(3)-1023)*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] = L1cos(-theta_a[i])cos(theta_s[i]) + p[0][i]; //EVEN [0,2,4]
q[1][i] = L1cos(-theta_a[i])sin(theta_s[i]) + p[1][i];
q[2][i] = -L1sin(-theta_a[i]);
}
else
{
q[0][i] = -L1cos(theta_a[i])cos(theta_s[i]) + p[0][i]; //ODD [1,3,5]
q[1][i] = -L1cos(theta_a[i])sin(theta_s[i]) + p[1][i];
q[2][i] = L1sin(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)
// Calculation of Rotational Matrix from basic equation //
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_aservo_mult;
// else servo_pos = servo_zero - theta_aservo_mult;
}
for(int i = 0; i < 6; i++)
{
servo[i].writeMicroseconds(servo_pos[i]);
}
delay(500);
}
_```
*_
Hello Vinceherman !
Forgive me i am still not that well aware about posting stuff here. Need some time, please.
I have posted the code and the picture of the mechanical design. I am using rc servos and to support the movements, Arduino Mega.
As I "Verfiy" the code in the Arduino IDE, it says error associated with the curly braces. This happens at the beginning of the " Void Loop ()". I understand the significance of the latter but have not been able to resolve it.
I attribute my failure to achieve results because of my lack of requisite programming skills. I do understand a fair deal about the coding but still cannot impart fineness to it.
As I see things, someone with patience can upload this code to their Arduino Board and then could appreciate the problem better.
I would appreciate this help. Once the values of "writeMicroseconds" for individual servos with change in values of pe[6] then it would be indicative that the code is working.
If at all there is help around, then this is the place.
Sincerely,
Jawadmalik
Impressive setup!
Now take it apart. While you are still in discovery mode about how to upload code and testing connections to servos, do not impose the added complexity of possibly causing a servo to bind and strip gears or overheat.
Do your early testing with the servo arms free to move independently from all the other servos.
I see chunks of code in reply #4 but I cannot tell what is what. It looks like you have a sketch and a half posted outside of the code tags, and possible another half of a sketch posted inside of code tags. For help with hunting down mismatch curly brackets, it is important to start with a known copy of the entire sketch.
Post it again, inside of code tags and we can go from there.
Also, one tip for hunting down problems with curly brackets is to use the auto-format tool. CTRL-T in the editor. It lines up matching sets making it easier to see where you might have accidentally put in too many begin or end brackets.
I went up to your original post and downloaded the text file, assuming that this is a good copy of your sketch. If not, please correct me.
I see this:
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];
//snip
Serial.println(dl2);
If d12 is an array of floats, I think you need to iterate through all of the array elements to print them. Something like this untested code
for(int i = 0; i < 6; i++)
{
Serial.println(d12[i]);
}
#include <Servo.h>
#include <stdio.h>
#include <SPI.h>
#include <Wire.h>
#include <String.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)
// */ is a multiline comment
Servo servo[6];
// Servos 0, 2, 4: reversed (+ = down, - = up)
// Servos 1, 3, 5: normal (+ = up, - = down)
void setup()
{
Serial.begin(9600);// opens serial port, sets data rate to 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; // 1023/103.2 for Arduino Mega//
pe[1] = (analogRead(1)-512)/51.2;
pe[2] = (analogRead(2)-512)/51.2;
pe[3] = (analogRead(3)-512)*ADC_mult;//(analogRead(3)-1023)*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)
// Calculation of Rotational Matrix from basic equation //
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;
for(int i = 0; i < 6; i++)
{
Serial.println(d12[i]);
}
// 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);
}
Hi Vinceherman,
I have put up the code once again. Thanks for the compliment; I shall dismantle the platform and check the code with free rotating servos.
I could not thank you enough for lending me help.
Sincerely,
Jawad
I have another website page. Maybe can help you with :
1.[u]https://01.org/developerjourney/recipe/building-stewart-platform[/u]
2.[u]Redirect Notice
nielyay!
Friend I visited both the websites; the first is based on a standalone processor from INTEL and then connects to the six servos via her I2C. I am having an Arduino Mega and my design is based on the second site you have so kindly pointed out.
My issue is with the curly braces within the code. The code is appended above. You could have a go at it if you desire so. The Maths is very clear to me but as I am not good at programming skills, thus stuck up.
Nevertheless, this forum has some good friends and knowledgeable engineers so I am banking on someone will correct the code for me.
Sincerely,
Jawad