sArm

Robotic Arm Smart Controller System with IoT

Aug 26, 2022

•

328 views

•

2 respects

Components and supplies

Jumper wires (generic)

Arduino UNO

SG90 Micro-servo motor

Apps and platforms

Blynk

Arduino IDE

Processing

Project description

Code

Arduino code

arduino

1#include <Servo.h>
2#include <math.h>
3
4Servo sl, sr, base, gr;  // sl - left Servo, sr - right Servo, base - base Servo gr - gripper servo
5double  l1 = 8, l2 = 8; // measurements of sArm
6
7/////////////////////////////////////////////////////////////////////////////////////////////
8void  setup() {
9  //attach the servos
10  sl.attach(9);
11  sr.attach(3);
12  base.attach(10);
13  gr.attach(5 , 544, 853);
14
15  homepos();
16}
17
18////////////////////////////////////////////////////////////////////////////////////////////////Initial  sArm position.
19void homepos() {
20  mvsl(180, 20);
21  delay(500);
22  mvsr(30,  20);
23  delay(500);
24  mvb(90, 20);
25  delay(500);
26  gr.write(30);
27  delay(1000);
28}
29
30////////////////////////////////////////////////////////////////////////////////////////////////  We use for loops so we can control the speed of the servo
31void mvsl( int p, int  d) { // to move the left servo
32  //  p - end position d - delay
33  // If previous  position is bigger then current position
34  int pp = sl.read(); // set previous  position
35  if (pp > p) {
36    for ( int j = pp; j >= p; j--) {   // Run servo  down
37      sl.write(j);
38      delay(d);    // defines the speed at which the  servo rotates
39    }
40  }
41  // If previous position is smaller then current  position
42  if (pp < p) {
43    for ( int j = pp; j <= p; j++) {   // Run servo  up
44      sl.write(j);
45      delay(d);
46    }
47  }
48  if (pp == p) {
49    delay(2);
50  }
51}
52void mvsr( int p, int d) {// to move the right servo
53  //  p - end possion d - delay
54  // If previous position is bigger then current  position
55  int pp = sr.read(); // set previous position
56  if (pp > p) {
57    for ( int j = pp; j >= p; j--) {   // Run servo down
58      sr.write(j);
59      delay(d);    // defines the speed at which the servo rotates
60    }
61  }
62  // If previous position is smaller then current position
63  if (pp <  p) {
64    for ( int j = pp; j <= p; j++) {   // Run servo up
65      sr.write(j);
66      delay(d);
67    }
68  }
69  if (pp == p) {
70    delay(2);
71  }
72}
73void  mvb( int p, int d) {// to move the base servo
74  //  p - end possion d - delay
75  // If previous position is bigger then current position
76  int pp = base.read();  // set previous position
77  if (pp > p) {
78    for ( int j = pp; j >= p; j--)  {   // Run servo down
79      base.write(j);
80      delay(d);    // defines the  speed at which the servo rotates
81    }
82  }
83  // If previous position is  smaller then current position
84  if (pp < p) {
85    for ( int j = pp; j <= p;  j++) {   // Run servo up
86      base.write(j);
87      delay(d);
88    }
89  }
90  if (pp == p) {
91    delay(2);
92  }
93}
94
95//open or closs the  gripper
96//  1 - open 0 - close
97void grf( int p) {
98  int pp = gr.read();  // set previous position
99  if (p == 1) {
100    for ( int j = pp; j >= 5; j--)  {
101      gr.write(j);
102      delay(20);    // defines the speed at which the  servo rotates
103    }
104  }
105  if (p == 0) {
106    for ( int j = pp; j <= 30;  j++) {
107      gr.write(j);
108      delay(20);
109    }
110  }
111}
112
113//////////////////////////////////////////////////////////////////////////////////////////////
114//Function  for inverse kinematics for sArm
115// Move end efactor to corsponting (X,Y,Z)
116void  ikMoveTo(double x, double y, double z) {
117  double t3 = degrees(atan2(y , x));
118  double t2 = degrees(acos((pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(l1, 2) - pow(l2,  2)) / (2 * l1 * l2)));
119  double t1 = degrees(atan(z / (sqrt(pow(x, 2) + pow(y,  2)))) + atan(l2 * sin(radians(t2)) / (l1 + l2 * cos(radians(t2)))));
120
121  mvsl((int(180  - (t2 - (t1 - 30)))), 5);
122  mvsr((int(130 - t1)), 5);
123  mvb((int(90 + t3)),  5);
124}
125
126////////////////////////////////////////////////////////////////////////////////////////////////  Draw a stright line that connect given two points.
127void drawLine(int a, int b,  int c, int p, int q, int r) {
128  if (a < p) {
129    for (int t = a; t <= p; t++)  {
130      double x = t;
131      double y = (((x - a) * (q - b)) / (p - a)) + b;
132      double z = (((x - a) * (r - c)) / (p - a)) + c;
133      ikMoveTo(x, y, z);
134      delay(10);
135    }
136  }
137  if (a > p) {
138    for (int t = a; t >= p;  t--) {
139      double x = t;
140      double y = (((x - a) * (q - b)) / (p - a))  + b;
141      double z = (((x - a) * (r - c)) / (p - a)) + c;
142      ikMoveTo(x,  y, z);
143      delay(10);
144    }
145  }
146  if (a == p) {
147    if (b < q)  {
148      for (int t = b; t <= q; t++) {
149        double y = t;
150        double  x = (((y - b) * (p - a)) / (q - b)) + a;
151        double z = (((x - a) * (r -  c)) / (p - a)) + c;
152        ikMoveTo(x, y, z);
153        delay(10);
154      }
155    }
156    if (b > q) {
157      for (int t = b; t >= q; t--) {
158        double  y = t;
159        double x = (((y - b) * (p - a)) / (q - b)) + a;
160        double  z = (((x - a) * (r - c)) / (p - a)) + c;
161        ikMoveTo(x, y, z);
162        delay(10);
163      }
164    }
165    if (b == q) {
166      if (c < r) {
167        for (int  t = c; t <= r; t++) {
168          double z = t;
169          double y = (((z -  c) * (q - b)) / (r - c)) + b;
170          double x = (((z - c) * (p - a)) / (r  - c)) + a;
171
172          ikMoveTo(x, y, z);
173          delay(10);
174        }
175      }
176      if (c > r) {
177        for (int t = c; t >= r; t--) {
178          double  z = t;
179          double y = (((z - c) * (q - b)) / (r - c)) + b;
180          double  x = (((z - c) * (p - a)) / (r - c)) + a;
181
182          ikMoveTo(x, y, z);
183          delay(10);
184        }
185      }
186      if (c == r) {
187        ikMoveTo(a,  b, c);
188      }
189    }
190  }
191}
192//////////////////////////////////////////////////////////////////////////////////////////////
193//  Draw a triangle that connect given three points in XYZ space.
194void drawTri(int  a, int b, int c, int p, int q, int r, int x, int y, int z) {
195  drawLine(a, b,  c, p, q, r);
196  delay(500);
197  drawLine(p, q, r, x, y, z);
198  delay(500);
199  drawLine(x, y, z, a, b, c);
200  delay(500);
201}
202//////////////////////////////////////////////////////////////////////////////////////////////
203void  loop() {
204  // program for a sample task, sArm do this again and again
205  //  you can program the sArm with these functions easily
206
207  drawTri(10, 0, 5,  17, 0, 13, 17, 0, 0);
208  delay(1000);
209  ikMoveTo(10, 1, 10);
210  delay(1000);
211  drawLine(10, 0, 0, 10, 0, 9);
212  delay(10);
213  grf(1);
214  delay(1000);
215  drawLine(10, 0, 9, 10, 0, 0);
216  delay(10);
217  grf(0);
218  delay(1000);
219}
220

#include <Servo.h>
#include <math.h>

Servo sl, sr, base, gr;  // sl - left Servo, sr - right Servo, base - base Servo gr - gripper servo
double  l1 = 8, l2 = 8; // measurements of sArm

/////////////////////////////////////////////////////////////////////////////////////////////
void  setup() {
  //attach the servos
  sl.attach(9);
  sr.attach(3);
  base.attach(10);
  gr.attach(5 , 544, 853);

  homepos();
}

////////////////////////////////////////////////////////////////////////////////////////////////Initial  sArm position.
void homepos() {
  mvsl(180, 20);
  delay(500);
  mvsr(30,  20);
  delay(500);
  mvb(90, 20);
  delay(500);
  gr.write(30);
  delay(1000);
}

////////////////////////////////////////////////////////////////////////////////////////////////  We use for loops so we can control the speed of the servo
void mvsl( int p, int  d) { // to move the left servo
  //  p - end position d - delay
  // If previous  position is bigger then current position
  int pp = sl.read(); // set previous  position
  if (pp > p) {
    for ( int j = pp; j >= p; j--) {   // Run servo  down
      sl.write(j);
      delay(d);    // defines the speed at which the  servo rotates
    }
  }
  // If previous position is smaller then current  position
  if (pp < p) {
    for ( int j = pp; j <= p; j++) {   // Run servo  up
      sl.write(j);
      delay(d);
    }
  }
  if (pp == p) {
    delay(2);
  }
}
void mvsr( int p, int d) {// to move the right servo
  //  p - end possion d - delay
  // If previous position is bigger then current  position
  int pp = sr.read(); // set previous position
  if (pp > p) {
    for ( int j = pp; j >= p; j--) {   // Run servo down
      sr.write(j);
      delay(d);    // defines the speed at which the servo rotates
    }
  }
  // If previous position is smaller then current position
  if (pp <  p) {
    for ( int j = pp; j <= p; j++) {   // Run servo up
      sr.write(j);
      delay(d);
    }
  }
  if (pp == p) {
    delay(2);
  }
}
void  mvb( int p, int d) {// to move the base servo
  //  p - end possion d - delay
  // If previous position is bigger then current position
  int pp = base.read();  // set previous position
  if (pp > p) {
    for ( int j = pp; j >= p; j--)  {   // Run servo down
      base.write(j);
      delay(d);    // defines the  speed at which the servo rotates
    }
  }
  // If previous position is  smaller then current position
  if (pp < p) {
    for ( int j = pp; j <= p;  j++) {   // Run servo up
      base.write(j);
      delay(d);
    }
  }
  if (pp == p) {
    delay(2);
  }
}

//open or closs the  gripper
//  1 - open 0 - close
void grf( int p) {
  int pp = gr.read();  // set previous position
  if (p == 1) {
    for ( int j = pp; j >= 5; j--)  {
      gr.write(j);
      delay(20);    // defines the speed at which the  servo rotates
    }
  }
  if (p == 0) {
    for ( int j = pp; j <= 30;  j++) {
      gr.write(j);
      delay(20);
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
//Function  for inverse kinematics for sArm
// Move end efactor to corsponting (X,Y,Z)
void  ikMoveTo(double x, double y, double z) {
  double t3 = degrees(atan2(y , x));
  double t2 = degrees(acos((pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(l1, 2) - pow(l2,  2)) / (2 * l1 * l2)));
  double t1 = degrees(atan(z / (sqrt(pow(x, 2) + pow(y,  2)))) + atan(l2 * sin(radians(t2)) / (l1 + l2 * cos(radians(t2)))));

  mvsl((int(180  - (t2 - (t1 - 30)))), 5);
  mvsr((int(130 - t1)), 5);
  mvb((int(90 + t3)),  5);
}

////////////////////////////////////////////////////////////////////////////////////////////////  Draw a stright line that connect given two points.
void drawLine(int a, int b,  int c, int p, int q, int r) {
  if (a < p) {
    for (int t = a; t <= p; t++)  {
      double x = t;
      double y = (((x - a) * (q - b)) / (p - a)) + b;
      double z = (((x - a) * (r - c)) / (p - a)) + c;
      ikMoveTo(x, y, z);
      delay(10);
    }
  }
  if (a > p) {
    for (int t = a; t >= p;  t--) {
      double x = t;
      double y = (((x - a) * (q - b)) / (p - a))  + b;
      double z = (((x - a) * (r - c)) / (p - a)) + c;
      ikMoveTo(x,  y, z);
      delay(10);
    }
  }
  if (a == p) {
    if (b < q)  {
      for (int t = b; t <= q; t++) {
        double y = t;
        double  x = (((y - b) * (p - a)) / (q - b)) + a;
        double z = (((x - a) * (r -  c)) / (p - a)) + c;
        ikMoveTo(x, y, z);
        delay(10);
      }
    }
    if (b > q) {
      for (int t = b; t >= q; t--) {
        double  y = t;
        double x = (((y - b) * (p - a)) / (q - b)) + a;
        double  z = (((x - a) * (r - c)) / (p - a)) + c;
        ikMoveTo(x, y, z);
        delay(10);
      }
    }
    if (b == q) {
      if (c < r) {
        for (int  t = c; t <= r; t++) {
          double z = t;
          double y = (((z -  c) * (q - b)) / (r - c)) + b;
          double x = (((z - c) * (p - a)) / (r  - c)) + a;

          ikMoveTo(x, y, z);
          delay(10);
        }
      }
      if (c > r) {
        for (int t = c; t >= r; t--) {
          double  z = t;
          double y = (((z - c) * (q - b)) / (r - c)) + b;
          double  x = (((z - c) * (p - a)) / (r - c)) + a;

          ikMoveTo(x, y, z);
          delay(10);
        }
      }
      if (c == r) {
        ikMoveTo(a,  b, c);
      }
    }
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////
//  Draw a triangle that connect given three points in XYZ space.
void drawTri(int  a, int b, int c, int p, int q, int r, int x, int y, int z) {
  drawLine(a, b,  c, p, q, r);
  delay(500);
  drawLine(p, q, r, x, y, z);
  delay(500);
  drawLine(x, y, z, a, b, c);
  delay(500);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void  loop() {
  // program for a sample task, sArm do this again and again
  //  you can program the sArm with these functions easily

  drawTri(10, 0, 5,  17, 0, 13, 17, 0, 0);
  delay(1000);
  ikMoveTo(10, 1, 10);
  delay(1000);
  drawLine(10, 0, 0, 10, 0, 9);
  delay(10);
  grf(1);
  delay(1000);
  drawLine(10, 0, 9, 10, 0, 0);
  delay(10);
  grf(0);
  delay(1000);
}

Processing Code

java

1import processing.serial.*;
2import controlP5.*;
3import cc.arduino.*;
4
5Arduino   arduino;
6ControlP5 cp5;
7
8float x=113, y = 0, z=95, x1=113, y1, t1, t2,   t3;
9float  l1=180, l2=200, l3=50; // sArm mesurements
10int k=0, k1=0; // index   for seaved poses
11float sliderX=x, sliderY=y, sliderZ=z, sliderG;
12// arraies   for save pos
13float[] posX = new float[10000];
14float[] posY = new float[10000];
15float[]   posZ = new float[10000];
16int rs=0; // run status
17
18void setup() {
19  size(1440,   840);
20
21  cp5 = new ControlP5(this);
22
23  println(Arduino.list());
24   //Select port from the listed array.
25  //replace [0] to [1],[2]...for selecting   a usable open port.
26  arduino = new Arduino(this, Arduino.list()[0], 57600);
27   
28  arduino.pinMode(9, Arduino.SERVO); // left servo
29  arduino.pinMode(3,   Arduino.SERVO); // right servo
30  arduino.pinMode(10, Arduino.SERVO); // base   servo
31  arduino.pinMode(5, Arduino.SERVO); // gripper
32
33  PFont pfont =   createFont("Arial", 225, true); // use true/false for smooth/no-smooth
34  ControlFont   font = new ControlFont(pfont, 20);
35  ControlFont font2 = new ControlFont(pfont,   25);
36
37  //X controls
38  cp5.addSlider("sliderX")
39    .setPosition(600,   height/2+100)
40    .setSize(270, 30)
41    .setRange(0, l1+l2-10) // Slider range,   corresponds to Joint 1 or theta1 angle that the robot can move to
42    .setColorLabel(225)
43     .setCaptionLabel("X")
44    .setFont(font)
45    ;
46  cp5.getController("sliderX").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);
47
48  //Y controls
49  cp5.addSlider("sliderY")
50     .setPosition(600, height/2+200)
51    .setSize(270, 30)
52    .setRange(-(l1+l2-50),   l1+l2-50) // Slider range, corresponds to Joint 1 or theta1 angle that the robot   can move to
53    .setColorLabel(225)
54    .setCaptionLabel("Y")
55    .setFont(font)
56     ;
57  cp5.getController("sliderY").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);
58
59  //Z controls
60  cp5.addSlider("sliderZ")
61     .setPosition(600, height/2+300)
62    .setSize(270, 30)
63    .setRange(-160,   180) // Slider range, corresponds to Joint 1 or theta1 angle that the robot can   move to
64    .setColorLabel(225)
65    .setCaptionLabel("Z")
66    .setFont(font)
67     ;
68  cp5.getController("sliderZ").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);
69
70  cp5.addSlider("sliderG")
71    .setPosition(950,   height/2+300)
72    .setSize(190, 30)
73    .setRange(0, 100)
74    .setColorLabel(225)
75     .setFont(font)
76    .setCaptionLabel("Gripper")
77    .setNumberOfTickMarks(5)
78     ;
79  cp5.getController("sliderG").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);
80
81  cp5.addButton("savePosition")
82     .setPosition(950, 520)
83    .setSize(215, 50)
84    .setFont(font2)
85    .setCaptionLabel("SAVE   POSITION")
86    ;
87
88  cp5.addButton("run")
89    .setPosition(1205, 520)
90     .setSize(215, 50)
91    .setFont(font2)
92    .setCaptionLabel("RUN PROGRAM")
93     ;
94
95  cp5.addButton("clearSteps")
96    .setPosition(1270, 700)
97     .setSize(135, 40)
98    .setFont(font)
99    .setCaptionLabel("(CLEAR)")
100     ;
101}
102
103void draw() {
104  background(20);
105
106  PVector m = new PVector(mouseX,   mouseY); // position vector of mouse
107  PVector c = new PVector(200, height/2);   //position vector of new center
108
109  pushMatrix();
110  translate(200, height/2);
111   PVector c1 = new PVector(800, 0);
112  m.sub(c);
113
114  if (k1>=k) {
115    k1=0;
116   }
117
118  if (rs==1) { // if run butten is pressed run the seaved possions
119     x=posX[k1];
120    y=posY[k1];
121    z=posZ[k1];
122    sliderX=x;
123    sliderZ=z;
124     sliderY=y;
125  }
126  k1++;
127
128  // to contrall the side view simulation
129   if (mousePressed == true && ((sq(200-mouseX) + sq(height/2-mouseY))<=sq(l1+l2))   && mouseX>200) {
130    x1 = m.x;
131    z = -m.y;
132    x = sqrt(sq(x1)-sq(y))   ;
133    sliderX=x;
134    sliderZ=z;
135  }
136
137  // to contrall the top view   simulation
138  if (mousePressed == true && ((sq(1000-mouseX) + sq(height/2 - mouseY))<=sq(l1+l2   -5)) && mouseY<height/2) {
139    y= m.x - 800;
140    x = -m.y;
141    x1 = sqrt(sq(y)+sq(m.y))   ;
142    sliderX=x;
143    sliderY=y;
144  }
145
146  x=sliderX;
147  y=sliderY;
148   z=sliderZ;
149
150  //press w or s to increes or decrees the Z
151  if (keyPressed)   {
152    if (key == 'w' || key == 'W') {
153      z++;
154    }
155    if (key ==   's' || key == 'S') {
156      z--;
157    }
158  }
159
160  cp5.getController("sliderX").setValue(x);
161   cp5.getController("sliderY").setValue(y);
162  cp5.getController("sliderZ").setValue(z);
163
164   //Inverse kinematics based on vectos.
165  t3= atan2(y, x);
166  t2= acos((sq(x)   +sq(y)+sq(z)-sq(l1)-sq(l2))/(2*l1*l2));
167  t1= (atan2(z, ( sqrt(sq(x)+sq(y))))   + atan2(l2*sin(t2), (l1+l2*cos(t2))));
168
169  PVector a = new PVector(l1*cos(-t1),   l1*sin(-t1));
170  PVector b = new PVector();
171  PVector p = new PVector(y, x);
172   p.sub(c1);
173  m.set(x1, -z);
174  b=PVector.sub(a, m);
175
176  float t4 = b.heading();
177
178   stroke(0, 250, 0);
179  strokeWeight(30);
180  line(a.x, a.y, a.x+l3*cos(t4),   a.y+l3*sin(t4));
181  b.mult(-1);
182  float t5 = b.heading();
183  line(a.x, a.y,   a.x+l2*cos(t5), a.y+l2*sin(t5));
184  stroke(255, 0, 200);
185  strokeWeight(20);
186   line(l3*cos(t4), l3*sin(t4), a.x+l3*cos(t4), a.y+l3*sin(t4));  
187  stroke(255);
188   line(0, 0, l3*cos(t4), l3*sin(t4));
189  stroke(255, 0, 0);
190  strokeWeight(40);
191   line(0, 0, a.x, a.y);
192  stroke(10);
193  ellipse(0, 0, 10, 10);
194
195  for   (int i=160; i<=2*(l1+l2 - 10); i=i+40) {
196    stroke(255);
197    strokeWeight(2);
198     noFill();
199    arc(800, 0, i, i, PI, 2*PI);
200  }
201
202  if  (((sq(p.x+800)   + sq(p.y))<=sq(l1+l2-5)) && -p.y<0) {
203    stroke(0, 0, 255);
204    strokeWeight(15);
205     line(800, 0, p.x + 1600, -p.y);
206    ellipse(p.x +1600, -p.y, 10, 10);
207   }
208
209  PVector l = new PVector(1, 0);
210  l.mult(-1);
211  float al = PVector.angleBetween(l,   b);
212  //l.mult(-1);
213  l.rotate(PI/2-radians(50));
214  float ar = PVector.angleBetween(l,   a);
215
216  // move the servos to the positions.
217  arduino.servoWrite(9, int(degrees(al))   );
218  arduino.servoWrite(3, int(degrees(ar)) );
219  arduino.servoWrite(10, int(90   +degrees(t3)) );
220
221  //print the texts on the GUI
222  textSize(18);
223  textAlign(CENTER);
224   text("X", -120, 270);
225  text("Y", -70, 270);
226  text("Z", -30, 270);
227   text(int(x), -120, 300);
228  text(int(y), -70, 300);
229  text(int(z), -30, 300);
230   text("leftServo Angle", 80, 270);
231  text(int(degrees(al)), 200, 270);
232   text("rightServo Angle", 80, 300);
233  text(int(degrees(ar)), 200, 300);
234   textSize(15);
235  textAlign(LEFT);
236  text("press w or s to increes or decrees   the Z", -130, 340);
237  text("preaa R key and move to recored", -130, 360);
238   textSize(60);
239  text("sArm manipulator", 130, -365);
240  popMatrix();
241//preaa   R key and move to recored
242  if (keyPressed) {
243    if (key == 'r' || key ==   'R') {
244      posX[k]= x;
245      posY[k]=y;
246      posZ[k]=z;
247      k++;
248       cp5.getController("savePosition").setCaptionLabel("RECODING");
249      cp5.getController("savePosition").setColorLabel(#e74c3c);
250     }
251  } else {
252    cp5.getController("savePosition").setCaptionLabel("SAVE   POSITION");
253    cp5.getController("savePosition").setColorLabel(255);
254   }
255}
256// seave position with butten
257public void savePosition() {
258  posX[k]=   x;
259  posY[k]=y;
260  posZ[k]=z;
261  k++;
262}
263// Play the seaved positions
264public   void run() {
265  if (rs == 0) {
266    cp5.getController("run").setCaptionLabel("STOP");
267     cp5.getController("run").setColorLabel(#e74c3c);
268
269    rs = 1;
270  }   else if (rs == 1) {
271    rs = 0;
272    cp5.getController("run").setCaptionLabel("RUN   PROGRAM");
273    cp5.getController("run").setColorLabel(255);
274  }
275}
276//   clear the seaved poses
277public void clearSteps() {
278  for (int i =0; i<k; i++)   {
279    posX[i]=posX[k];
280    posY[i]=posY[k];
281    posZ[i]=posZ[k];
282  }
283   k=0;
284  posX[k]= 113;
285  posY[k]=0;
286  posZ[k]=95;
287}
288

import processing.serial.*;
import controlP5.*;
import cc.arduino.*;

Arduino   arduino;
ControlP5 cp5;

float x=113, y = 0, z=95, x1=113, y1, t1, t2,   t3;
float  l1=180, l2=200, l3=50; // sArm mesurements
int k=0, k1=0; // index   for seaved poses
float sliderX=x, sliderY=y, sliderZ=z, sliderG;
// arraies   for save pos
float[] posX = new float[10000];
float[] posY = new float[10000];
float[]   posZ = new float[10000];
int rs=0; // run status

void setup() {
  size(1440,   840);

  cp5 = new ControlP5(this);

  println(Arduino.list());
   //Select port from the listed array.
  //replace [0] to [1],[2]...for selecting   a usable open port.
  arduino = new Arduino(this, Arduino.list()[0], 57600);
   
  arduino.pinMode(9, Arduino.SERVO); // left servo
  arduino.pinMode(3,   Arduino.SERVO); // right servo
  arduino.pinMode(10, Arduino.SERVO); // base   servo
  arduino.pinMode(5, Arduino.SERVO); // gripper

  PFont pfont =   createFont("Arial", 225, true); // use true/false for smooth/no-smooth
  ControlFont   font = new ControlFont(pfont, 20);
  ControlFont font2 = new ControlFont(pfont,   25);

  //X controls
  cp5.addSlider("sliderX")
    .setPosition(600,   height/2+100)
    .setSize(270, 30)
    .setRange(0, l1+l2-10) // Slider range,   corresponds to Joint 1 or theta1 angle that the robot can move to
    .setColorLabel(225)
     .setCaptionLabel("X")
    .setFont(font)
    ;
  cp5.getController("sliderX").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);

  //Y controls
  cp5.addSlider("sliderY")
     .setPosition(600, height/2+200)
    .setSize(270, 30)
    .setRange(-(l1+l2-50),   l1+l2-50) // Slider range, corresponds to Joint 1 or theta1 angle that the robot   can move to
    .setColorLabel(225)
    .setCaptionLabel("Y")
    .setFont(font)
     ;
  cp5.getController("sliderY").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);

  //Z controls
  cp5.addSlider("sliderZ")
     .setPosition(600, height/2+300)
    .setSize(270, 30)
    .setRange(-160,   180) // Slider range, corresponds to Joint 1 or theta1 angle that the robot can   move to
    .setColorLabel(225)
    .setCaptionLabel("Z")
    .setFont(font)
     ;
  cp5.getController("sliderZ").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);

  cp5.addSlider("sliderG")
    .setPosition(950,   height/2+300)
    .setSize(190, 30)
    .setRange(0, 100)
    .setColorLabel(225)
     .setFont(font)
    .setCaptionLabel("Gripper")
    .setNumberOfTickMarks(5)
     ;
  cp5.getController("sliderG").getCaptionLabel().align(ControlP5.LEFT,   ControlP5.TOP_OUTSIDE).setPaddingX(0);

  cp5.addButton("savePosition")
     .setPosition(950, 520)
    .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("SAVE   POSITION")
    ;

  cp5.addButton("run")
    .setPosition(1205, 520)
     .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("RUN PROGRAM")
     ;

  cp5.addButton("clearSteps")
    .setPosition(1270, 700)
     .setSize(135, 40)
    .setFont(font)
    .setCaptionLabel("(CLEAR)")
     ;
}

void draw() {
  background(20);

  PVector m = new PVector(mouseX,   mouseY); // position vector of mouse
  PVector c = new PVector(200, height/2);   //position vector of new center

  pushMatrix();
  translate(200, height/2);
   PVector c1 = new PVector(800, 0);
  m.sub(c);

  if (k1>=k) {
    k1=0;
   }

  if (rs==1) { // if run butten is pressed run the seaved possions
     x=posX[k1];
    y=posY[k1];
    z=posZ[k1];
    sliderX=x;
    sliderZ=z;
     sliderY=y;
  }
  k1++;

  // to contrall the side view simulation
   if (mousePressed == true && ((sq(200-mouseX) + sq(height/2-mouseY))<=sq(l1+l2))   && mouseX>200) {
    x1 = m.x;
    z = -m.y;
    x = sqrt(sq(x1)-sq(y))   ;
    sliderX=x;
    sliderZ=z;
  }

  // to contrall the top view   simulation
  if (mousePressed == true && ((sq(1000-mouseX) + sq(height/2 - mouseY))<=sq(l1+l2   -5)) && mouseY<height/2) {
    y= m.x - 800;
    x = -m.y;
    x1 = sqrt(sq(y)+sq(m.y))   ;
    sliderX=x;
    sliderY=y;
  }

  x=sliderX;
  y=sliderY;
   z=sliderZ;

  //press w or s to increes or decrees the Z
  if (keyPressed)   {
    if (key == 'w' || key == 'W') {
      z++;
    }
    if (key ==   's' || key == 'S') {
      z--;
    }
  }

  cp5.getController("sliderX").setValue(x);
   cp5.getController("sliderY").setValue(y);
  cp5.getController("sliderZ").setValue(z);

   //Inverse kinematics based on vectos.
  t3= atan2(y, x);
  t2= acos((sq(x)   +sq(y)+sq(z)-sq(l1)-sq(l2))/(2*l1*l2));
  t1= (atan2(z, ( sqrt(sq(x)+sq(y))))   + atan2(l2*sin(t2), (l1+l2*cos(t2))));

  PVector a = new PVector(l1*cos(-t1),   l1*sin(-t1));
  PVector b = new PVector();
  PVector p = new PVector(y, x);
   p.sub(c1);
  m.set(x1, -z);
  b=PVector.sub(a, m);

  float t4 = b.heading();

   stroke(0, 250, 0);
  strokeWeight(30);
  line(a.x, a.y, a.x+l3*cos(t4),   a.y+l3*sin(t4));
  b.mult(-1);
  float t5 = b.heading();
  line(a.x, a.y,   a.x+l2*cos(t5), a.y+l2*sin(t5));
  stroke(255, 0, 200);
  strokeWeight(20);
   line(l3*cos(t4), l3*sin(t4), a.x+l3*cos(t4), a.y+l3*sin(t4));  
  stroke(255);
   line(0, 0, l3*cos(t4), l3*sin(t4));
  stroke(255, 0, 0);
  strokeWeight(40);
   line(0, 0, a.x, a.y);
  stroke(10);
  ellipse(0, 0, 10, 10);

  for   (int i=160; i<=2*(l1+l2 - 10); i=i+40) {
    stroke(255);
    strokeWeight(2);
     noFill();
    arc(800, 0, i, i, PI, 2*PI);
  }

  if  (((sq(p.x+800)   + sq(p.y))<=sq(l1+l2-5)) && -p.y<0) {
    stroke(0, 0, 255);
    strokeWeight(15);
     line(800, 0, p.x + 1600, -p.y);
    ellipse(p.x +1600, -p.y, 10, 10);
   }

  PVector l = new PVector(1, 0);
  l.mult(-1);
  float al = PVector.angleBetween(l,   b);
  //l.mult(-1);
  l.rotate(PI/2-radians(50));
  float ar = PVector.angleBetween(l,   a);

  // move the servos to the positions.
  arduino.servoWrite(9, int(degrees(al))   );
  arduino.servoWrite(3, int(degrees(ar)) );
  arduino.servoWrite(10, int(90   +degrees(t3)) );

  //print the texts on the GUI
  textSize(18);
  textAlign(CENTER);
   text("X", -120, 270);
  text("Y", -70, 270);
  text("Z", -30, 270);
   text(int(x), -120, 300);
  text(int(y), -70, 300);
  text(int(z), -30, 300);
   text("leftServo Angle", 80, 270);
  text(int(degrees(al)), 200, 270);
   text("rightServo Angle", 80, 300);
  text(int(degrees(ar)), 200, 300);
   textSize(15);
  textAlign(LEFT);
  text("press w or s to increes or decrees   the Z", -130, 340);
  text("preaa R key and move to recored", -130, 360);
   textSize(60);
  text("sArm manipulator", 130, -365);
  popMatrix();
//preaa   R key and move to recored
  if (keyPressed) {
    if (key == 'r' || key ==   'R') {
      posX[k]= x;
      posY[k]=y;
      posZ[k]=z;
      k++;
       cp5.getController("savePosition").setCaptionLabel("RECODING");
      cp5.getController("savePosition").setColorLabel(#e74c3c);
     }
  } else {
    cp5.getController("savePosition").setCaptionLabel("SAVE   POSITION");
    cp5.getController("savePosition").setColorLabel(255);
   }
}
// seave position with butten
public void savePosition() {
  posX[k]=   x;
  posY[k]=y;
  posZ[k]=z;
  k++;
}
// Play the seaved positions
public   void run() {
  if (rs == 0) {
    cp5.getController("run").setCaptionLabel("STOP");
     cp5.getController("run").setColorLabel(#e74c3c);

    rs = 1;
  }   else if (rs == 1) {
    rs = 0;
    cp5.getController("run").setCaptionLabel("RUN   PROGRAM");
    cp5.getController("run").setColorLabel(255);
  }
}
//   clear the seaved poses
public void clearSteps() {
  for (int i =0; i<k; i++)   {
    posX[i]=posX[k];
    posY[i]=posY[k];
    posZ[i]=posZ[k];
  }
   k=0;
  posX[k]= 113;
  posY[k]=0;
  posZ[k]=95;
}

Arduino code

arduino

1#include <Servo.h>
2#include <math.h>
3
4Servo sl, sr, base, gr;
5  // sl - left Servo, sr - right Servo, base - base Servo gr - gripper servo
6double
7  l1 = 8, l2 = 8; // measurements of sArm
8
9/////////////////////////////////////////////////////////////////////////////////////////////
10void
11  setup() {
12  //attach the servos
13  sl.attach(9);
14  sr.attach(3);
15  base.attach(10);
16
17  gr.attach(5 , 544, 853);
18
19  homepos();
20}
21
22////////////////////////////////////////////////////////////////////////////////////////////////Initial
23  sArm position.
24void homepos() {
25  mvsl(180, 20);
26  delay(500);
27  mvsr(30,
28  20);
29  delay(500);
30  mvb(90, 20);
31  delay(500);
32  gr.write(30);
33
34  delay(1000);
35}
36
37////////////////////////////////////////////////////////////////////////////////////////////////
38  We use for loops so we can control the speed of the servo
39void mvsl( int p, int
40  d) { // to move the left servo
41  //  p - end position d - delay
42  // If previous
43  position is bigger then current position
44  int pp = sl.read(); // set previous
45  position
46  if (pp > p) {
47    for ( int j = pp; j >= p; j--) {   // Run servo
48  down
49      sl.write(j);
50      delay(d);    // defines the speed at which the
51  servo rotates
52    }
53  }
54  // If previous position is smaller then current
55  position
56  if (pp < p) {
57    for ( int j = pp; j <= p; j++) {   // Run servo
58  up
59      sl.write(j);
60      delay(d);
61    }
62  }
63  if (pp == p) {
64
65    delay(2);
66  }
67}
68void mvsr( int p, int d) {// to move the right servo
69
70  //  p - end possion d - delay
71  // If previous position is bigger then current
72  position
73  int pp = sr.read(); // set previous position
74  if (pp > p) {
75
76    for ( int j = pp; j >= p; j--) {   // Run servo down
77      sr.write(j);
78
79      delay(d);    // defines the speed at which the servo rotates
80    }
81
82  }
83  // If previous position is smaller then current position
84  if (pp <
85  p) {
86    for ( int j = pp; j <= p; j++) {   // Run servo up
87      sr.write(j);
88
89      delay(d);
90    }
91  }
92  if (pp == p) {
93    delay(2);
94  }
95}
96void
97  mvb( int p, int d) {// to move the base servo
98  //  p - end possion d - delay
99
100  // If previous position is bigger then current position
101  int pp = base.read();
102  // set previous position
103  if (pp > p) {
104    for ( int j = pp; j >= p; j--)
105  {   // Run servo down
106      base.write(j);
107      delay(d);    // defines the
108  speed at which the servo rotates
109    }
110  }
111  // If previous position is
112  smaller then current position
113  if (pp < p) {
114    for ( int j = pp; j <= p;
115  j++) {   // Run servo up
116      base.write(j);
117      delay(d);
118    }
119
120  }
121  if (pp == p) {
122    delay(2);
123  }
124}
125
126//open or closs the
127  gripper
128//  1 - open 0 - close
129void grf( int p) {
130  int pp = gr.read();
131  // set previous position
132  if (p == 1) {
133    for ( int j = pp; j >= 5; j--)
134  {
135      gr.write(j);
136      delay(20);    // defines the speed at which the
137  servo rotates
138    }
139  }
140  if (p == 0) {
141    for ( int j = pp; j <= 30;
142  j++) {
143      gr.write(j);
144      delay(20);
145    }
146  }
147}
148
149//////////////////////////////////////////////////////////////////////////////////////////////
150//Function
151  for inverse kinematics for sArm
152// Move end efactor to corsponting (X,Y,Z)
153void
154  ikMoveTo(double x, double y, double z) {
155  double t3 = degrees(atan2(y , x));
156
157  double t2 = degrees(acos((pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(l1, 2) - pow(l2,
158  2)) / (2 * l1 * l2)));
159  double t1 = degrees(atan(z / (sqrt(pow(x, 2) + pow(y,
160  2)))) + atan(l2 * sin(radians(t2)) / (l1 + l2 * cos(radians(t2)))));
161
162  mvsl((int(180
163  - (t2 - (t1 - 30)))), 5);
164  mvsr((int(130 - t1)), 5);
165  mvb((int(90 + t3)),
166  5);
167}
168
169////////////////////////////////////////////////////////////////////////////////////////////////
170  Draw a stright line that connect given two points.
171void drawLine(int a, int b,
172  int c, int p, int q, int r) {
173  if (a < p) {
174    for (int t = a; t <= p; t++)
175  {
176      double x = t;
177      double y = (((x - a) * (q - b)) / (p - a)) + b;
178
179      double z = (((x - a) * (r - c)) / (p - a)) + c;
180      ikMoveTo(x, y, z);
181
182      delay(10);
183    }
184  }
185  if (a > p) {
186    for (int t = a; t >= p;
187  t--) {
188      double x = t;
189      double y = (((x - a) * (q - b)) / (p - a))
190  + b;
191      double z = (((x - a) * (r - c)) / (p - a)) + c;
192      ikMoveTo(x,
193  y, z);
194      delay(10);
195    }
196  }
197  if (a == p) {
198    if (b < q)
199  {
200      for (int t = b; t <= q; t++) {
201        double y = t;
202        double
203  x = (((y - b) * (p - a)) / (q - b)) + a;
204        double z = (((x - a) * (r -
205  c)) / (p - a)) + c;
206        ikMoveTo(x, y, z);
207        delay(10);
208      }
209
210    }
211    if (b > q) {
212      for (int t = b; t >= q; t--) {
213        double
214  y = t;
215        double x = (((y - b) * (p - a)) / (q - b)) + a;
216        double
217  z = (((x - a) * (r - c)) / (p - a)) + c;
218        ikMoveTo(x, y, z);
219        delay(10);
220
221      }
222    }
223    if (b == q) {
224      if (c < r) {
225        for (int
226  t = c; t <= r; t++) {
227          double z = t;
228          double y = (((z -
229  c) * (q - b)) / (r - c)) + b;
230          double x = (((z - c) * (p - a)) / (r
231  - c)) + a;
232
233          ikMoveTo(x, y, z);
234          delay(10);
235        }
236
237      }
238      if (c > r) {
239        for (int t = c; t >= r; t--) {
240          double
241  z = t;
242          double y = (((z - c) * (q - b)) / (r - c)) + b;
243          double
244  x = (((z - c) * (p - a)) / (r - c)) + a;
245
246          ikMoveTo(x, y, z);
247
248          delay(10);
249        }
250      }
251      if (c == r) {
252        ikMoveTo(a,
253  b, c);
254      }
255    }
256  }
257}
258//////////////////////////////////////////////////////////////////////////////////////////////
259//
260  Draw a triangle that connect given three points in XYZ space.
261void drawTri(int
262  a, int b, int c, int p, int q, int r, int x, int y, int z) {
263  drawLine(a, b,
264  c, p, q, r);
265  delay(500);
266  drawLine(p, q, r, x, y, z);
267  delay(500);
268
269  drawLine(x, y, z, a, b, c);
270  delay(500);
271}
272//////////////////////////////////////////////////////////////////////////////////////////////
273void
274  loop() {
275  // program for a sample task, sArm do this again and again
276  //
277  you can program the sArm with these functions easily
278
279  drawTri(10, 0, 5,
280  17, 0, 13, 17, 0, 0);
281  delay(1000);
282  ikMoveTo(10, 1, 10);
283  delay(1000);
284
285  drawLine(10, 0, 0, 10, 0, 9);
286  delay(10);
287  grf(1);
288  delay(1000);
289
290  drawLine(10, 0, 9, 10, 0, 0);
291  delay(10);
292  grf(0);
293  delay(1000);
294}
295

#include <Servo.h>
#include <math.h>

Servo sl, sr, base, gr;
  // sl - left Servo, sr - right Servo, base - base Servo gr - gripper servo
double
  l1 = 8, l2 = 8; // measurements of sArm

/////////////////////////////////////////////////////////////////////////////////////////////
void
  setup() {
  //attach the servos
  sl.attach(9);
  sr.attach(3);
  base.attach(10);

  gr.attach(5 , 544, 853);

  homepos();
}

////////////////////////////////////////////////////////////////////////////////////////////////Initial
  sArm position.
void homepos() {
  mvsl(180, 20);
  delay(500);
  mvsr(30,
  20);
  delay(500);
  mvb(90, 20);
  delay(500);
  gr.write(30);

  delay(1000);
}

////////////////////////////////////////////////////////////////////////////////////////////////
  We use for loops so we can control the speed of the servo
void mvsl( int p, int
  d) { // to move the left servo
  //  p - end position d - delay
  // If previous
  position is bigger then current position
  int pp = sl.read(); // set previous
  position
  if (pp > p) {
    for ( int j = pp; j >= p; j--) {   // Run servo
  down
      sl.write(j);
      delay(d);    // defines the speed at which the
  servo rotates
    }
  }
  // If previous position is smaller then current
  position
  if (pp < p) {
    for ( int j = pp; j <= p; j++) {   // Run servo
  up
      sl.write(j);
      delay(d);
    }
  }
  if (pp == p) {

    delay(2);
  }
}
void mvsr( int p, int d) {// to move the right servo

  //  p - end possion d - delay
  // If previous position is bigger then current
  position
  int pp = sr.read(); // set previous position
  if (pp > p) {

    for ( int j = pp; j >= p; j--) {   // Run servo down
      sr.write(j);

      delay(d);    // defines the speed at which the servo rotates
    }

  }
  // If previous position is smaller then current position
  if (pp <
  p) {
    for ( int j = pp; j <= p; j++) {   // Run servo up
      sr.write(j);

      delay(d);
    }
  }
  if (pp == p) {
    delay(2);
  }
}
void
  mvb( int p, int d) {// to move the base servo
  //  p - end possion d - delay

  // If previous position is bigger then current position
  int pp = base.read();
  // set previous position
  if (pp > p) {
    for ( int j = pp; j >= p; j--)
  {   // Run servo down
      base.write(j);
      delay(d);    // defines the
  speed at which the servo rotates
    }
  }
  // If previous position is
  smaller then current position
  if (pp < p) {
    for ( int j = pp; j <= p;
  j++) {   // Run servo up
      base.write(j);
      delay(d);
    }

  }
  if (pp == p) {
    delay(2);
  }
}

//open or closs the
  gripper
//  1 - open 0 - close
void grf( int p) {
  int pp = gr.read();
  // set previous position
  if (p == 1) {
    for ( int j = pp; j >= 5; j--)
  {
      gr.write(j);
      delay(20);    // defines the speed at which the
  servo rotates
    }
  }
  if (p == 0) {
    for ( int j = pp; j <= 30;
  j++) {
      gr.write(j);
      delay(20);
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
//Function
  for inverse kinematics for sArm
// Move end efactor to corsponting (X,Y,Z)
void
  ikMoveTo(double x, double y, double z) {
  double t3 = degrees(atan2(y , x));

  double t2 = degrees(acos((pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(l1, 2) - pow(l2,
  2)) / (2 * l1 * l2)));
  double t1 = degrees(atan(z / (sqrt(pow(x, 2) + pow(y,
  2)))) + atan(l2 * sin(radians(t2)) / (l1 + l2 * cos(radians(t2)))));

  mvsl((int(180
  - (t2 - (t1 - 30)))), 5);
  mvsr((int(130 - t1)), 5);
  mvb((int(90 + t3)),
  5);
}

////////////////////////////////////////////////////////////////////////////////////////////////
  Draw a stright line that connect given two points.
void drawLine(int a, int b,
  int c, int p, int q, int r) {
  if (a < p) {
    for (int t = a; t <= p; t++)
  {
      double x = t;
      double y = (((x - a) * (q - b)) / (p - a)) + b;

      double z = (((x - a) * (r - c)) / (p - a)) + c;
      ikMoveTo(x, y, z);

      delay(10);
    }
  }
  if (a > p) {
    for (int t = a; t >= p;
  t--) {
      double x = t;
      double y = (((x - a) * (q - b)) / (p - a))
  + b;
      double z = (((x - a) * (r - c)) / (p - a)) + c;
      ikMoveTo(x,
  y, z);
      delay(10);
    }
  }
  if (a == p) {
    if (b < q)
  {
      for (int t = b; t <= q; t++) {
        double y = t;
        double
  x = (((y - b) * (p - a)) / (q - b)) + a;
        double z = (((x - a) * (r -
  c)) / (p - a)) + c;
        ikMoveTo(x, y, z);
        delay(10);
      }

    }
    if (b > q) {
      for (int t = b; t >= q; t--) {
        double
  y = t;
        double x = (((y - b) * (p - a)) / (q - b)) + a;
        double
  z = (((x - a) * (r - c)) / (p - a)) + c;
        ikMoveTo(x, y, z);
        delay(10);

      }
    }
    if (b == q) {
      if (c < r) {
        for (int
  t = c; t <= r; t++) {
          double z = t;
          double y = (((z -
  c) * (q - b)) / (r - c)) + b;
          double x = (((z - c) * (p - a)) / (r
  - c)) + a;

          ikMoveTo(x, y, z);
          delay(10);
        }

      }
      if (c > r) {
        for (int t = c; t >= r; t--) {
          double
  z = t;
          double y = (((z - c) * (q - b)) / (r - c)) + b;
          double
  x = (((z - c) * (p - a)) / (r - c)) + a;

          ikMoveTo(x, y, z);

          delay(10);
        }
      }
      if (c == r) {
        ikMoveTo(a,
  b, c);
      }
    }
  }
}
//////////////////////////////////////////////////////////////////////////////////////////////
//
  Draw a triangle that connect given three points in XYZ space.
void drawTri(int
  a, int b, int c, int p, int q, int r, int x, int y, int z) {
  drawLine(a, b,
  c, p, q, r);
  delay(500);
  drawLine(p, q, r, x, y, z);
  delay(500);

  drawLine(x, y, z, a, b, c);
  delay(500);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void
  loop() {
  // program for a sample task, sArm do this again and again
  //
  you can program the sArm with these functions easily

  drawTri(10, 0, 5,
  17, 0, 13, 17, 0, 0);
  delay(1000);
  ikMoveTo(10, 1, 10);
  delay(1000);

  drawLine(10, 0, 0, 10, 0, 9);
  delay(10);
  grf(1);
  delay(1000);

  drawLine(10, 0, 9, 10, 0, 0);
  delay(10);
  grf(0);
  delay(1000);
}

Processing Code

java

1import processing.serial.*;
2import controlP5.*;
3import cc.arduino.*;
4
5Arduino  arduino;
6ControlP5 cp5;
7
8float x=113, y = 0, z=95, x1=113, y1, t1, t2,  t3;
9float  l1=180, l2=200, l3=50; // sArm mesurements
10int k=0, k1=0; // index  for seaved poses
11float sliderX=x, sliderY=y, sliderZ=z, sliderG;
12// arraies  for save pos
13float[] posX = new float[10000];
14float[] posY = new float[10000];
15float[]  posZ = new float[10000];
16int rs=0; // run status
17
18void setup() {
19  size(1440,  840);
20
21  cp5 = new ControlP5(this);
22
23  println(Arduino.list());
24  //Select port from the listed array.
25  //replace [0] to [1],[2]...for selecting  a usable open port.
26  arduino = new Arduino(this, Arduino.list()[0], 57600);
27  
28  arduino.pinMode(9, Arduino.SERVO); // left servo
29  arduino.pinMode(3,  Arduino.SERVO); // right servo
30  arduino.pinMode(10, Arduino.SERVO); // base  servo
31  arduino.pinMode(5, Arduino.SERVO); // gripper
32
33  PFont pfont =  createFont("Arial", 225, true); // use true/false for smooth/no-smooth
34  ControlFont  font = new ControlFont(pfont, 20);
35  ControlFont font2 = new ControlFont(pfont,  25);
36
37  //X controls
38  cp5.addSlider("sliderX")
39    .setPosition(600,  height/2+100)
40    .setSize(270, 30)
41    .setRange(0, l1+l2-10) // Slider range,  corresponds to Joint 1 or theta1 angle that the robot can move to
42    .setColorLabel(225)
43    .setCaptionLabel("X")
44    .setFont(font)
45    ;
46  cp5.getController("sliderX").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);
47
48  //Y controls
49  cp5.addSlider("sliderY")
50    .setPosition(600, height/2+200)
51    .setSize(270, 30)
52    .setRange(-(l1+l2-50),  l1+l2-50) // Slider range, corresponds to Joint 1 or theta1 angle that the robot  can move to
53    .setColorLabel(225)
54    .setCaptionLabel("Y")
55    .setFont(font)
56    ;
57  cp5.getController("sliderY").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);
58
59  //Z controls
60  cp5.addSlider("sliderZ")
61    .setPosition(600, height/2+300)
62    .setSize(270, 30)
63    .setRange(-160,  180) // Slider range, corresponds to Joint 1 or theta1 angle that the robot can  move to
64    .setColorLabel(225)
65    .setCaptionLabel("Z")
66    .setFont(font)
67    ;
68  cp5.getController("sliderZ").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);
69
70  cp5.addSlider("sliderG")
71    .setPosition(950,  height/2+300)
72    .setSize(190, 30)
73    .setRange(0, 100)
74    .setColorLabel(225)
75    .setFont(font)
76    .setCaptionLabel("Gripper")
77    .setNumberOfTickMarks(5)
78    ;
79  cp5.getController("sliderG").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);
80
81  cp5.addButton("savePosition")
82    .setPosition(950, 520)
83    .setSize(215, 50)
84    .setFont(font2)
85    .setCaptionLabel("SAVE  POSITION")
86    ;
87
88  cp5.addButton("run")
89    .setPosition(1205, 520)
90    .setSize(215, 50)
91    .setFont(font2)
92    .setCaptionLabel("RUN PROGRAM")
93    ;
94
95  cp5.addButton("clearSteps")
96    .setPosition(1270, 700)
97    .setSize(135, 40)
98    .setFont(font)
99    .setCaptionLabel("(CLEAR)")
100    ;
101}
102
103void draw() {
104  background(20);
105
106  PVector m = new PVector(mouseX,  mouseY); // position vector of mouse
107  PVector c = new PVector(200, height/2);  //position vector of new center
108
109  pushMatrix();
110  translate(200, height/2);
111  PVector c1 = new PVector(800, 0);
112  m.sub(c);
113
114  if (k1>=k) {
115    k1=0;
116  }
117
118  if (rs==1) { // if run butten is pressed run the seaved possions
119    x=posX[k1];
120    y=posY[k1];
121    z=posZ[k1];
122    sliderX=x;
123    sliderZ=z;
124    sliderY=y;
125  }
126  k1++;
127
128  // to contrall the side view simulation
129  if (mousePressed == true && ((sq(200-mouseX) + sq(height/2-mouseY))<=sq(l1+l2))  && mouseX>200) {
130    x1 = m.x;
131    z = -m.y;
132    x = sqrt(sq(x1)-sq(y))  ;
133    sliderX=x;
134    sliderZ=z;
135  }
136
137  // to contrall the top view  simulation
138  if (mousePressed == true && ((sq(1000-mouseX) + sq(height/2 - mouseY))<=sq(l1+l2  -5)) && mouseY<height/2) {
139    y= m.x - 800;
140    x = -m.y;
141    x1 = sqrt(sq(y)+sq(m.y))  ;
142    sliderX=x;
143    sliderY=y;
144  }
145
146  x=sliderX;
147  y=sliderY;
148  z=sliderZ;
149
150  //press w or s to increes or decrees the Z
151  if (keyPressed)  {
152    if (key == 'w' || key == 'W') {
153      z++;
154    }
155    if (key ==  's' || key == 'S') {
156      z--;
157    }
158  }
159
160  cp5.getController("sliderX").setValue(x);
161  cp5.getController("sliderY").setValue(y);
162  cp5.getController("sliderZ").setValue(z);
163
164  //Inverse kinematics based on vectos.
165  t3= atan2(y, x);
166  t2= acos((sq(x)  +sq(y)+sq(z)-sq(l1)-sq(l2))/(2*l1*l2));
167  t1= (atan2(z, ( sqrt(sq(x)+sq(y))))  + atan2(l2*sin(t2), (l1+l2*cos(t2))));
168
169  PVector a = new PVector(l1*cos(-t1),  l1*sin(-t1));
170  PVector b = new PVector();
171  PVector p = new PVector(y, x);
172  p.sub(c1);
173  m.set(x1, -z);
174  b=PVector.sub(a, m);
175
176  float t4 = b.heading();
177
178  stroke(0, 250, 0);
179  strokeWeight(30);
180  line(a.x, a.y, a.x+l3*cos(t4),  a.y+l3*sin(t4));
181  b.mult(-1);
182  float t5 = b.heading();
183  line(a.x, a.y,  a.x+l2*cos(t5), a.y+l2*sin(t5));
184  stroke(255, 0, 200);
185  strokeWeight(20);
186  line(l3*cos(t4), l3*sin(t4), a.x+l3*cos(t4), a.y+l3*sin(t4));  
187  stroke(255);
188  line(0, 0, l3*cos(t4), l3*sin(t4));
189  stroke(255, 0, 0);
190  strokeWeight(40);
191  line(0, 0, a.x, a.y);
192  stroke(10);
193  ellipse(0, 0, 10, 10);
194
195  for  (int i=160; i<=2*(l1+l2 - 10); i=i+40) {
196    stroke(255);
197    strokeWeight(2);
198    noFill();
199    arc(800, 0, i, i, PI, 2*PI);
200  }
201
202  if  (((sq(p.x+800)  + sq(p.y))<=sq(l1+l2-5)) && -p.y<0) {
203    stroke(0, 0, 255);
204    strokeWeight(15);
205    line(800, 0, p.x + 1600, -p.y);
206    ellipse(p.x +1600, -p.y, 10, 10);
207  }
208
209  PVector l = new PVector(1, 0);
210  l.mult(-1);
211  float al = PVector.angleBetween(l,  b);
212  //l.mult(-1);
213  l.rotate(PI/2-radians(50));
214  float ar = PVector.angleBetween(l,  a);
215
216  // move the servos to the positions.
217  arduino.servoWrite(9, int(degrees(al))  );
218  arduino.servoWrite(3, int(degrees(ar)) );
219  arduino.servoWrite(10, int(90  +degrees(t3)) );
220
221  //print the texts on the GUI
222  textSize(18);
223  textAlign(CENTER);
224  text("X", -120, 270);
225  text("Y", -70, 270);
226  text("Z", -30, 270);
227  text(int(x), -120, 300);
228  text(int(y), -70, 300);
229  text(int(z), -30, 300);
230  text("leftServo Angle", 80, 270);
231  text(int(degrees(al)), 200, 270);
232  text("rightServo Angle", 80, 300);
233  text(int(degrees(ar)), 200, 300);
234  textSize(15);
235  textAlign(LEFT);
236  text("press w or s to increes or decrees  the Z", -130, 340);
237  text("preaa R key and move to recored", -130, 360);
238  textSize(60);
239  text("sArm manipulator", 130, -365);
240  popMatrix();
241//preaa  R key and move to recored
242  if (keyPressed) {
243    if (key == 'r' || key ==  'R') {
244      posX[k]= x;
245      posY[k]=y;
246      posZ[k]=z;
247      k++;
248      cp5.getController("savePosition").setCaptionLabel("RECODING");
249      cp5.getController("savePosition").setColorLabel(#e74c3c);
250    }
251  } else {
252    cp5.getController("savePosition").setCaptionLabel("SAVE  POSITION");
253    cp5.getController("savePosition").setColorLabel(255);
254  }
255}
256// seave position with butten
257public void savePosition() {
258  posX[k]=  x;
259  posY[k]=y;
260  posZ[k]=z;
261  k++;
262}
263// Play the seaved positions
264public  void run() {
265  if (rs == 0) {
266    cp5.getController("run").setCaptionLabel("STOP");
267    cp5.getController("run").setColorLabel(#e74c3c);
268
269    rs = 1;
270  }  else if (rs == 1) {
271    rs = 0;
272    cp5.getController("run").setCaptionLabel("RUN  PROGRAM");
273    cp5.getController("run").setColorLabel(255);
274  }
275}
276//  clear the seaved poses
277public void clearSteps() {
278  for (int i =0; i<k; i++)  {
279    posX[i]=posX[k];
280    posY[i]=posY[k];
281    posZ[i]=posZ[k];
282  }
283  k=0;
284  posX[k]= 113;
285  posY[k]=0;
286  posZ[k]=95;
287}
288

import processing.serial.*;
import controlP5.*;
import cc.arduino.*;

Arduino  arduino;
ControlP5 cp5;

float x=113, y = 0, z=95, x1=113, y1, t1, t2,  t3;
float  l1=180, l2=200, l3=50; // sArm mesurements
int k=0, k1=0; // index  for seaved poses
float sliderX=x, sliderY=y, sliderZ=z, sliderG;
// arraies  for save pos
float[] posX = new float[10000];
float[] posY = new float[10000];
float[]  posZ = new float[10000];
int rs=0; // run status

void setup() {
  size(1440,  840);

  cp5 = new ControlP5(this);

  println(Arduino.list());
  //Select port from the listed array.
  //replace [0] to [1],[2]...for selecting  a usable open port.
  arduino = new Arduino(this, Arduino.list()[0], 57600);
  
  arduino.pinMode(9, Arduino.SERVO); // left servo
  arduino.pinMode(3,  Arduino.SERVO); // right servo
  arduino.pinMode(10, Arduino.SERVO); // base  servo
  arduino.pinMode(5, Arduino.SERVO); // gripper

  PFont pfont =  createFont("Arial", 225, true); // use true/false for smooth/no-smooth
  ControlFont  font = new ControlFont(pfont, 20);
  ControlFont font2 = new ControlFont(pfont,  25);

  //X controls
  cp5.addSlider("sliderX")
    .setPosition(600,  height/2+100)
    .setSize(270, 30)
    .setRange(0, l1+l2-10) // Slider range,  corresponds to Joint 1 or theta1 angle that the robot can move to
    .setColorLabel(225)
    .setCaptionLabel("X")
    .setFont(font)
    ;
  cp5.getController("sliderX").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);

  //Y controls
  cp5.addSlider("sliderY")
    .setPosition(600, height/2+200)
    .setSize(270, 30)
    .setRange(-(l1+l2-50),  l1+l2-50) // Slider range, corresponds to Joint 1 or theta1 angle that the robot  can move to
    .setColorLabel(225)
    .setCaptionLabel("Y")
    .setFont(font)
    ;
  cp5.getController("sliderY").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);

  //Z controls
  cp5.addSlider("sliderZ")
    .setPosition(600, height/2+300)
    .setSize(270, 30)
    .setRange(-160,  180) // Slider range, corresponds to Joint 1 or theta1 angle that the robot can  move to
    .setColorLabel(225)
    .setCaptionLabel("Z")
    .setFont(font)
    ;
  cp5.getController("sliderZ").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);

  cp5.addSlider("sliderG")
    .setPosition(950,  height/2+300)
    .setSize(190, 30)
    .setRange(0, 100)
    .setColorLabel(225)
    .setFont(font)
    .setCaptionLabel("Gripper")
    .setNumberOfTickMarks(5)
    ;
  cp5.getController("sliderG").getCaptionLabel().align(ControlP5.LEFT,  ControlP5.TOP_OUTSIDE).setPaddingX(0);

  cp5.addButton("savePosition")
    .setPosition(950, 520)
    .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("SAVE  POSITION")
    ;

  cp5.addButton("run")
    .setPosition(1205, 520)
    .setSize(215, 50)
    .setFont(font2)
    .setCaptionLabel("RUN PROGRAM")
    ;

  cp5.addButton("clearSteps")
    .setPosition(1270, 700)
    .setSize(135, 40)
    .setFont(font)
    .setCaptionLabel("(CLEAR)")
    ;
}

void draw() {
  background(20);

  PVector m = new PVector(mouseX,  mouseY); // position vector of mouse
  PVector c = new PVector(200, height/2);  //position vector of new center

  pushMatrix();
  translate(200, height/2);
  PVector c1 = new PVector(800, 0);
  m.sub(c);

  if (k1>=k) {
    k1=0;
  }

  if (rs==1) { // if run butten is pressed run the seaved possions
    x=posX[k1];
    y=posY[k1];
    z=posZ[k1];
    sliderX=x;
    sliderZ=z;
    sliderY=y;
  }
  k1++;

  // to contrall the side view simulation
  if (mousePressed == true && ((sq(200-mouseX) + sq(height/2-mouseY))<=sq(l1+l2))  && mouseX>200) {
    x1 = m.x;
    z = -m.y;
    x = sqrt(sq(x1)-sq(y))  ;
    sliderX=x;
    sliderZ=z;
  }

  // to contrall the top view  simulation
  if (mousePressed == true && ((sq(1000-mouseX) + sq(height/2 - mouseY))<=sq(l1+l2  -5)) && mouseY<height/2) {
    y= m.x - 800;
    x = -m.y;
    x1 = sqrt(sq(y)+sq(m.y))  ;
    sliderX=x;
    sliderY=y;
  }

  x=sliderX;
  y=sliderY;
  z=sliderZ;

  //press w or s to increes or decrees the Z
  if (keyPressed)  {
    if (key == 'w' || key == 'W') {
      z++;
    }
    if (key ==  's' || key == 'S') {
      z--;
    }
  }

  cp5.getController("sliderX").setValue(x);
  cp5.getController("sliderY").setValue(y);
  cp5.getController("sliderZ").setValue(z);

  //Inverse kinematics based on vectos.
  t3= atan2(y, x);
  t2= acos((sq(x)  +sq(y)+sq(z)-sq(l1)-sq(l2))/(2*l1*l2));
  t1= (atan2(z, ( sqrt(sq(x)+sq(y))))  + atan2(l2*sin(t2), (l1+l2*cos(t2))));

  PVector a = new PVector(l1*cos(-t1),  l1*sin(-t1));
  PVector b = new PVector();
  PVector p = new PVector(y, x);
  p.sub(c1);
  m.set(x1, -z);
  b=PVector.sub(a, m);

  float t4 = b.heading();

  stroke(0, 250, 0);
  strokeWeight(30);
  line(a.x, a.y, a.x+l3*cos(t4),  a.y+l3*sin(t4));
  b.mult(-1);
  float t5 = b.heading();
  line(a.x, a.y,  a.x+l2*cos(t5), a.y+l2*sin(t5));
  stroke(255, 0, 200);
  strokeWeight(20);
  line(l3*cos(t4), l3*sin(t4), a.x+l3*cos(t4), a.y+l3*sin(t4));  
  stroke(255);
  line(0, 0, l3*cos(t4), l3*sin(t4));
  stroke(255, 0, 0);
  strokeWeight(40);
  line(0, 0, a.x, a.y);
  stroke(10);
  ellipse(0, 0, 10, 10);

  for  (int i=160; i<=2*(l1+l2 - 10); i=i+40) {
    stroke(255);
    strokeWeight(2);
    noFill();
    arc(800, 0, i, i, PI, 2*PI);
  }

  if  (((sq(p.x+800)  + sq(p.y))<=sq(l1+l2-5)) && -p.y<0) {
    stroke(0, 0, 255);
    strokeWeight(15);
    line(800, 0, p.x + 1600, -p.y);
    ellipse(p.x +1600, -p.y, 10, 10);
  }

  PVector l = new PVector(1, 0);
  l.mult(-1);
  float al = PVector.angleBetween(l,  b);
  //l.mult(-1);
  l.rotate(PI/2-radians(50));
  float ar = PVector.angleBetween(l,  a);

  // move the servos to the positions.
  arduino.servoWrite(9, int(degrees(al))  );
  arduino.servoWrite(3, int(degrees(ar)) );
  arduino.servoWrite(10, int(90  +degrees(t3)) );

  //print the texts on the GUI
  textSize(18);
  textAlign(CENTER);
  text("X", -120, 270);
  text("Y", -70, 270);
  text("Z", -30, 270);
  text(int(x), -120, 300);
  text(int(y), -70, 300);
  text(int(z), -30, 300);
  text("leftServo Angle", 80, 270);
  text(int(degrees(al)), 200, 270);
  text("rightServo Angle", 80, 300);
  text(int(degrees(ar)), 200, 300);
  textSize(15);
  textAlign(LEFT);
  text("press w or s to increes or decrees  the Z", -130, 340);
  text("preaa R key and move to recored", -130, 360);
  textSize(60);
  text("sArm manipulator", 130, -365);
  popMatrix();
//preaa  R key and move to recored
  if (keyPressed) {
    if (key == 'r' || key ==  'R') {
      posX[k]= x;
      posY[k]=y;
      posZ[k]=z;
      k++;
      cp5.getController("savePosition").setCaptionLabel("RECODING");
      cp5.getController("savePosition").setColorLabel(#e74c3c);
    }
  } else {
    cp5.getController("savePosition").setCaptionLabel("SAVE  POSITION");
    cp5.getController("savePosition").setColorLabel(255);
  }
}
// seave position with butten
public void savePosition() {
  posX[k]=  x;
  posY[k]=y;
  posZ[k]=z;
  k++;
}
// Play the seaved positions
public  void run() {
  if (rs == 0) {
    cp5.getController("run").setCaptionLabel("STOP");
    cp5.getController("run").setColorLabel(#e74c3c);

    rs = 1;
  }  else if (rs == 1) {
    rs = 0;
    cp5.getController("run").setCaptionLabel("RUN  PROGRAM");
    cp5.getController("run").setColorLabel(255);
  }
}
//  clear the seaved poses
public void clearSteps() {
  for (int i =0; i<k; i++)  {
    posX[i]=posX[k];
    posY[i]=posY[k];
    posZ[i]=posZ[k];
  }
  k=0;
  posX[k]= 113;
  posY[k]=0;
  posZ[k]=95;
}

Downloadable files

Circuit

Comments

Only logged in users can leave comments

sathursan_s

0 Followers

•

0 Projects

Intro

Components and supplies

Apps and platforms

Project description

Code

Downloadable files