Published January 17, 2019 © GPL3+

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.

AdvancedShowcase (no instructions)9,751

Things used in this project

Hardware components

Arduino Mega 2560

This is the main board that controlls the Delta Robot.

Arduino UNO

This is used to capture and display on an LCD screen the output message of the Arduino Mega.

LCD Screen Shield for Arduino UNO

It is used to display the coordinates of the Delta Robot's Head.

Prototype Shield for Arduino Mega

The controls and LEDs were soldered here.

Servomotor

They move the robot's arms.

Polyfuse

To protect the servomotors from high stalling currents.

Electromagnet

For lifting objects.

Relay (generic)

They power the electromagnet and the bright LED.

MOSFET

They power the coils of the relays.

Computer Power Supply

Supplies everything with electric power.

HC-06 Bluetooth Module

This is optional part if wireless communication via Bluetooth is desired.

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

Software apps and online services

Arduino IDE

All the Arduino codes were developed using the official Arduino IDE.

Arduino Delta Robot Controller

This is the Application for Android Tablets which was used to control the Delta Robot using touch gestures and trasmitting the coordinates via Bluetooth.

Hand tools and fabrication machines

Soldering iron (generic)

Story

This is a delta robot that comes to life due to an Arduino Mega board. It has an electromagnet at its head in order to be able to grab and move ferromagnetic objects.

At its control panel, there are buttons, swithes and potensiometers in order to be able to move its head to the desired position and turn on and off the electromagnet. Moreover it can be controlled remotely using the UART serial port 1 (PINs 18 and 19). It can receive coordinates and the desired state of the magnet via Serial port. Consequently with the use of a Bluetooth Adaptor such as HC-05 or HC-06 it can be controlled wirelessly by another Bluetooth device. So it can be used at a lot of applications according to the remote device which is connected to it.

The message that must be sent to Arduino Mega is @xxxx, yyyy, zzzz, m$ where xxxx, yyyy and zzzz are the ASCII codes of the digits of the x, y and z coordinates respectively and m is the desired magnet status which can be either '0' or '1' in order to turn it off or on respectively. All these numbers are actually the ASCII codes of each digit, not integers, so the Serial1.parseInt() function is used to read these numbers. The symbols '@' and '$' denote the start and the end of message respectively so that the Arduino Mega can indetify what each number represents and whether the message is complete or not. When it is done, it sends the character '@' to the remote device to say that everything is OK and to ask for the next message.

In the following picture there is an overview of the entire structure with the Arduino and the power supply:

In the next two pictures the control panel with the power supply and the servomotors are shown:

Additionally, it sends its current coordinates and current status to an external device via the UART port 0 (PINs 0 and 1) which is also connected to the USB port. Thus the user can see the output on the serial terminal of the Arduino IDE or on any other device with serial port such as another Arduino.

To capture the output, I have used an Arduino UNO with an LCD screen shield for UNO, where one can see the coordinates of the delta robot's head. Moreover it shows either a happy face if everything is OK or a sad face if the coordinates sent by the remote device to the robot were outside the region its head can reach. In such case, namely if the remote device sends to the Arduino Mega invalid position, the Robot does not move and it sends as an output this condition which can be capured by the code that runs on the Aduino UNO.

In the following picture the Arduino UNO with the LCD screen shield on top of it is shown:

In order to control it by an Android tablet via Bluetooth using a Bluetooth adapter such as HC-05 or HC-06, I have developed an Android application that can be found at the following link:

https://play.google.com/store/apps/details?id=com.georgdag.arduinobaseddeltarobotremotecontroller

It uses the touch screen of the tablet to convert touch gestures into coordinates and send them via Bluetooth to the Arduino. Of course this app can be used to control every other similar project. Here is a screenshot of this app:

To demostrate here the robot I have uploaded a video which shows it playing Tic Tac Toe against human using tokens. For that reason I have used a Raspberry Pi 2 B+ board which runs Raspbian OS (derived from Debian Linux). The Tic Tac Toe game code runs on the Raspberry and is written in C++ with the help of OpenCV library for the camera use. Using a Pi Camera fixed on the top of the robot looking down, it can recognize opponent's moves. Then it decides its next move. If the player has cheated, it moves the tokens back to their last valid positions and asks the opponent to play again. When the Raspberry has decided to move the head of the robot or to switch the magnet on or off, then it sends the coordinates and the desired status of the electromagnet to the delta robot, namely to the Arduino Mega, via the UART port. Maybe I have to give more thorough details about that project in a future post, although the Tic Tac Toe game is not a pure Arduino project as it runs entirely on the Raspberry. Here I want to focus on the Arduino part of the project, that's why I stick to the robot, while the Tic Tac Toe game is referred in order to demonstrate the robot's capabilities and its ability to communicate with external devices and follow orders.

For that demonstration, here follows a Youtube video showing delta robot in action playing Tic Tac Toe:

Code

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

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

Credits

geordag

0 projects • 4 followers

Arduino Controlled Delta Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Schematics

The Robot with the Raspberry Pi

The Controls

The Board with the Relays

The Polyfuses

The Electromagnet

The Arduino UNO with the LCD Screen Shield

Code

Main Code

The Code of the Arduino UNO

Credits

geordag

Comments

Embed the widget on your own site

Arduino Controlled Delta Robot

Arduino Controlled Delta Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Schematics

The Robot with the Raspberry Pi

The Controls

The Board with the Relays

The Polyfuses

The Electromagnet

The Arduino UNO with the LCD Screen Shield

Code

Main Code

The Code of the Arduino UNO

Credits

geordag

Comments

Related channels and tags