stewart

A Stewart platform for radiation oncology testing and calibration.

Oct 28, 2022

•

1080 views

•

0 respects

health

robots

Components and supplies

Servo Module (Generic)

Arduino UNO

Apps and platforms

Fedora Linux

Project description

Code

Stewart platform

c_cpp

Radiation oncology testing

1#include <Servo.h>
2#include <BasicLinearAlgebra.h>
3
4#define ARM_LENGTH	  2.0
5#define LEG_LENGTH	 9.0
6
7#define P0x -1.430
8#define P0y  0.976
9#define  P1x -1.432
10#define P1y -1.000
11#define P2x -0.137
12#define P2y -1.743
13#define  P3x  1.577
14#define P3y -0.752
15#define P4x  1.582
16#define P4y  0.740
17#define  P5x -0.130
18#define P5y  1.726
19
20#define B0x -5.40268
21#define B0y  2.60979
22#define  B1x -5.40268
23#define B1y -2.60979
24#define B2x  0.441194
25#define B2y -5.98376
26#define  B3x  4.96149
27#define B3y -3.37396
28#define B4x  4.96149
29#define B4y  3.37396
30#define  B5x  0.441194
31#define B5y  5.98376
32
33typedef struct {
34  double x;
35  double y;
36} pos_s;
37
38class Platform {
39  public:
40  Platform() {
41    pitch = 0;
42    roll  = 0;
43    yaw   = 0;
44    dx    = 0;
45    dy    =  0;
46    dz    = 0;
47    pos[0].x = P0x; pos[0].y = P0y;
48    pos[1].x = P1x;  pos[1].y = P1y;
49    pos[2].x = P2x; pos[2].y = P2y;
50    pos[3].x = P3x; pos[3].y  = P3y;
51    pos[4].x = P4x; pos[4].y = P4y;
52    pos[5].x = P5x; pos[5].y =  P5y;
53  }
54  double pitch;
55  double roll;
56  double yaw;
57  double dx;
58  double dy;
59  double dz;
60  pos_s pos[6];
61};
62
63class Base {
64  public:
65  Base() {
66    pos[0].x = B0x; pos[0].y = B0y;
67    pos[1].x = B1x; pos[1].y  = B1y;
68    pos[2].x = B2x; pos[2].y = B2y;
69    pos[3].x = B3x; pos[3].y =  B3y;
70    pos[4].x = B4x; pos[4].y = B4y;
71    pos[5].x = B5x; pos[5].y = B5y;
72  }
73  pos_s pos[6];
74};
75
76#define NUM_SERVOS 6
77Servo myServo[NUM_SERVOS];
78
79Platform  myPlatform;
80Base myBase;
81
82double h0;
83
84 
85// x-axis
86static BLA::Matrix<4,4>  rotatePitch(double r)
87{
88  BLA::Matrix<4,4> mtx = {
89    1.0,     0.0,    0.0,        0.0,
90    0.0,     cos(r), -sin(r),    0.0,
91    0.0,     sin(r),  cos(r),    0.0,
92    0.0,     0.0,     0.0,       1.0
93  };
94  return mtx;
95}
96
97//  y-axis
98static BLA::Matrix<4,4> rotateRoll(double r)
99{
100  BLA::Matrix<4,4>  mtx = {
101    cos(r),  0.0, sin(r), 0.0,
102    0.0,     1.0, 0.0,    0.0,
103    -sin(r), 0.0, cos(r), 0.0,
104    0.0,     0.0, 0.0,    1.0
105  };
106  return  mtx;
107}
108
109// z-axis
110static BLA::Matrix<4,4> rotateYaw(double r)
111{
112  BLA::Matrix<4,4> mtx = {
113    cos(r),  -sin(r), 0.0, 0.0,
114    sin(r),   cos(r),  0.0, 0.0,
115    0.0,      0.0,    1.0, 0.0,
116    0.0,      0.0,    0.0, 1.0
117  };
118  return mtx;
119}
120
121static double length(BLA::Matrix<4> v)
122{
123  return sqrt((v(0) * v(0)) +
124                  (v(1) * v(1)) +
125                  (v(2)  * v(2)));
126}
127
128#define R2D(r) ((r / M_PI) * 180.0)
129
130double alpha[6]  = {180.0, 0.0, 180.0, 0.0, 180.0, 0.180};
131double incr[8];
132double pincr;
133
134static  void update_alpha()
135{
136
137  BLA::Matrix<4,4> pRot = rotatePitch(myPlatform.pitch);
138  BLA::Matrix<4,4> rRot = rotatePitch(myPlatform.roll);
139  BLA::Matrix<4,4> yRot  = rotatePitch(myPlatform.yaw);
140  BLA::Matrix<4,4> cRot = pRot * rRot * yRot;
141
142  //double alpha_stage[6];
143  bool is_valid = true;
144
145  for (int i = 0; i  < NUM_SERVOS; i++) {
146    // Eq 3
147    BLA::Matrix<4> P = {myPlatform.dx,myPlatform.dy  + h0,
148                        myPlatform.dz, 1.0};
149    BLA::Matrix<4> B =  {myBase.pos[i].x,myBase.pos[i].y, 
150                         0.0, 1.0};
151    BLA::Matrix<4>  deltaPB = P - B;
152    
153    // Eq 9
154    double beta = (i & 1) ? (M_PI/6.0)  : (-M_PI/6.0);
155  
156
157    double l = length (deltaPB);
158    print("l =  "); println(l);
159
160    double L  =
161      (l * l) - ((LEG_LENGTH * LEG_LENGTH)  - 
162                 (ARM_LENGTH * ARM_LENGTH));
163    
164    double M  = 2.0  * ARM_LENGTH * deltaPB(1);
165    double N  = 2.0 * ARM_LENGTH *
166      (deltaPB(0)  * cos (beta) + deltaPB(2) * sin (beta));
167    N = -fabs (N);
168    double arg  = L / sqrt ((M * M) + (N * N));
169    //Serial.println(arg);
170    double alphaX  = asin (arg) - atan2 (N, M);
171    if (isnan (alphaX)) {
172      is_valid = false;
173      Serial.println(i);
174      Serial.println(" boom");
175      break;
176    }
177    else {
178      alpha[i] = R2D((i&1) ? -alphaX : alphaX);
179      Serial.println(alpha[i]);
180    }
181
182#if 0
183  if (!is_valid) {
184  Serial.println("boom");
185
186  //  for (int i = 0; i < servos.size (); i++)
187  //    servos[i]->alpha = alpha_stage[i];
188  }
189  #endif
190  }
191}
192
193bool run_loop = true;
194
195void 
196setup()  {
197  Serial.begin(115200); // Any baud rate should work
198  Serial.println("Hello  Arduino\
199");
200  
201  myServo[0].attach( 11);
202  myServo[1].attach( 10);
203  myServo[2].attach( 9);
204  myServo[3].attach( 6);
205  myServo[4].attach( 5);
206  myServo[5].attach( 3);
207
208  h0 = 0.0;					// Eq 10
209  for (int i =  0; i < 6; i++) {
210    incr[i] = ((double)random(-1000, 1000))/500.0;
211    double  xp = myPlatform.pos[i].x;
212    double yp = myPlatform.pos[i].y;
213    double  xb = myBase.pos[i].x;
214    double yb = myBase.pos[i].y;
215    h0 += sqrt (pow  (LEG_LENGTH, 2.0) +
216		   pow (ARM_LENGTH, 2.0) -
217		  (pow ((xp - xb),  2.0) +
218		   pow ((yp - yb), 2.0)));
219  }
220  h0 /= 6.0;
221  update_alpha  ();
222  pincr = ((double)random(1000))/10000.0;
223}
224
225void 
226loop() {
227
228if  (run_loop) {
229  for(int i=0; i< NUM_SERVOS; i++) {
230    //Serial.println(alpha[i]);
231
232    myServo[i].write( alpha[i]);
233  }
234  myPlatform.yaw += pincr;
235  //Serial.println("yaw  ");
236  //Serial.println(myPlatform.yaw);
237  //Serial.println("\
238");
239  //Serial.println("\
240");
241  if (myPlatform.yaw > 10.0) {
242    pincr = -pincr;
243    myPlatform.yaw = 10.0;
244  }
245  else if (myPlatform.yaw < -10.0) {
246    pincr  = -pincr;
247    myPlatform.yaw = -10.0;
248 }
249 update_alpha();
250 delay( 2);
251  run_loop = false;
252}
253
254#if 0
255      alpha[i] += incr[i];
256      if (alpha[i]  > 180.0) {
257        incr[i] = -incr[i];
258        alpha[i] = 180.0;
259      }
260      if (alpha[i] < 0.0) {
261        incr[i] = -incr[i];
262        alpha[i] =  0.0;
263      }
264#endif
265  }
266
267
268
269
270#if 0
271  for (int a = 0; a<15;  a++) {
272    for(int i=0; i< NUM_SERVOS; i++) {
273      myServo[i].write( random(  0, 181));
274      delay( 2);
275    }
276    delay( 150);
277  }
278  
279  for(  int i=0; i<NUM_SERVOS; i++)  {
280    myServo[i].write( 0); // set to horn rotated  left
281  }
282  delay( 1000);
283
284  for( int a=0; a<3; a++) {
285    for( int  r=0; r<=180; r++) {     // move horns right
286      for( int i=0; i<NUM_SERVOS;  i++)  {
287        myServo[i].write( r);
288      }
289      delay( 6);
290    }
291    for( int r=180; r>=0; r--) {
292      for( int i=0; i<NUM_SERVOS; i++) { //horns  left
293        myServo[i].write( r);
294      }
295      delay( 6);
296    }
297  }
298#endif
299
300

#include <Servo.h>
#include <BasicLinearAlgebra.h>

#define ARM_LENGTH	  2.0
#define LEG_LENGTH	 9.0

#define P0x -1.430
#define P0y  0.976
#define  P1x -1.432
#define P1y -1.000
#define P2x -0.137
#define P2y -1.743
#define  P3x  1.577
#define P3y -0.752
#define P4x  1.582
#define P4y  0.740
#define  P5x -0.130
#define P5y  1.726

#define B0x -5.40268
#define B0y  2.60979
#define  B1x -5.40268
#define B1y -2.60979
#define B2x  0.441194
#define B2y -5.98376
#define  B3x  4.96149
#define B3y -3.37396
#define B4x  4.96149
#define B4y  3.37396
#define  B5x  0.441194
#define B5y  5.98376

typedef struct {
  double x;
  double y;
} pos_s;

class Platform {
  public:
  Platform() {
    pitch = 0;
    roll  = 0;
    yaw   = 0;
    dx    = 0;
    dy    =  0;
    dz    = 0;
    pos[0].x = P0x; pos[0].y = P0y;
    pos[1].x = P1x;  pos[1].y = P1y;
    pos[2].x = P2x; pos[2].y = P2y;
    pos[3].x = P3x; pos[3].y  = P3y;
    pos[4].x = P4x; pos[4].y = P4y;
    pos[5].x = P5x; pos[5].y =  P5y;
  }
  double pitch;
  double roll;
  double yaw;
  double dx;
  double dy;
  double dz;
  pos_s pos[6];
};

class Base {
  public:
  Base() {
    pos[0].x = B0x; pos[0].y = B0y;
    pos[1].x = B1x; pos[1].y  = B1y;
    pos[2].x = B2x; pos[2].y = B2y;
    pos[3].x = B3x; pos[3].y =  B3y;
    pos[4].x = B4x; pos[4].y = B4y;
    pos[5].x = B5x; pos[5].y = B5y;
  }
  pos_s pos[6];
};

#define NUM_SERVOS 6
Servo myServo[NUM_SERVOS];

Platform  myPlatform;
Base myBase;

double h0;

 
// x-axis
static BLA::Matrix<4,4>  rotatePitch(double r)
{
  BLA::Matrix<4,4> mtx = {
    1.0,     0.0,    0.0,        0.0,
    0.0,     cos(r), -sin(r),    0.0,
    0.0,     sin(r),  cos(r),    0.0,
    0.0,     0.0,     0.0,       1.0
  };
  return mtx;
}

//  y-axis
static BLA::Matrix<4,4> rotateRoll(double r)
{
  BLA::Matrix<4,4>  mtx = {
    cos(r),  0.0, sin(r), 0.0,
    0.0,     1.0, 0.0,    0.0,
    -sin(r), 0.0, cos(r), 0.0,
    0.0,     0.0, 0.0,    1.0
  };
  return  mtx;
}

// z-axis
static BLA::Matrix<4,4> rotateYaw(double r)
{
  BLA::Matrix<4,4> mtx = {
    cos(r),  -sin(r), 0.0, 0.0,
    sin(r),   cos(r),  0.0, 0.0,
    0.0,      0.0,    1.0, 0.0,
    0.0,      0.0,    0.0, 1.0
  };
  return mtx;
}

static double length(BLA::Matrix<4> v)
{
  return sqrt((v(0) * v(0)) +
                  (v(1) * v(1)) +
                  (v(2)  * v(2)));
}

#define R2D(r) ((r / M_PI) * 180.0)

double alpha[6]  = {180.0, 0.0, 180.0, 0.0, 180.0, 0.180};
double incr[8];
double pincr;

static  void update_alpha()
{

  BLA::Matrix<4,4> pRot = rotatePitch(myPlatform.pitch);
  BLA::Matrix<4,4> rRot = rotatePitch(myPlatform.roll);
  BLA::Matrix<4,4> yRot  = rotatePitch(myPlatform.yaw);
  BLA::Matrix<4,4> cRot = pRot * rRot * yRot;

  //double alpha_stage[6];
  bool is_valid = true;

  for (int i = 0; i  < NUM_SERVOS; i++) {
    // Eq 3
    BLA::Matrix<4> P = {myPlatform.dx,myPlatform.dy  + h0,
                        myPlatform.dz, 1.0};
    BLA::Matrix<4> B =  {myBase.pos[i].x,myBase.pos[i].y, 
                         0.0, 1.0};
    BLA::Matrix<4>  deltaPB = P - B;
    
    // Eq 9
    double beta = (i & 1) ? (M_PI/6.0)  : (-M_PI/6.0);
  

    double l = length (deltaPB);
    print("l =  "); println(l);

    double L  =
      (l * l) - ((LEG_LENGTH * LEG_LENGTH)  - 
                 (ARM_LENGTH * ARM_LENGTH));
    
    double M  = 2.0  * ARM_LENGTH * deltaPB(1);
    double N  = 2.0 * ARM_LENGTH *
      (deltaPB(0)  * cos (beta) + deltaPB(2) * sin (beta));
    N = -fabs (N);
    double arg  = L / sqrt ((M * M) + (N * N));
    //Serial.println(arg);
    double alphaX  = asin (arg) - atan2 (N, M);
    if (isnan (alphaX)) {
      is_valid = false;
      Serial.println(i);
      Serial.println(" boom");
      break;
    }
    else {
      alpha[i] = R2D((i&1) ? -alphaX : alphaX);
      Serial.println(alpha[i]);
    }

#if 0
  if (!is_valid) {
  Serial.println("boom");

  //  for (int i = 0; i < servos.size (); i++)
  //    servos[i]->alpha = alpha_stage[i];
  }
  #endif
  }
}

bool run_loop = true;

void 
setup()  {
  Serial.begin(115200); // Any baud rate should work
  Serial.println("Hello  Arduino\
");
  
  myServo[0].attach( 11);
  myServo[1].attach( 10);
  myServo[2].attach( 9);
  myServo[3].attach( 6);
  myServo[4].attach( 5);
  myServo[5].attach( 3);

  h0 = 0.0;					// Eq 10
  for (int i =  0; i < 6; i++) {
    incr[i] = ((double)random(-1000, 1000))/500.0;
    double  xp = myPlatform.pos[i].x;
    double yp = myPlatform.pos[i].y;
    double  xb = myBase.pos[i].x;
    double yb = myBase.pos[i].y;
    h0 += sqrt (pow  (LEG_LENGTH, 2.0) +
		   pow (ARM_LENGTH, 2.0) -
		  (pow ((xp - xb),  2.0) +
		   pow ((yp - yb), 2.0)));
  }
  h0 /= 6.0;
  update_alpha  ();
  pincr = ((double)random(1000))/10000.0;
}

void 
loop() {

if  (run_loop) {
  for(int i=0; i< NUM_SERVOS; i++) {
    //Serial.println(alpha[i]);

    myServo[i].write( alpha[i]);
  }
  myPlatform.yaw += pincr;
  //Serial.println("yaw  ");
  //Serial.println(myPlatform.yaw);
  //Serial.println("\
");
  //Serial.println("\
");
  if (myPlatform.yaw > 10.0) {
    pincr = -pincr;
    myPlatform.yaw = 10.0;
  }
  else if (myPlatform.yaw < -10.0) {
    pincr  = -pincr;
    myPlatform.yaw = -10.0;
 }
 update_alpha();
 delay( 2);
  run_loop = false;
}

#if 0
      alpha[i] += incr[i];
      if (alpha[i]  > 180.0) {
        incr[i] = -incr[i];
        alpha[i] = 180.0;
      }
      if (alpha[i] < 0.0) {
        incr[i] = -incr[i];
        alpha[i] =  0.0;
      }
#endif
  }




#if 0
  for (int a = 0; a<15;  a++) {
    for(int i=0; i< NUM_SERVOS; i++) {
      myServo[i].write( random(  0, 181));
      delay( 2);
    }
    delay( 150);
  }
  
  for(  int i=0; i<NUM_SERVOS; i++)  {
    myServo[i].write( 0); // set to horn rotated  left
  }
  delay( 1000);

  for( int a=0; a<3; a++) {
    for( int  r=0; r<=180; r++) {     // move horns right
      for( int i=0; i<NUM_SERVOS;  i++)  {
        myServo[i].write( r);
      }
      delay( 6);
    }
    for( int r=180; r>=0; r--) {
      for( int i=0; i<NUM_SERVOS; i++) { //horns  left
        myServo[i].write( r);
      }
      delay( 6);
    }
  }
#endif

Comments

Only logged in users can leave comments

chrismoller

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content