Arduino Controlled Delta Robot

It is about a delta robot that can grab and move objects. It is connected to a Raspberry Pi to play Tic Tac Toe using computer vision.

Jan 15, 2019

•

13410 views

•

5 respects

remote control

games

robots

Components and supplies

Servomotor

Computer Power Supply

MOSFET

Prototype Shield for Arduino Mega

LEDs, Resistors, Diodes, Swithes, Buttons, Potensiometers, etc.

Arduino Mega 2560

HC-06 Bluetooth Module

Polyfuse

Relay (generic)

LCD Screen Shield for Arduino UNO

Arduino UNO

Electromagnet

Tools and machines

Soldering iron (generic)

Apps and platforms

Arduino IDE

Arduino Delta Robot Controller

Project description

Code

Main Code

arduino

This is the code that runs on the Arduino Mega and gives life to the robot.

1  #include <EEPROM.h>
2  #include <Servo.h> // Servo library
3  
4  Servo servo_1; // create servo object to control a servo
5  Servo servo_2; //  a maximum of eight servo objects can be created
6  Servo servo_3;
7
8  const  int servo1[]PROGMEM =
9    {641,651,659,668,676,680,690,700,706,714,722,730,737,746,754,763,772,779,788,796,808,815,825,833,843,854,862,871,878,888,896,
10    903,910,919,932,943,951,960,969,976,986,996,1007,1016,1022,1032,1043,1050,1060,1071,1081,1089,1097,1108,1116,1127,1137,1147,
11    1158,1169,1180,1189,1198,1206,1216,1226,1236,1248,1258,1268,1274,1285,1300,1307,1317,1328,1338,1348,1357,1364,1374,1386,1395,
12    1406,1413,1421,1432,1443,1454,1463,1474,1483,1492,1498,1508,1520,1529,1538,1547,1557,1566,1578,1586,1596,1607,1615,1625,1631,
13    1639,1655,1664,1675,1683,1694,1703,1711,1726,1737,1746,1756,1765,1770,1780,1791,1801,1812,1821,1830,1839,1851,1861,1867,1877,
14    1887,1894,1902,1911,1924,1933,1940,1948,1956,1968,1977,1985,1994,2004,2011,2023,2030,2040,2049,2057,2065,2075,2084,2093,2103,
15    2109,2119,2129,2137,2146,2153,2161,2170,2180,2188,2199,2208,2220,2228,2234,2242,2252,2264,2271,2279,2288,2296,2305};
16  
17  const int servo2[]PROGMEM =
18    {587,594,600,608,612,620,627,633,641,651,659,662,672,680,688,695,703,713,721,730,738,744,752,761,768,777,783,791,802,809,819,
19    827,835,847,856,861,873,883,889,896,907,916,925,935,941,953,961,970,976,984,995,1003,1012,1024,1033,1043,1050,1059,1069,1077,
20    1086,1100,1105,1117,1128,1136,1146,1156,1165,1178,1185,1194,1206,1213,1225,1232,1241,1251,1261,1268,1275,1288,1298,1307,1321,
21    1330,1335,1348,1356,1369,1373,1384,1396,1408,1418,1426,1437,1449,1460,1469,1476,1488,1498,1507,1520,1531,1540,1549,1560,1572,
22    1583,1593,1601,1614,1623,1637,1645,1656,1672,1682,1691,1700,1710,1717,1729,1743,1754,1764,1776,1789,1800,1812,1818,1829,1841,
23    1851,1862,1869,1882,1888,1898,1911,1920,1931,1943,1954,1961,1970,1976,1988,2000,2008,2016,2029,2038,2047,2056,2064,2080,2090,
24    2099,2109,2120,2132,2139,2147,2159,2170,2178,2187,2198,2205,2215,2228,2239,2249,2256,2265,2276,2282,2296};
25    
26  const int servo3[]PROGMEM =
27    {587,596,603,609,616,626,632,640,648,657,665,671,679,688,696,701,708,716,722,729,739,749,755,764,771,780,787,794,806,814,820,
28    827,834,843,852,861,870,878,888,896,904,911,923,931,941,952,959,968,980,987,995,1003,1014,1023,1034,1042,1052,1060,1071,1080,
29    1088,1096,1108,1118,1128,1138,1146,1158,1167,1175,1185,1196,1206,1215,1224,1235,1243,1254,1263,1272,1284,1294,1303,1315,1325,
30    1333,1346,1356,1365,1376,1386,1395,1402,1413,1423,1432,1440,1451,1460,1469,1477,1486,1496,1508,1518,1524,1534,1544,1554,1563,
31    1573,1584,1596,1603,1610,1621,1630,1637,1652,1661,1670,1680,1688,1698,1706,1719,1728,1738,1746,1757,1769,1779,1788,1793,1802,
32    1813,1821,1830,1843,1852,1862,1874,1882,1893,1902,1912,1920,1931,1938,1946,1957,1964,1972,1981,1991,1998,2010,2017,2027,2036,
33    2042,2053,2060,2070,2079,2086,2095,2102,2111,2119,2128,2137,2146,2153,2162,2169,2175,2184,2193,2202,2208};
34
35  
36  // robot geometry
37  const float e = 8.0;     // end effector
38  const  float f = 23.32;     // base
39  const float re = 35.0;
40  const float rf = 6.0;
41
42  // trigonometric constants
43  const float sqrt3 = sqrt(3.0);
44  const float  pi = 3.141592653;    // PI
45  const float sin120 = sqrt3/2.0;   
46  const float  cos120 = -0.5;        
47  const float tan60 = sqrt3;
48  const float sin30 =  0.5;
49  const float tan30 = 1.0/sqrt3;
50
51  const int offset_1 =111; // rf  are horizontal
52  const int offset_2 =110;
53  const int offset_3 =116;
54
55  const int em = 12; // Defines the pin which controls the electromagnet
56  const  int led = 8; // Defines the pin which controls the LED of the Delta Robot    
57    
58  const int pinForRemoteControl=52; // Low to control via external device
59  const int pinForStop=6;  // Low to stop
60  const int pinForLocalControl=48;  // Low to control delta Robot via potensiometers
61  const int pinToControlMagnet=5;  // Input Pin that defines magnet state when controlMode is 2. Low for On, High for  Off.
62  const int pinToControlLED=7; // Input Pin that defines LED state when  controlMode is 2. Low for On, High for Off.
63  const int pinToToggleInputMode=4;  // When controlMode is 2 defines whether input values are angles or coordinates.  
64                                       // Low for Angle, High for Coordinates
65  const int redLed=3;   // Pin for red led. Led is On at stop mode
66  const int  yellowLed=2;  // Pin for yellow led. Led is On at local control mode
67  const  int orangeLed=26; // Pin for orange led. Led is On when waiting to receive data  at remote control mode
68  const int greenLed=40;   // Pin for green led. Led is  On at remote control mode
69  const int led13=13;   // Pin for Arduino built in  LED 13. Led 13 is On when the position is valid and off otherwise or in Pause mode.
70  const int pinForXorAngle1Potensiometer=1; // Pin connected to the potensiometer  that defines X coordinate or Angle1
71  const int pinForYorAngle2Potensiometer=5;  // Pin connected to the potensiometer that defines Y coordinate or Angle2
72  const  int pinForZorAngle3Potensiometer=8; // Pin connected to the potensiometer that defines  Z coordinate or Angle3
73  const int pinForZoomPotensiometer=15; // Pin connected  to the potensiometer that defines Zoom factor for the other three
74
75    /*  Also built in led 13 is on when received coordinates are valid and off when bad  coordinates have been received at control mode 0 (remote)
76     * or at by the  user at control mode 2 when receiving coordinates. At any other cases (mode 1 or  mode 2 when receiving angles) led 13 is off.
77     */
78  
79  const float tableLevel=-34.77;  // The z coordinate of the table
80  float verticalLimit=tableLevel; // The lower  vertical limit that delta robot head is allowable to reach
81  int verticalFlag=0;  // If the given z coordinate is lower than vertical Limit then vertical flag=1  else =0.
82  
83  const unsigned long intervalBetweenButtonPress=1000; // For  button debouncing
84  unsigned long currentTime=0, previousTime=0;
85    
86  float t1;
87  float t2;
88  float t3;
89  int angle_1=0,angle_2=0,angle_3=0;  // Desired servo angles
90  int oldAngle1=0, oldAngle2=0, oldAngle3=0;  // Current  servo angles
91      
92  float x=0.0, y=0.0, z=-33.0;  // x,y,z coordinates
93  float multipl=1.0;
94  int electromagnet=0, ledState=0, magStatus=0, ledStatus=0;  // ...State= the desired condition (On or Off), ...Status= the current condition.
95  int retStat=0; // Return status of angles calculation. 0 If everything is OK,  -1, -2, -3 if one two or three angles are bad.
96  int controlMode=0; // Defines  who controls the servo. 0 for external serial device, 1 to stop (Pause mode) and  2 for potensiometers
97  int coord=HIGH;
98  int validPosition=0; // Equals 0  if the given x,y,z coordinates correspond to a valid position, negative if not and  1 when bad data is received.
99                       // If it is 2 then the given  z coordinate is less than vertical limit.
100
101    
102
103  int delta_calcAngleYZ(float  x0, float y0, float z0, float &theta)
104  {
105    float y1 = -0.5 * 0.57735 *  f; // f/2 * tg 30
106    //float y1 = yy1;
107    y0 -= 0.5 * 0.57735    * e;    //  shift center to edge
108    // z = a + b*y
109    float a = (x0*x0 + y0*y0 + z0*z0  +rf*rf - re*re - y1*y1)/(2*z0);
110    float b = (y1-y0)/z0;
111    // discriminant
112    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); 
113    if (d < 0) return -1; //  non-existing point
114    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing  outer point
115    float zj = a + b*yj;
116    theta = 180.0*atan(-zj/(y1 - yj))/pi  + ((yj>y1)?180.0:0.0);
117    if ((theta < -180) || (theta > 180))
118        return  -1;
119    return 0;
120  }
121
122  // inverse kinematics: (x0, y0, z0) -> (theta1,  theta2, theta3)
123  // returned status: 0=OK, negative=non-existing position, the  negative number is the number of wrong angles
124  int delta_calcInverse(float x0,  float y0, float z0, float &theta1, float &theta2, float &theta3)
125  {
126    theta1  = theta2 = theta3 = 0;
127    int stat1 = delta_calcAngleYZ(x0, y0, z0, theta1);
128    int stat2 = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0,  theta2);  // rotate coords to +120 deg
129    int stat3 = delta_calcAngleYZ(x0*cos120  - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
130    return stat1+stat2+stat3;
131  }
132
133              
134              
135    void setup()
136      { 
137        Serial.begin(9600);   // To output data  to Serial monitor or any other connected device.
138        Serial1.begin(9600);  // Communication with external device for remote control. Used for data input,  such as coordinates.
139        pinMode(led13, OUTPUT);
140        pinMode(em, OUTPUT);
141        pinMode(led, OUTPUT);
142
143        digitalWrite(led13,HIGH);
144        digitalWrite(em,LOW);
145        digitalWrite(led,LOW);
146
147        servo_1.attach(9,500,2500); // attaches  the servo on pin 9 to the servo object
148        servo_2.attach(10,400,2500); //  attaches the servo on pin 10 to the servo object
149        servo_3.attach(11,400,2500);  // attaches the servo on pin 11 to the servo object
150        
151         //all  servos horizontal (x,y,z)=(0,0,-31)
152        servo_1.writeMicroseconds(pgm_read_word_near(servo1+offset_1));
153        angle_1=offset_1;
154        servo_2.writeMicroseconds(pgm_read_word_near(servo2+offset_2));
155        angle_2=offset_2;
156        servo_3.writeMicroseconds(pgm_read_word_near(servo3+offset_3));
157        angle_3=offset_3;
158
159        delay(100);
160
161        pinMode(redLed,OUTPUT);
162        digitalWrite(redLed,HIGH);
163        delay (100);
164        pinMode(yellowLed,OUTPUT);
165        digitalWrite(yellowLed,HIGH);
166        delay (100);
167        pinMode(greenLed,OUTPUT);
168        digitalWrite(greenLed,HIGH);
169        delay(100);
170        pinMode(orangeLed,OUTPUT);
171        digitalWrite(orangeLed,HIGH);
172        delay(100);
173
174        pinMode(pinForRemoteControl,INPUT_PULLUP);
175        pinMode(pinForStop,INPUT_PULLUP);
176        pinMode(pinForLocalControl,INPUT_PULLUP);  
177        pinMode(pinToControlMagnet,INPUT_PULLUP);
178        pinMode(pinToControlLED,INPUT_PULLUP);
179        pinMode(pinToToggleInputMode,INPUT_PULLUP);            
180              
181        delay(500);
182
183         // If both switches are pressed during boot,  then store desired vertical limit offset to EEPROM
184        if (digitalRead(pinToControlMagnet)==LOW  && digitalRead(pinToToggleInputMode)==LOW)
185        {
186          int value=0;
187          unsigned long past=0;
188          do
189          {
190            if  (millis()-past>500)
191            {
192              digitalWrite(led13,!digitalRead(led13));  // Built in LED is flashing during procedure.
193              past=millis();
194            }
195            value=analogRead(pinForZoomPotensiometer)/4; // read  potensiometer and map from 0-1023 to 0-255
196            Serial.println("@!");
197            Serial.print("Table Level= ");
198            Serial.println(tableLevel+(float)value/200.0);
199            Serial.println('$');            
200          } while (digitalRead(pinToControlMagnet)==LOW  || digitalRead(pinToToggleInputMode)==LOW); // When both switches are released,  then save the result
201          EEPROM[3]=(byte)value;
202          verticalLimit=tableLevel+(float)value/200.0;
203        }
204
205        // If switch is pressed during boot then load vertical  limit offset from EEPROM
206        if (digitalRead(pinToControlLED)==LOW) 
207        {
208          int value=EEPROM[3];
209          unsigned long past=0;
210          do
211          {
212            if (millis()-past>500)
213            {
214              digitalWrite(led13,!digitalRead(led13));  // Built in LED is flashing  during procedure.
215              past=millis();
216            }
217            Serial.println("@!");
218            Serial.print("Table Level= ");
219            Serial.println(tableLevel+(float)value/200.0);
220            Serial.println('$');
221          } while (digitalRead(pinToControlLED)==LOW);
222          verticalLimit=tableLevel+(float)value/200.0;
223        }
224
225        digitalWrite(led13,HIGH);  // Led 13 is on because servos are in valid position (0,0,-31)
226       
227        digitalWrite(redLed,LOW);
228        delay(100);
229        digitalWrite(yellowLed,LOW);
230        delay(100);
231        digitalWrite(greenLed,LOW);
232        delay(100);
233        digitalWrite(orangeLed,LOW);
234        delay(100);
235        
236        
237        digitalWrite(led,HIGH); // Turn on Delta Robot's LED
238        ledStatus=1;
239        digitalWrite(greenLed,HIGH);  // Default mode is 0 (Remotely controlled)
240        Serial1.write('@'); // Informs external device that delta robot is ready  to receive data.
241             
242      }
243      
244
245  void loop()
246    {
247
248      if (digitalRead(pinForRemoteControl)==LOW) // If the Black button  is pressed then change cotrol mode to 0 (Remotely Controlled)
249      {
250        digitalWrite(led,HIGH);
251        ledStatus=HIGH;
252        
253        currentTime=millis();
254        //  If button pressed when already in controlMode 0, then send a @, but avoid sending  multiple @s.
255        if (controlMode==0&&currentTime-previousTime>intervalBetweenButtonPress)  Serial1.write('@'); 
256        previousTime=currentTime;
257        
258        controlMode=0;
259        digitalWrite(yellowLed,LOW);
260        digitalWrite(redLed,LOW);
261        digitalWrite(greenLed,HIGH);
262      }
263      
264      if (digitalRead(pinForStop)==LOW) // If stop (Red Button)  is pressed turn off magnet and go horizontal (Mode 1)
265      {
266        controlMode=1;
267        digitalWrite(yellowLed,LOW);        
268        digitalWrite(greenLed,LOW);
269        digitalWrite(orangeLed,LOW);
270        digitalWrite(redLed,HIGH);
271        digitalWrite(led13,LOW);  // There is no reason examining wheather the position is valid or not since it  is already known that it is valid
272          
273        digitalWrite(em,LOW);
274        magStatus=0; 
275                      
276        servo_1.writeMicroseconds(pgm_read_word_near(servo1+offset_1));
277        oldAngle1=offset_1;                 
278        servo_2.writeMicroseconds(pgm_read_word_near(servo2+offset_2));
279        oldAngle2=offset_2;                 
280        servo_3.writeMicroseconds(pgm_read_word_near(servo3+offset_3));
281        oldAngle3=offset_3;
282
283        digitalWrite(led,LOW);  // Also turn  off Delta Robot's LED
284        ledStatus=LOW;
285      }
286      
287      if  (digitalRead(pinForLocalControl)==LOW) // Locally controlled with potensiometers  (Mode 2)
288      {
289        controlMode=2;        
290        digitalWrite(redLed,LOW);
291        digitalWrite(greenLed,LOW);
292        digitalWrite(orangeLed,LOW);
293        digitalWrite(yellowLed,HIGH);
294      }
295
296      if (controlMode==0&&!Serial1.available())  digitalWrite(orangeLed,HIGH); // No data received yet
297      if (controlMode==0&&Serial1.available())   // If there is data in Serial buffer, ... 
298        {  
299         
300           digitalWrite(orangeLed,LOW);
301           
302           if (Serial1.find("@"))       // ..., then search for  start character,@, and read x,y,z,electromagnet
303            {            
304              x=(float)Serial1.parseInt()/100.0;
305              y=(float)Serial1.parseInt()/100.0;
306              z=(float)Serial1.parseInt()/100.0;
307              electromagnet=Serial1.parseInt();
308
309              if (z<verticalLimit)  // If the z coordinate is lower than the vertical  limit then z is equal to that limit
310              {
311                z=verticalLimit;
312                verticalFlag=1;
313              }
314              else verticalFlag=0;
315                       
316              if (Serial1.find("$"))    // If end character,$,is  found, then no data is missing. 
317                {
318                  if (electromagnet==0&&magStatus==1)  // If magnet should be off and is on, then turn it off
319                      {                        
320                        digitalWrite(em,LOW);
321                        magStatus=0;
322                      }
323                  if (electromagnet==1&&magStatus==0)  // If magnet should be on and is off, then turn it on
324                      {
325                        digitalWrite(em,HIGH);
326                        magStatus=1;                         
327                      }
328                  
329                   retStat=delta_calcInverse(x,y,z,t1,t2,t3);  // Calculate theta1, theta2 and theta3 angles
330                   angle_1=offset_1-round(t1);
331                   angle_2=offset_2-round(t2);
332                   angle_3=offset_3-round(t3);
333
334                   // If return Status is 0 (OK), all the angles are within the  limits (0 to 180), then write to the corresponding servos
335                   if  (retStat==0 && angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<= 180 && angle_3>=0  && angle_3<=180)
336                      {
337                         validPosition=0;     // If everything is OK then validPosition=0                    
338                         if  (oldAngle1!=angle_1)  // If the desired new angle (angle_1) is different from the  current angle (oldAngle1) then write it to the servo1
339                          {                            
340                            servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
341                            oldAngle1=angle_1;
342                          }
343                         if (oldAngle2!=angle_2)  // If the desired new angle (angle_2)  is different from the current angle (oldAngle2) then write it to the servo2
344                          {                            
345                            servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
346                            oldAngle2=angle_2;
347                          }
348                         if (oldAngle3!=angle_3)  // If the desired new angle (angle_3)  is different from the current angle (oldAngle3) then write it to the servo3
349                          {                            
350                            servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
351                            oldAngle3=angle_3;
352                          }                 
353                    }
354                  else validPosition=retStat==0?-4:retStat;  // If return Status==0(OK) but any of the above limitations is not met then validPosition=-4,  else =retStat
355                } // Endif for Serial.find("$")  
356                
357               else validPosition=1;   // If bad data has been received or data  is missing, then validPosition is 1 
358               if (validPosition==0 && verticalFlag==1)  validPosition=2; // If everything else is OK but vertical limit has been violated  then validPosition=2
359               digitalWrite(led13,validPosition==0?HIGH:LOW);
360               
361            } // Endif for Serial.find("@") 
362        
363          if (!Serial1.available()) Serial1.write('@'); // Send @ to ask for new  data        
364       } // Endif for control mode 0     
365
366      if (controlMode==2)   // Locally controlled with potensiometers
367      {
368        ledState=digitalRead(pinToControlLED);  // Inverse Logic
369        if (ledState==HIGH&&ledStatus==1)   // If Delta Robot  LED is on and the switch is open, then turn the LED off
370           {                        
371              digitalWrite(led,LOW);
372              ledStatus=0;
373           }
374        if (ledState==LOW&&ledStatus==0)    // If Delta Robot LED is off and the  switch is closed, then turn the LED on
375           {
376              digitalWrite(led,HIGH);
377              ledStatus=1;                         
378           }
379        
380        coord=digitalRead(pinToToggleInputMode);  // Coordinates or angles input
381        if (coord==HIGH)     // If switch is open, then read the x,y,z coordinates  from potensiometers and act accordingly
382        {
383          multipl=5.0+17.0*(float)analogRead(pinForZoomPotensiometer)/1023.0;   // Multiplier factor that defines the scale and range of x,y values that read  the potensiometers
384          x=(float)analogRead(pinForXorAngle1Potensiometer)*2.0*multipl/1023.0-multipl;  // Reads the x value and map it according to multiplier factor
385          y=(float)analogRead(pinForYorAngle2Potensiometer)*2.0*multipl/1023.0-multipl;  // Reads the y value and map it according to multiplier factor
386          z=(float)analogRead(pinForZorAngle3Potensiometer)*11.0/1023.0-40.0;   // Reads the z value and map without taking into account the multiplier factor
387
388          if (z<verticalLimit)  // If the z coordinate is lower than the vertical  limit then z is equal to that limit
389          {
390            z=verticalLimit;
391            verticalFlag=1;
392          }
393          else verticalFlag=0;
394          
395          retStat=delta_calcInverse(x,y,z,t1,t2,t3);  // Calculate  theta1, theta2 and theta3 angles and do as above
396          angle_1=offset_1-round(t1);
397          angle_2=offset_2-round(t2);
398          angle_3=offset_3-round(t3);
399          if (retStat==0 && angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<=180  && angle_3>=0 && angle_3<=180)
400              {
401                  validPosition=0;                  
402                  if (oldAngle1!=angle_1)
403                   {                            
404                       servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
405                       oldAngle1=angle_1;
406                   }
407                  if  (oldAngle2!=angle_2)
408                   {                            
409                       servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
410                       oldAngle2=angle_2;
411                   }
412                  if  (oldAngle3!=angle_3)
413                   {                            
414                       servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
415                       oldAngle3=angle_3;
416                   }                 
417              }
418              
419            else validPosition=retStat==0?-4:retStat;  // If return Status==0(OK) but any of the above limitations is not met then validPosition=-4,  else =retStat
420            if (validPosition==0 && verticalFlag==1) validPosition=2;  // If everything else is OK but vertical limit has been violated then validPosition=2
421            digitalWrite(led13,validPosition==0?HIGH:LOW);  
422             
423        }   // Endif coord High.
424
425        if (coord==LOW)   // If switch is  closed, then read the desired angles of the servos from potensiometers
426        {
427           digitalWrite(led13,LOW);   // There is no reason to be on. Every angle  between 0 to 180 can be achieved by servos.
428           angle_1=analogRead(pinForXorAngle1Potensiometer)*180.0/1023.0;
429           angle_2=analogRead(pinForYorAngle2Potensiometer)*180.0/1023.0;
430           angle_3=analogRead(pinForZorAngle3Potensiometer)*180.0/1023.0;
431           
432           if (angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<=180  && angle_3>=0 && angle_3<=180)
433              {
434                  if (oldAngle1!=angle_1)
435                   {                            
436                       servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
437                       oldAngle1=angle_1;
438                   }
439                  if  (oldAngle2!=angle_2)
440                   {                            
441                       servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
442                       oldAngle2=angle_2;
443                   }
444                  if  (oldAngle3!=angle_3)
445                   {                            
446                       servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
447                       oldAngle3=angle_3;
448                   }                   
449              }  
450        }   // Endif coord Low
451
452        electromagnet=digitalRead(pinToControlMagnet);  // Inverse Logic        
453        if (electromagnet==HIGH&&magStatus==1)  // If  magnet switch is open and magnet is on, then turn it off
454           {                        
455              digitalWrite(em,LOW);
456              magStatus=0;
457           }
458        if (electromagnet==LOW&&magStatus==0)   // If magnet switch is closed and  magnet is off, then turn it on
459           {
460              digitalWrite(em,HIGH);
461              magStatus=1;                         
462           }   
463      }  // Endif for control mode 2
464
465        // If there is enough space in buffer  then write in order to avoid delay in code execution.
466      if (Serial.availableForWrite()>62)
467         {
468            if (controlMode==2&&coord==LOW)
469               {
470                  Serial.println("@#");
471                  Serial.print("Servo1=  ");
472                  Serial.println(angle_1);
473                  Serial.print("Servo2=  ");
474                  Serial.println(angle_2);
475                  Serial.print("Servo3=  ");
476                  Serial.println(angle_3); 
477                  Serial.println('$');
478               }
479            else if (controlMode==1)
480               {
481                  Serial.println("@%");
482                  Serial.println("  Paused!");
483                  Serial.println('$');
484               }
485            else
486               {
487                  Serial.println('@');
488                  Serial.print("X=  ");
489                  Serial.println(x);
490                  Serial.print("Y=  ");
491                  Serial.println(y);
492                  Serial.print("Z=  ");
493                  Serial.println(z); 
494                  if (validPosition==0)  Serial.println(" OK");
495                  if (validPosition<0) Serial.println("  Bad Position!");
496                  if (validPosition==1) Serial.println(" Bad  Data!");
497                  if (validPosition==2) Serial.println(" z Limit!");
498                  Serial.print(validPosition);  
499                  Serial.println('$');
500               } 
501         }             
502  }
503  
504

  #include <EEPROM.h>
  #include <Servo.h> // Servo library
  
  Servo servo_1; // create servo object to control a servo
  Servo servo_2; //  a maximum of eight servo objects can be created
  Servo servo_3;

  const  int servo1[]PROGMEM =
    {641,651,659,668,676,680,690,700,706,714,722,730,737,746,754,763,772,779,788,796,808,815,825,833,843,854,862,871,878,888,896,
    903,910,919,932,943,951,960,969,976,986,996,1007,1016,1022,1032,1043,1050,1060,1071,1081,1089,1097,1108,1116,1127,1137,1147,
    1158,1169,1180,1189,1198,1206,1216,1226,1236,1248,1258,1268,1274,1285,1300,1307,1317,1328,1338,1348,1357,1364,1374,1386,1395,
    1406,1413,1421,1432,1443,1454,1463,1474,1483,1492,1498,1508,1520,1529,1538,1547,1557,1566,1578,1586,1596,1607,1615,1625,1631,
    1639,1655,1664,1675,1683,1694,1703,1711,1726,1737,1746,1756,1765,1770,1780,1791,1801,1812,1821,1830,1839,1851,1861,1867,1877,
    1887,1894,1902,1911,1924,1933,1940,1948,1956,1968,1977,1985,1994,2004,2011,2023,2030,2040,2049,2057,2065,2075,2084,2093,2103,
    2109,2119,2129,2137,2146,2153,2161,2170,2180,2188,2199,2208,2220,2228,2234,2242,2252,2264,2271,2279,2288,2296,2305};
  
  const int servo2[]PROGMEM =
    {587,594,600,608,612,620,627,633,641,651,659,662,672,680,688,695,703,713,721,730,738,744,752,761,768,777,783,791,802,809,819,
    827,835,847,856,861,873,883,889,896,907,916,925,935,941,953,961,970,976,984,995,1003,1012,1024,1033,1043,1050,1059,1069,1077,
    1086,1100,1105,1117,1128,1136,1146,1156,1165,1178,1185,1194,1206,1213,1225,1232,1241,1251,1261,1268,1275,1288,1298,1307,1321,
    1330,1335,1348,1356,1369,1373,1384,1396,1408,1418,1426,1437,1449,1460,1469,1476,1488,1498,1507,1520,1531,1540,1549,1560,1572,
    1583,1593,1601,1614,1623,1637,1645,1656,1672,1682,1691,1700,1710,1717,1729,1743,1754,1764,1776,1789,1800,1812,1818,1829,1841,
    1851,1862,1869,1882,1888,1898,1911,1920,1931,1943,1954,1961,1970,1976,1988,2000,2008,2016,2029,2038,2047,2056,2064,2080,2090,
    2099,2109,2120,2132,2139,2147,2159,2170,2178,2187,2198,2205,2215,2228,2239,2249,2256,2265,2276,2282,2296};
    
  const int servo3[]PROGMEM =
    {587,596,603,609,616,626,632,640,648,657,665,671,679,688,696,701,708,716,722,729,739,749,755,764,771,780,787,794,806,814,820,
    827,834,843,852,861,870,878,888,896,904,911,923,931,941,952,959,968,980,987,995,1003,1014,1023,1034,1042,1052,1060,1071,1080,
    1088,1096,1108,1118,1128,1138,1146,1158,1167,1175,1185,1196,1206,1215,1224,1235,1243,1254,1263,1272,1284,1294,1303,1315,1325,
    1333,1346,1356,1365,1376,1386,1395,1402,1413,1423,1432,1440,1451,1460,1469,1477,1486,1496,1508,1518,1524,1534,1544,1554,1563,
    1573,1584,1596,1603,1610,1621,1630,1637,1652,1661,1670,1680,1688,1698,1706,1719,1728,1738,1746,1757,1769,1779,1788,1793,1802,
    1813,1821,1830,1843,1852,1862,1874,1882,1893,1902,1912,1920,1931,1938,1946,1957,1964,1972,1981,1991,1998,2010,2017,2027,2036,
    2042,2053,2060,2070,2079,2086,2095,2102,2111,2119,2128,2137,2146,2153,2162,2169,2175,2184,2193,2202,2208};

  
  // robot geometry
  const float e = 8.0;     // end effector
  const  float f = 23.32;     // base
  const float re = 35.0;
  const float rf = 6.0;

  // trigonometric constants
  const float sqrt3 = sqrt(3.0);
  const float  pi = 3.141592653;    // PI
  const float sin120 = sqrt3/2.0;   
  const float  cos120 = -0.5;        
  const float tan60 = sqrt3;
  const float sin30 =  0.5;
  const float tan30 = 1.0/sqrt3;

  const int offset_1 =111; // rf  are horizontal
  const int offset_2 =110;
  const int offset_3 =116;

  const int em = 12; // Defines the pin which controls the electromagnet
  const  int led = 8; // Defines the pin which controls the LED of the Delta Robot    
    
  const int pinForRemoteControl=52; // Low to control via external device
  const int pinForStop=6;  // Low to stop
  const int pinForLocalControl=48;  // Low to control delta Robot via potensiometers
  const int pinToControlMagnet=5;  // Input Pin that defines magnet state when controlMode is 2. Low for On, High for  Off.
  const int pinToControlLED=7; // Input Pin that defines LED state when  controlMode is 2. Low for On, High for Off.
  const int pinToToggleInputMode=4;  // When controlMode is 2 defines whether input values are angles or coordinates.  
                                       // Low for Angle, High for Coordinates
  const int redLed=3;   // Pin for red led. Led is On at stop mode
  const int  yellowLed=2;  // Pin for yellow led. Led is On at local control mode
  const  int orangeLed=26; // Pin for orange led. Led is On when waiting to receive data  at remote control mode
  const int greenLed=40;   // Pin for green led. Led is  On at remote control mode
  const int led13=13;   // Pin for Arduino built in  LED 13. Led 13 is On when the position is valid and off otherwise or in Pause mode.
  const int pinForXorAngle1Potensiometer=1; // Pin connected to the potensiometer  that defines X coordinate or Angle1
  const int pinForYorAngle2Potensiometer=5;  // Pin connected to the potensiometer that defines Y coordinate or Angle2
  const  int pinForZorAngle3Potensiometer=8; // Pin connected to the potensiometer that defines  Z coordinate or Angle3
  const int pinForZoomPotensiometer=15; // Pin connected  to the potensiometer that defines Zoom factor for the other three

    /*  Also built in led 13 is on when received coordinates are valid and off when bad  coordinates have been received at control mode 0 (remote)
     * or at by the  user at control mode 2 when receiving coordinates. At any other cases (mode 1 or  mode 2 when receiving angles) led 13 is off.
     */
  
  const float tableLevel=-34.77;  // The z coordinate of the table
  float verticalLimit=tableLevel; // The lower  vertical limit that delta robot head is allowable to reach
  int verticalFlag=0;  // If the given z coordinate is lower than vertical Limit then vertical flag=1  else =0.
  
  const unsigned long intervalBetweenButtonPress=1000; // For  button debouncing
  unsigned long currentTime=0, previousTime=0;
    
  float t1;
  float t2;
  float t3;
  int angle_1=0,angle_2=0,angle_3=0;  // Desired servo angles
  int oldAngle1=0, oldAngle2=0, oldAngle3=0;  // Current  servo angles
      
  float x=0.0, y=0.0, z=-33.0;  // x,y,z coordinates
  float multipl=1.0;
  int electromagnet=0, ledState=0, magStatus=0, ledStatus=0;  // ...State= the desired condition (On or Off), ...Status= the current condition.
  int retStat=0; // Return status of angles calculation. 0 If everything is OK,  -1, -2, -3 if one two or three angles are bad.
  int controlMode=0; // Defines  who controls the servo. 0 for external serial device, 1 to stop (Pause mode) and  2 for potensiometers
  int coord=HIGH;
  int validPosition=0; // Equals 0  if the given x,y,z coordinates correspond to a valid position, negative if not and  1 when bad data is received.
                       // If it is 2 then the given  z coordinate is less than vertical limit.

    

  int delta_calcAngleYZ(float  x0, float y0, float z0, float &theta)
  {
    float y1 = -0.5 * 0.57735 *  f; // f/2 * tg 30
    //float y1 = yy1;
    y0 -= 0.5 * 0.57735    * e;    //  shift center to edge
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0  +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // discriminant
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); 
    if (d < 0) return -1; //  non-existing point
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing  outer point
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi  + ((yj>y1)?180.0:0.0);
    if ((theta < -180) || (theta > 180))
        return  -1;
    return 0;
  }

  // inverse kinematics: (x0, y0, z0) -> (theta1,  theta2, theta3)
  // returned status: 0=OK, negative=non-existing position, the  negative number is the number of wrong angles
  int delta_calcInverse(float x0,  float y0, float z0, float &theta1, float &theta2, float &theta3)
  {
    theta1  = theta2 = theta3 = 0;
    int stat1 = delta_calcAngleYZ(x0, y0, z0, theta1);
    int stat2 = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0,  theta2);  // rotate coords to +120 deg
    int stat3 = delta_calcAngleYZ(x0*cos120  - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
    return stat1+stat2+stat3;
  }

              
              
    void setup()
      { 
        Serial.begin(9600);   // To output data  to Serial monitor or any other connected device.
        Serial1.begin(9600);  // Communication with external device for remote control. Used for data input,  such as coordinates.
        pinMode(led13, OUTPUT);
        pinMode(em, OUTPUT);
        pinMode(led, OUTPUT);

        digitalWrite(led13,HIGH);
        digitalWrite(em,LOW);
        digitalWrite(led,LOW);

        servo_1.attach(9,500,2500); // attaches  the servo on pin 9 to the servo object
        servo_2.attach(10,400,2500); //  attaches the servo on pin 10 to the servo object
        servo_3.attach(11,400,2500);  // attaches the servo on pin 11 to the servo object
        
         //all  servos horizontal (x,y,z)=(0,0,-31)
        servo_1.writeMicroseconds(pgm_read_word_near(servo1+offset_1));
        angle_1=offset_1;
        servo_2.writeMicroseconds(pgm_read_word_near(servo2+offset_2));
        angle_2=offset_2;
        servo_3.writeMicroseconds(pgm_read_word_near(servo3+offset_3));
        angle_3=offset_3;

        delay(100);

        pinMode(redLed,OUTPUT);
        digitalWrite(redLed,HIGH);
        delay (100);
        pinMode(yellowLed,OUTPUT);
        digitalWrite(yellowLed,HIGH);
        delay (100);
        pinMode(greenLed,OUTPUT);
        digitalWrite(greenLed,HIGH);
        delay(100);
        pinMode(orangeLed,OUTPUT);
        digitalWrite(orangeLed,HIGH);
        delay(100);

        pinMode(pinForRemoteControl,INPUT_PULLUP);
        pinMode(pinForStop,INPUT_PULLUP);
        pinMode(pinForLocalControl,INPUT_PULLUP);  
        pinMode(pinToControlMagnet,INPUT_PULLUP);
        pinMode(pinToControlLED,INPUT_PULLUP);
        pinMode(pinToToggleInputMode,INPUT_PULLUP);            
              
        delay(500);

         // If both switches are pressed during boot,  then store desired vertical limit offset to EEPROM
        if (digitalRead(pinToControlMagnet)==LOW  && digitalRead(pinToToggleInputMode)==LOW)
        {
          int value=0;
          unsigned long past=0;
          do
          {
            if  (millis()-past>500)
            {
              digitalWrite(led13,!digitalRead(led13));  // Built in LED is flashing during procedure.
              past=millis();
            }
            value=analogRead(pinForZoomPotensiometer)/4; // read  potensiometer and map from 0-1023 to 0-255
            Serial.println("@!");
            Serial.print("Table Level= ");
            Serial.println(tableLevel+(float)value/200.0);
            Serial.println('$');            
          } while (digitalRead(pinToControlMagnet)==LOW  || digitalRead(pinToToggleInputMode)==LOW); // When both switches are released,  then save the result
          EEPROM[3]=(byte)value;
          verticalLimit=tableLevel+(float)value/200.0;
        }

        // If switch is pressed during boot then load vertical  limit offset from EEPROM
        if (digitalRead(pinToControlLED)==LOW) 
        {
          int value=EEPROM[3];
          unsigned long past=0;
          do
          {
            if (millis()-past>500)
            {
              digitalWrite(led13,!digitalRead(led13));  // Built in LED is flashing  during procedure.
              past=millis();
            }
            Serial.println("@!");
            Serial.print("Table Level= ");
            Serial.println(tableLevel+(float)value/200.0);
            Serial.println('$');
          } while (digitalRead(pinToControlLED)==LOW);
          verticalLimit=tableLevel+(float)value/200.0;
        }

        digitalWrite(led13,HIGH);  // Led 13 is on because servos are in valid position (0,0,-31)
       
        digitalWrite(redLed,LOW);
        delay(100);
        digitalWrite(yellowLed,LOW);
        delay(100);
        digitalWrite(greenLed,LOW);
        delay(100);
        digitalWrite(orangeLed,LOW);
        delay(100);
        
        
        digitalWrite(led,HIGH); // Turn on Delta Robot's LED
        ledStatus=1;
        digitalWrite(greenLed,HIGH);  // Default mode is 0 (Remotely controlled)
        Serial1.write('@'); // Informs external device that delta robot is ready  to receive data.
             
      }
      

  void loop()
    {

      if (digitalRead(pinForRemoteControl)==LOW) // If the Black button  is pressed then change cotrol mode to 0 (Remotely Controlled)
      {
        digitalWrite(led,HIGH);
        ledStatus=HIGH;
        
        currentTime=millis();
        //  If button pressed when already in controlMode 0, then send a @, but avoid sending  multiple @s.
        if (controlMode==0&&currentTime-previousTime>intervalBetweenButtonPress)  Serial1.write('@'); 
        previousTime=currentTime;
        
        controlMode=0;
        digitalWrite(yellowLed,LOW);
        digitalWrite(redLed,LOW);
        digitalWrite(greenLed,HIGH);
      }
      
      if (digitalRead(pinForStop)==LOW) // If stop (Red Button)  is pressed turn off magnet and go horizontal (Mode 1)
      {
        controlMode=1;
        digitalWrite(yellowLed,LOW);        
        digitalWrite(greenLed,LOW);
        digitalWrite(orangeLed,LOW);
        digitalWrite(redLed,HIGH);
        digitalWrite(led13,LOW);  // There is no reason examining wheather the position is valid or not since it  is already known that it is valid
          
        digitalWrite(em,LOW);
        magStatus=0; 
                      
        servo_1.writeMicroseconds(pgm_read_word_near(servo1+offset_1));
        oldAngle1=offset_1;                 
        servo_2.writeMicroseconds(pgm_read_word_near(servo2+offset_2));
        oldAngle2=offset_2;                 
        servo_3.writeMicroseconds(pgm_read_word_near(servo3+offset_3));
        oldAngle3=offset_3;

        digitalWrite(led,LOW);  // Also turn  off Delta Robot's LED
        ledStatus=LOW;
      }
      
      if  (digitalRead(pinForLocalControl)==LOW) // Locally controlled with potensiometers  (Mode 2)
      {
        controlMode=2;        
        digitalWrite(redLed,LOW);
        digitalWrite(greenLed,LOW);
        digitalWrite(orangeLed,LOW);
        digitalWrite(yellowLed,HIGH);
      }

      if (controlMode==0&&!Serial1.available())  digitalWrite(orangeLed,HIGH); // No data received yet
      if (controlMode==0&&Serial1.available())   // If there is data in Serial buffer, ... 
        {  
         
           digitalWrite(orangeLed,LOW);
           
           if (Serial1.find("@"))       // ..., then search for  start character,@, and read x,y,z,electromagnet
            {            
              x=(float)Serial1.parseInt()/100.0;
              y=(float)Serial1.parseInt()/100.0;
              z=(float)Serial1.parseInt()/100.0;
              electromagnet=Serial1.parseInt();

              if (z<verticalLimit)  // If the z coordinate is lower than the vertical  limit then z is equal to that limit
              {
                z=verticalLimit;
                verticalFlag=1;
              }
              else verticalFlag=0;
                       
              if (Serial1.find("$"))    // If end character,$,is  found, then no data is missing. 
                {
                  if (electromagnet==0&&magStatus==1)  // If magnet should be off and is on, then turn it off
                      {                        
                        digitalWrite(em,LOW);
                        magStatus=0;
                      }
                  if (electromagnet==1&&magStatus==0)  // If magnet should be on and is off, then turn it on
                      {
                        digitalWrite(em,HIGH);
                        magStatus=1;                         
                      }
                  
                   retStat=delta_calcInverse(x,y,z,t1,t2,t3);  // Calculate theta1, theta2 and theta3 angles
                   angle_1=offset_1-round(t1);
                   angle_2=offset_2-round(t2);
                   angle_3=offset_3-round(t3);

                   // If return Status is 0 (OK), all the angles are within the  limits (0 to 180), then write to the corresponding servos
                   if  (retStat==0 && angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<= 180 && angle_3>=0  && angle_3<=180)
                      {
                         validPosition=0;     // If everything is OK then validPosition=0                    
                         if  (oldAngle1!=angle_1)  // If the desired new angle (angle_1) is different from the  current angle (oldAngle1) then write it to the servo1
                          {                            
                            servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
                            oldAngle1=angle_1;
                          }
                         if (oldAngle2!=angle_2)  // If the desired new angle (angle_2)  is different from the current angle (oldAngle2) then write it to the servo2
                          {                            
                            servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
                            oldAngle2=angle_2;
                          }
                         if (oldAngle3!=angle_3)  // If the desired new angle (angle_3)  is different from the current angle (oldAngle3) then write it to the servo3
                          {                            
                            servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
                            oldAngle3=angle_3;
                          }                 
                    }
                  else validPosition=retStat==0?-4:retStat;  // If return Status==0(OK) but any of the above limitations is not met then validPosition=-4,  else =retStat
                } // Endif for Serial.find("$")  
                
               else validPosition=1;   // If bad data has been received or data  is missing, then validPosition is 1 
               if (validPosition==0 && verticalFlag==1)  validPosition=2; // If everything else is OK but vertical limit has been violated  then validPosition=2
               digitalWrite(led13,validPosition==0?HIGH:LOW);
               
            } // Endif for Serial.find("@") 
        
          if (!Serial1.available()) Serial1.write('@'); // Send @ to ask for new  data        
       } // Endif for control mode 0     

      if (controlMode==2)   // Locally controlled with potensiometers
      {
        ledState=digitalRead(pinToControlLED);  // Inverse Logic
        if (ledState==HIGH&&ledStatus==1)   // If Delta Robot  LED is on and the switch is open, then turn the LED off
           {                        
              digitalWrite(led,LOW);
              ledStatus=0;
           }
        if (ledState==LOW&&ledStatus==0)    // If Delta Robot LED is off and the  switch is closed, then turn the LED on
           {
              digitalWrite(led,HIGH);
              ledStatus=1;                         
           }
        
        coord=digitalRead(pinToToggleInputMode);  // Coordinates or angles input
        if (coord==HIGH)     // If switch is open, then read the x,y,z coordinates  from potensiometers and act accordingly
        {
          multipl=5.0+17.0*(float)analogRead(pinForZoomPotensiometer)/1023.0;   // Multiplier factor that defines the scale and range of x,y values that read  the potensiometers
          x=(float)analogRead(pinForXorAngle1Potensiometer)*2.0*multipl/1023.0-multipl;  // Reads the x value and map it according to multiplier factor
          y=(float)analogRead(pinForYorAngle2Potensiometer)*2.0*multipl/1023.0-multipl;  // Reads the y value and map it according to multiplier factor
          z=(float)analogRead(pinForZorAngle3Potensiometer)*11.0/1023.0-40.0;   // Reads the z value and map without taking into account the multiplier factor

          if (z<verticalLimit)  // If the z coordinate is lower than the vertical  limit then z is equal to that limit
          {
            z=verticalLimit;
            verticalFlag=1;
          }
          else verticalFlag=0;
          
          retStat=delta_calcInverse(x,y,z,t1,t2,t3);  // Calculate  theta1, theta2 and theta3 angles and do as above
          angle_1=offset_1-round(t1);
          angle_2=offset_2-round(t2);
          angle_3=offset_3-round(t3);
          if (retStat==0 && angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<=180  && angle_3>=0 && angle_3<=180)
              {
                  validPosition=0;                  
                  if (oldAngle1!=angle_1)
                   {                            
                       servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
                       oldAngle1=angle_1;
                   }
                  if  (oldAngle2!=angle_2)
                   {                            
                       servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
                       oldAngle2=angle_2;
                   }
                  if  (oldAngle3!=angle_3)
                   {                            
                       servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
                       oldAngle3=angle_3;
                   }                 
              }
              
            else validPosition=retStat==0?-4:retStat;  // If return Status==0(OK) but any of the above limitations is not met then validPosition=-4,  else =retStat
            if (validPosition==0 && verticalFlag==1) validPosition=2;  // If everything else is OK but vertical limit has been violated then validPosition=2
            digitalWrite(led13,validPosition==0?HIGH:LOW);  
             
        }   // Endif coord High.

        if (coord==LOW)   // If switch is  closed, then read the desired angles of the servos from potensiometers
        {
           digitalWrite(led13,LOW);   // There is no reason to be on. Every angle  between 0 to 180 can be achieved by servos.
           angle_1=analogRead(pinForXorAngle1Potensiometer)*180.0/1023.0;
           angle_2=analogRead(pinForYorAngle2Potensiometer)*180.0/1023.0;
           angle_3=analogRead(pinForZorAngle3Potensiometer)*180.0/1023.0;
           
           if (angle_1>=0 && angle_1<=180 && angle_2>=0 && angle_2<=180  && angle_3>=0 && angle_3<=180)
              {
                  if (oldAngle1!=angle_1)
                   {                            
                       servo_1.writeMicroseconds(pgm_read_word_near(servo1+angle_1));
                       oldAngle1=angle_1;
                   }
                  if  (oldAngle2!=angle_2)
                   {                            
                       servo_2.writeMicroseconds(pgm_read_word_near(servo2+angle_2));
                       oldAngle2=angle_2;
                   }
                  if  (oldAngle3!=angle_3)
                   {                            
                       servo_3.writeMicroseconds(pgm_read_word_near(servo3+angle_3));
                       oldAngle3=angle_3;
                   }                   
              }  
        }   // Endif coord Low

        electromagnet=digitalRead(pinToControlMagnet);  // Inverse Logic        
        if (electromagnet==HIGH&&magStatus==1)  // If  magnet switch is open and magnet is on, then turn it off
           {                        
              digitalWrite(em,LOW);
              magStatus=0;
           }
        if (electromagnet==LOW&&magStatus==0)   // If magnet switch is closed and  magnet is off, then turn it on
           {
              digitalWrite(em,HIGH);
              magStatus=1;                         
           }   
      }  // Endif for control mode 2

        // If there is enough space in buffer  then write in order to avoid delay in code execution.
      if (Serial.availableForWrite()>62)
         {
            if (controlMode==2&&coord==LOW)
               {
                  Serial.println("@#");
                  Serial.print("Servo1=  ");
                  Serial.println(angle_1);
                  Serial.print("Servo2=  ");
                  Serial.println(angle_2);
                  Serial.print("Servo3=  ");
                  Serial.println(angle_3); 
                  Serial.println('$');
               }
            else if (controlMode==1)
               {
                  Serial.println("@%");
                  Serial.println("  Paused!");
                  Serial.println('$');
               }
            else
               {
                  Serial.println('@');
                  Serial.print("X=  ");
                  Serial.println(x);
                  Serial.print("Y=  ");
                  Serial.println(y);
                  Serial.print("Z=  ");
                  Serial.println(z); 
                  if (validPosition==0)  Serial.println(" OK");
                  if (validPosition<0) Serial.println("  Bad Position!");
                  if (validPosition==1) Serial.println(" Bad  Data!");
                  if (validPosition==2) Serial.println(" z Limit!");
                  Serial.print(validPosition);  
                  Serial.println('$');
               } 
         }             
  }

The Code of the Arduino UNO

arduino

This is the Code that runs on the Arduino UNO which receives the output messages of the Arduino MEGA and shows on the LCD screen the Coordinates of the Delta Robot's head.

1#include<LiquidCrystal.h>
2
3LiquidCrystal lcd(7,6,5,4,3,2);
4
5
6      byte smiley[8] =
7      {
8        B00000,
9        B10001,
10        B00000,
11        B00000,
12        B10001,
13        B01110,
14        B00000,
15        B00000
16      };
17
18
19      byte bad[8] =
20      {
21        B00000,
22        B10001,
23        B00000,
24        B00000,
25        B01110,
26        B10001,
27        B00000,
28        B00000
29      };
30
31
32      byte bump[8] =
33      {
34        B00000,
35        B00100,
36        B00100,
37        B00100,
38        B00100,
39        B01110,
40        B11111,
41        B00000
42      };
43
44
45  
46 float x=0.0,y=0.0,z=0.0,limit=-34.77;
47  float oldx=0.0, oldy=0.0, oldz=0.0, oldlimit=0.0;
48 int angle1=0, angle2=0, angle3=0;
49  int old1=0, old2=0, old3=0;
50 int prevLayout=-1;
51 int layout=-1;
52 int validPosition=0;
53  int nextChar=0;
54 
55              
56 void setup()
57      { 
58        Serial.begin(9600);
59        pinMode(13, OUTPUT);
60        digitalWrite(13,HIGH); 
61
62        lcd.createChar(1,smiley);
63        lcd.createChar(2,bad);
64        lcd.createChar(3,bump);
65
66        lcd.begin(16,2);
67        lcd.clear();
68        lcd.print("Waiting For Data");
69        lcd.setCursor(0,1);
70        lcd.print("Baud Rate= 9600");
71      }
72      
73
74 void loop()
75    {
76   
77       if(Serial.available()>=4)
78         {
79            Serial.find("@");
80            nextChar=Serial.peek();
81            if (nextChar=='#') layout=2;
82            else if (nextChar=='%') layout=1;
83            else if (nextChar=='!')  layout=3;
84            else layout=0;
85
86            if (layout==0)
87            {
88              x=Serial.parseFloat();            
89              y=Serial.parseFloat();            
90              z=Serial.parseFloat();
91              validPosition=Serial.parseInt();
92
93              if (x!=oldx||y!=oldy||z!=oldz||prevLayout!=0)
94                  {                     
95                      lcd.clear();
96                      lcd.print("X=  ");
97                      lcd.print(x);
98                      lcd.setCursor(11,0);
99                      lcd.write(validPosition==0?1:2);
100                      if  (validPosition==1) lcd.write('!');
101                      if (validPosition<0)  lcd.print(-1*validPosition);
102                      if (validPosition==2) lcd.write(3);
103                      lcd.setCursor(14,0);
104                      lcd.print("Z=  ");
105                      lcd.setCursor(0,1);
106                      lcd.print("Y=  ");
107                      lcd.print(y);
108                      lcd.setCursor(10,1);
109                      lcd.print(z);
110                      
111                      oldx=x;
112                      oldz=z;
113                      oldy=y;
114                      prevLayout=0;      
115                  }
116                  
117            }
118            else  if (layout==1)
119            {
120              if (prevLayout!=1)
121              {
122                lcd.clear();
123                lcd.print(" Delta Robot is");
124                lcd.setCursor(5,1);
125                lcd.print("PAUSED");
126                prevLayout=1;
127              }
128            }
129            else  if (layout==2)            
130            {
131              Serial.find('=');
132              angle1=Serial.parseInt();
133              Serial.find('=');            
134              angle2=Serial.parseInt();
135              Serial.find('=');            
136              angle3=Serial.parseInt();
137
138              if (angle1!=old1||angle2!=old2||angle3!=old3||prevLayout!=2)
139                 {                       
140           
141                    lcd.clear();
142
143                    lcd.setCursor(2,0);
144                    lcd.write(127);
145                    lcd.write(230);
146                    lcd.write('1');
147                    lcd.setCursor(6,0);
148                    lcd.write(127);
149                    lcd.write(230);
150                    lcd.write('2');
151                    lcd.setCursor(10,0);
152                    lcd.write(127);
153                    lcd.write(230);
154                    lcd.write('3');
155                    
156                    lcd.setCursor(2,1);
157                    lcd.print(angle1);
158                    lcd.setCursor(6,1);
159                    lcd.print(angle2);
160                    lcd.setCursor(10,1);
161                    lcd.print(angle3);
162
163                    old1=angle1;
164                    old2=angle2;
165                    old3=angle3;
166                    prevLayout=2;    
167                 }
168                 
169            }
170            else
171            {
172                 limit=Serial.parseFloat();
173
174                 if (limit!=oldlimit||prevLayout!=3)
175                 {
176
177                    lcd.clear();
178                    
179                    lcd.print("  Lower Limit =");
180
181                    lcd.setCursor(5,1);
182                    lcd.print(limit);
183                                        
184                    oldlimit=limit;
185                    prevLayout=3;
186                 }
187            }
188
189            Serial.find("$");
190       }
191    }
192   
193

#include<LiquidCrystal.h>

LiquidCrystal lcd(7,6,5,4,3,2);


      byte smiley[8] =
      {
        B00000,
        B10001,
        B00000,
        B00000,
        B10001,
        B01110,
        B00000,
        B00000
      };


      byte bad[8] =
      {
        B00000,
        B10001,
        B00000,
        B00000,
        B01110,
        B10001,
        B00000,
        B00000
      };


      byte bump[8] =
      {
        B00000,
        B00100,
        B00100,
        B00100,
        B00100,
        B01110,
        B11111,
        B00000
      };


  
 float x=0.0,y=0.0,z=0.0,limit=-34.77;
  float oldx=0.0, oldy=0.0, oldz=0.0, oldlimit=0.0;
 int angle1=0, angle2=0, angle3=0;
  int old1=0, old2=0, old3=0;
 int prevLayout=-1;
 int layout=-1;
 int validPosition=0;
  int nextChar=0;
 
              
 void setup()
      { 
        Serial.begin(9600);
        pinMode(13, OUTPUT);
        digitalWrite(13,HIGH); 

        lcd.createChar(1,smiley);
        lcd.createChar(2,bad);
        lcd.createChar(3,bump);

        lcd.begin(16,2);
        lcd.clear();
        lcd.print("Waiting For Data");
        lcd.setCursor(0,1);
        lcd.print("Baud Rate= 9600");
      }
      

 void loop()
    {
   
       if(Serial.available()>=4)
         {
            Serial.find("@");
            nextChar=Serial.peek();
            if (nextChar=='#') layout=2;
            else if (nextChar=='%') layout=1;
            else if (nextChar=='!')  layout=3;
            else layout=0;

            if (layout==0)
            {
              x=Serial.parseFloat();            
              y=Serial.parseFloat();            
              z=Serial.parseFloat();
              validPosition=Serial.parseInt();

              if (x!=oldx||y!=oldy||z!=oldz||prevLayout!=0)
                  {                     
                      lcd.clear();
                      lcd.print("X=  ");
                      lcd.print(x);
                      lcd.setCursor(11,0);
                      lcd.write(validPosition==0?1:2);
                      if  (validPosition==1) lcd.write('!');
                      if (validPosition<0)  lcd.print(-1*validPosition);
                      if (validPosition==2) lcd.write(3);
                      lcd.setCursor(14,0);
                      lcd.print("Z=  ");
                      lcd.setCursor(0,1);
                      lcd.print("Y=  ");
                      lcd.print(y);
                      lcd.setCursor(10,1);
                      lcd.print(z);
                      
                      oldx=x;
                      oldz=z;
                      oldy=y;
                      prevLayout=0;      
                  }
                  
            }
            else  if (layout==1)
            {
              if (prevLayout!=1)
              {
                lcd.clear();
                lcd.print(" Delta Robot is");
                lcd.setCursor(5,1);
                lcd.print("PAUSED");
                prevLayout=1;
              }
            }
            else  if (layout==2)            
            {
              Serial.find('=');
              angle1=Serial.parseInt();
              Serial.find('=');            
              angle2=Serial.parseInt();
              Serial.find('=');            
              angle3=Serial.parseInt();

              if (angle1!=old1||angle2!=old2||angle3!=old3||prevLayout!=2)
                 {                       
           
                    lcd.clear();

                    lcd.setCursor(2,0);
                    lcd.write(127);
                    lcd.write(230);
                    lcd.write('1');
                    lcd.setCursor(6,0);
                    lcd.write(127);
                    lcd.write(230);
                    lcd.write('2');
                    lcd.setCursor(10,0);
                    lcd.write(127);
                    lcd.write(230);
                    lcd.write('3');
                    
                    lcd.setCursor(2,1);
                    lcd.print(angle1);
                    lcd.setCursor(6,1);
                    lcd.print(angle2);
                    lcd.setCursor(10,1);
                    lcd.print(angle3);

                    old1=angle1;
                    old2=angle2;
                    old3=angle3;
                    prevLayout=2;    
                 }
                 
            }
            else
            {
                 limit=Serial.parseFloat();

                 if (limit!=oldlimit||prevLayout!=3)
                 {

                    lcd.clear();
                    
                    lcd.print("  Lower Limit =");

                    lcd.setCursor(5,1);
                    lcd.print(limit);
                                        
                    oldlimit=limit;
                    prevLayout=3;
                 }
            }

            Serial.find("$");
       }
    }

The Code of the Arduino UNO

arduino

This is the Code that runs on the Arduino UNO which receives the output messages of the Arduino MEGA and shows on the LCD screen the Coordinates of the Delta Robot's head.

1#include<LiquidCrystal.h>
2
3LiquidCrystal lcd(7,6,5,4,3,2);
4
5
6
7      byte smiley[8] =
8      {
9        B00000,
10        B10001,
11        B00000,
12
13        B00000,
14        B10001,
15        B01110,
16        B00000,
17        B00000
18
19      };
20
21
22      byte bad[8] =
23      {
24        B00000,
25        B10001,
26
27        B00000,
28        B00000,
29        B01110,
30        B10001,
31        B00000,
32
33        B00000
34      };
35
36
37      byte bump[8] =
38      {
39        B00000,
40
41        B00100,
42        B00100,
43        B00100,
44        B00100,
45        B01110,
46
47        B11111,
48        B00000
49      };
50
51
52  
53 float x=0.0,y=0.0,z=0.0,limit=-34.77;
54
55  float oldx=0.0, oldy=0.0, oldz=0.0, oldlimit=0.0;
56 int angle1=0, angle2=0, angle3=0;
57
58  int old1=0, old2=0, old3=0;
59 int prevLayout=-1;
60 int layout=-1;
61 int validPosition=0;
62
63  int nextChar=0;
64 
65              
66 void setup()
67      { 
68        Serial.begin(9600);
69
70        pinMode(13, OUTPUT);
71        digitalWrite(13,HIGH); 
72
73        lcd.createChar(1,smiley);
74
75        lcd.createChar(2,bad);
76        lcd.createChar(3,bump);
77
78        lcd.begin(16,2);
79
80        lcd.clear();
81        lcd.print("Waiting For Data");
82        lcd.setCursor(0,1);
83
84        lcd.print("Baud Rate= 9600");
85      }
86      
87
88 void loop()
89
90    {
91   
92       if(Serial.available()>=4)
93         {
94            Serial.find("@");
95
96            nextChar=Serial.peek();
97            if (nextChar=='#') layout=2;
98
99            else if (nextChar=='%') layout=1;
100            else if (nextChar=='!')
101  layout=3;
102            else layout=0;
103
104            if (layout==0)
105            {
106
107              x=Serial.parseFloat();            
108              y=Serial.parseFloat();
109            
110              z=Serial.parseFloat();
111              validPosition=Serial.parseInt();
112
113
114              if (x!=oldx||y!=oldy||z!=oldz||prevLayout!=0)
115                  {
116                     
117                      lcd.clear();
118                      lcd.print("X=
119  ");
120                      lcd.print(x);
121                      lcd.setCursor(11,0);
122
123                      lcd.write(validPosition==0?1:2);
124                      if
125  (validPosition==1) lcd.write('!');
126                      if (validPosition<0)
127  lcd.print(-1*validPosition);
128                      if (validPosition==2) lcd.write(3);
129
130                      lcd.setCursor(14,0);
131                      lcd.print("Z=
132  ");
133                      lcd.setCursor(0,1);
134                      lcd.print("Y=
135  ");
136                      lcd.print(y);
137                      lcd.setCursor(10,1);
138
139                      lcd.print(z);
140                      
141                      oldx=x;
142
143                      oldz=z;
144                      oldy=y;
145                      prevLayout=0;
146      
147                  }
148                  
149            }
150            else
151  if (layout==1)
152            {
153              if (prevLayout!=1)
154              {
155
156                lcd.clear();
157                lcd.print(" Delta Robot is");
158
159                lcd.setCursor(5,1);
160                lcd.print("PAUSED");
161
162                prevLayout=1;
163              }
164            }
165            else
166  if (layout==2)            
167            {
168              Serial.find('=');
169
170              angle1=Serial.parseInt();
171              Serial.find('=');            
172
173              angle2=Serial.parseInt();
174              Serial.find('=');            
175
176              angle3=Serial.parseInt();
177
178              if (angle1!=old1||angle2!=old2||angle3!=old3||prevLayout!=2)
179
180                 {                       
181           
182                    lcd.clear();
183
184
185                    lcd.setCursor(2,0);
186                    lcd.write(127);
187
188                    lcd.write(230);
189                    lcd.write('1');
190                    lcd.setCursor(6,0);
191
192                    lcd.write(127);
193                    lcd.write(230);
194                    lcd.write('2');
195
196                    lcd.setCursor(10,0);
197                    lcd.write(127);
198
199                    lcd.write(230);
200                    lcd.write('3');
201                    
202
203                    lcd.setCursor(2,1);
204                    lcd.print(angle1);
205
206                    lcd.setCursor(6,1);
207                    lcd.print(angle2);
208
209                    lcd.setCursor(10,1);
210                    lcd.print(angle3);
211
212
213                    old1=angle1;
214                    old2=angle2;
215                    old3=angle3;
216
217                    prevLayout=2;    
218                 }
219                 
220
221            }
222            else
223            {
224                 limit=Serial.parseFloat();
225
226
227                 if (limit!=oldlimit||prevLayout!=3)
228                 {
229
230
231                    lcd.clear();
232                    
233                    lcd.print("
234  Lower Limit =");
235
236                    lcd.setCursor(5,1);
237                    lcd.print(limit);
238
239                                        
240                    oldlimit=limit;
241
242                    prevLayout=3;
243                 }
244            }
245
246
247            Serial.find("$");
248       }
249    }
250   
251

#include<LiquidCrystal.h>

LiquidCrystal lcd(7,6,5,4,3,2);



      byte smiley[8] =
      {
        B00000,
        B10001,
        B00000,

        B00000,
        B10001,
        B01110,
        B00000,
        B00000

      };


      byte bad[8] =
      {
        B00000,
        B10001,

        B00000,
        B00000,
        B01110,
        B10001,
        B00000,

        B00000
      };


      byte bump[8] =
      {
        B00000,

        B00100,
        B00100,
        B00100,
        B00100,
        B01110,

        B11111,
        B00000
      };


  
 float x=0.0,y=0.0,z=0.0,limit=-34.77;

  float oldx=0.0, oldy=0.0, oldz=0.0, oldlimit=0.0;
 int angle1=0, angle2=0, angle3=0;

  int old1=0, old2=0, old3=0;
 int prevLayout=-1;
 int layout=-1;
 int validPosition=0;

  int nextChar=0;
 
              
 void setup()
      { 
        Serial.begin(9600);

        pinMode(13, OUTPUT);
        digitalWrite(13,HIGH); 

        lcd.createChar(1,smiley);

        lcd.createChar(2,bad);
        lcd.createChar(3,bump);

        lcd.begin(16,2);

        lcd.clear();
        lcd.print("Waiting For Data");
        lcd.setCursor(0,1);

        lcd.print("Baud Rate= 9600");
      }
      

 void loop()

    {
   
       if(Serial.available()>=4)
         {
            Serial.find("@");

            nextChar=Serial.peek();
            if (nextChar=='#') layout=2;

            else if (nextChar=='%') layout=1;
            else if (nextChar=='!')
  layout=3;
            else layout=0;

            if (layout==0)
            {

              x=Serial.parseFloat();            
              y=Serial.parseFloat();
            
              z=Serial.parseFloat();
              validPosition=Serial.parseInt();


              if (x!=oldx||y!=oldy||z!=oldz||prevLayout!=0)
                  {
                     
                      lcd.clear();
                      lcd.print("X=
  ");
                      lcd.print(x);
                      lcd.setCursor(11,0);

                      lcd.write(validPosition==0?1:2);
                      if
  (validPosition==1) lcd.write('!');
                      if (validPosition<0)
  lcd.print(-1*validPosition);
                      if (validPosition==2) lcd.write(3);

                      lcd.setCursor(14,0);
                      lcd.print("Z=
  ");
                      lcd.setCursor(0,1);
                      lcd.print("Y=
  ");
                      lcd.print(y);
                      lcd.setCursor(10,1);

                      lcd.print(z);
                      
                      oldx=x;

                      oldz=z;
                      oldy=y;
                      prevLayout=0;
      
                  }
                  
            }
            else
  if (layout==1)
            {
              if (prevLayout!=1)
              {

                lcd.clear();
                lcd.print(" Delta Robot is");

                lcd.setCursor(5,1);
                lcd.print("PAUSED");

                prevLayout=1;
              }
            }
            else
  if (layout==2)            
            {
              Serial.find('=');

              angle1=Serial.parseInt();
              Serial.find('=');            

              angle2=Serial.parseInt();
              Serial.find('=');            

              angle3=Serial.parseInt();

              if (angle1!=old1||angle2!=old2||angle3!=old3||prevLayout!=2)

                 {                       
           
                    lcd.clear();


                    lcd.setCursor(2,0);
                    lcd.write(127);

                    lcd.write(230);
                    lcd.write('1');
                    lcd.setCursor(6,0);

                    lcd.write(127);
                    lcd.write(230);
                    lcd.write('2');

                    lcd.setCursor(10,0);
                    lcd.write(127);

                    lcd.write(230);
                    lcd.write('3');
                    

                    lcd.setCursor(2,1);
                    lcd.print(angle1);

                    lcd.setCursor(6,1);
                    lcd.print(angle2);

                    lcd.setCursor(10,1);
                    lcd.print(angle3);


                    old1=angle1;
                    old2=angle2;
                    old3=angle3;

                    prevLayout=2;    
                 }
                 

            }
            else
            {
                 limit=Serial.parseFloat();


                 if (limit!=oldlimit||prevLayout!=3)
                 {


                    lcd.clear();
                    
                    lcd.print("
  Lower Limit =");

                    lcd.setCursor(5,1);
                    lcd.print(limit);

                                        
                    oldlimit=limit;

                    prevLayout=3;
                 }
            }


            Serial.find("$");
       }
    }

Downloadable files

The Robot with the Raspberry Pi

Overview of the Delta Robot Connected to the Raspberry Pi and the Arduino UNO with the LCD screen to show the output of the robot.

The Robot with the Raspberry Pi

The Controls

These are the controls. The potensiometers move the head and the switches turn on or off the magnet and the LED on the top of the structure. The colored buttons select the working mode, namely whether the Robot is controlled by the potensiometers and the switches or by the remote device via serial port. The LEDs show the status of the robot.

The Controls

The Electromagnet

This is the Delta Robot's Head with the electromagnet. A metallic pin head has been lifted by the magnet.

The Electromagnet

The Polyfuses

This is the board with the servomotor polyfuses. They are a type of fuse that can be tripped and reset multiple times and they reset automatically when the sort circuit is removed.

The Polyfuses

The Board with the Relays

This is the board with the relays that control the magnet and the bright LED on top of the Robot. The relay coils are controlled by Arduino Mega via MOSFETs.

The Board with the Relays

The Arduino UNO with the LCD Screen Shield

The Arduino UNO with the LCD Screen Shield showing the angles of the servomotors during maintenance.

The Arduino UNO with the LCD Screen Shield

The Robot with the Raspberry Pi

Overview of the Delta Robot Connected to the Raspberry Pi and the Arduino UNO with the LCD screen to show the output of the robot.

The Robot with the Raspberry Pi

The Controls

The Electromagnet

This is the Delta Robot's Head with the electromagnet. A metallic pin head has been lifted by the magnet.

The Electromagnet

The Arduino UNO with the LCD Screen Shield

The Arduino UNO with the LCD Screen Shield showing the angles of the servomotors during maintenance.

The Arduino UNO with the LCD Screen Shield

The Board with the Relays

This is the board with the relays that control the magnet and the bright LED on top of the Robot. The relay coils are controlled by Arduino Mega via MOSFETs.

The Board with the Relays

The Polyfuses

This is the board with the servomotor polyfuses. They are a type of fuse that can be tripped and reset multiple times and they reset automatically when the sort circuit is removed.

The Polyfuses

Comments

Only logged in users can leave comments

geordag

0 Followers

•

0 Projects

Intro