DIY Industrial Pick-&-Place Robot | Building a 3D Hybrid Delta Robot | Arduino | Servo Motor|

1#include <Servo.h>
2
3Servo m1,m2,m3;    
4
5float l1, l2;
6float tl = 9.5;
7float l3 = 4.7;
8float l4 = 5.5;
9
10float h1x, h1y, h1z;
11float h2x, h2y, h2z;
12float h3x, h3y, h3z;
13
14float toRadians(float deg) { return deg * PI / 180.0; }
15float toDegrees(float rad) { return rad * 180.0 / PI; }
16
17void setup() {
18  m1.attach(5); // 0 degreee
19  m2.attach(6); // 120 degree 
20  m3.attach(11); // 240 degree
21  Serial.begin(9600);
22  points();
23}
24
25bool motors_move(float theta1, float theta2, float theta3){
26  
27  if ( (theta1>=10 and theta1<=100) && (theta2>=10 and theta2<=100) && (theta3>=10 and theta3<=100) ){
28    theta1 = 180 - theta1;
29    theta2 = 180 - theta2;
30    theta3 = 180 - theta3;
31
32    m1.write(theta1);
33    m2.write(theta2);
34    m3.write(theta3);
35    return true;
36  }else{
37    return false;
38  }
39}
40
41bool calculate(float x, float y, float z){
42  float theta1, theta2, theta3;
43  theta1 = calculate_angle(x,y,z, h1x,h1y,h1z);
44  theta2 = calculate_angle(x,y,z, h2x,h2y,h2z);
45  theta3 = calculate_angle(x,y,z, h3x,h3y,h3z);
46
47  if (motors_move(theta1, theta2, theta3)){
48    return true;
49  }else{
50    return false;
51  }
52}
53
54void workspace(){
55  for(float i=-10; i<10;i+=1){
56    for(float j=-10; j<10;j+=1){
57      for(float k=-10; k<10;k+=1){
58        if (calculate(i,j,k)){
59          Serial.print(i); Serial.print(", "); Serial.print(j); Serial.print(", "); Serial.println(k);
60        } 
61      }
62    }
63  }
64}
65
66void loop() {
67   workspace();
68}