Go Down

Topic: stewart platform - CODE (Read 1 time) previous topic - next topic

jawadmalik

Sep 01, 2018, 09:38 am Last Edit: Sep 01, 2018, 09:41 am by jawadmalik
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.

PaulS

Quote
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.
The art of getting good answers lies in asking good questions.

vinceherman

#2
Sep 04, 2018, 03:57 pm Last Edit: Sep 04, 2018, 04:02 pm by vinceherman
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:
1. Post the entire sketch, after using the auto-format feature in the IDE and using code tags.
2. Describe how you expect the sketch to behave.
2. Describe how it actually behaves and how that differs from your expectations.

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.

PaulS

Code: [Select]
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.
The art of getting good answers lies in asking good questions.

jawadmalik

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

Code: [Select]


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;

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

vinceherman



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.

vinceherman

#7
Sep 07, 2018, 04:35 pm Last Edit: Sep 07, 2018, 04:35 pm by vinceherman
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:
Code: [Select]

  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

Code: [Select]
for(int i = 0; i < 6; i++)
{
  Serial.println(d12[i]);
}

jawadmalik

Code: [Select]


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

jawadmalik

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


























nielyay

I have another website page. Maybe can help you with :

1.https://01.org/developerjourney/recipe/building-stewart-platform

2.https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=17&cad=rja&uact=8&ved=2ahUKEwitjMngxLndAhUDv48KHW6oBoEQFjAQegQIAxAC&url=https%3A%2F%2Fwww.mips-lab.net%2FMembers%2Fnsouch%2Fplateforme-de-stewart%2F%40%40download%2Fplans%2FArdruino_6DOF_Motion_Platform_LoRes.pdf&usg=AOvVaw1_gygXpWq95erJUJaNcFt2

jawadmalik

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

Go Up