AUTOmelette Automated Articulated Omelette Cooking Robot
4545 404
Ashwin Ajit Patil

AUTOmelette Automated Articulated Omelette Cooking Robot

As we are aware that, a large number of young individuals stay away from their home for higher...

Description:-

As we are aware that, a large number of young individuals stay away from their home for higher education, jobs & other stuff. An average individual spends near about 30 to 35 rupees on an average daily on breakfast. We often imagine how working individuals are busy in their hectic daily schedule, meanwhile they tend to get exhausted while returning from their jobs and then end up buying online food which is quite expensive or in some cases unhealthy too. Same applies to Hostellers or abroad studying college students who have to either make their own food after returning from a tiresome day from their college or have to depend on mess for breakfast or brunch. But it is costly and unhealthy to eat online fast-food like everyday burgers, pizzas, chinese , etc or stale food at mess. Thus, we decided to research and work on a project using which we can overcome this problem. Hence, we came up with an ideation of automating this cooking process, so that every individual can enjoy homemade food which will eventually be prepared by a robot in a hygiene surrounding. Also, we took it to the next step by accepting the challenge to start the cooking process by accepting command through Google Assistant and serving the food hot when the respective person reaches home. This is a prototype model, real ones need a little high investments, but through completions like this we hope we would stand better in future & keep working on this research. Further we are eager to work on this, more deeply & dive into AI Kitchens driven by Robots. This invention, which is now in prototype phase, would benefit further to people living out of their city, college students, college campuses, industry areas, meeting rooms, conference halls, etc . Its just like a Coffee machine fitted everywhere, the same way this robot can also be taken into applications in variety of areas in a few bucks.

Project Used Hardware

Automelette Hardware

1. 3 x 20 Kgcm High Torque Servo Motors (Robokits) 2. 2 x MG996R motors with 10Kgcm. 3. SG90 motor with 2kgcm (locking system.) 4. Robokits servo motor controller board. 5. ESP8266-01 module. 6. Jumper Cables 7. Induction cooktop. 8. 0-12 Volts Variable Power Supply 9. Steel Spatula 10. Silicon Brush(for oil) 11. Steel Measure Cup(60ml), ETC (Only Main components mentioned, other miscellaneous)

Software Used

1. Solid works 2. Easyeda 3. Robokits servo controller software 4. Arduino IDE 5. Google Assistant 6. IFTTT 7. Adafruit IO

Project Hardware Software Selection

Automelette Hardware

Hardware Selection :- 1) Motors Selection - While designing we considered stepper Motors as our first preference as the required torque was high, but these are quite bulky and robust, which results to huge cost of motors, also it was not possible to design an arm consisting of stepper motors with low budget. Thus, we went for Servo Motors which would go in our budget. We have selected 3 * 20 Kg/cm high torque motors from which 1 motor is at the base for 1st axial rotation of the arm and other 2 motors are used for 2nd and 3rd axial movement of upper and lower arm. 2 * 10Kgcm is used for axial movement of wrist of the Arm. One 2Kgcm motors is used for locking mechanism of attachable equipment like Spoon, Oil brush and Spatula. 2) Controller Selection - At the beginning, we selected Arduino Uno as our main control board, but it was not feasible to control the positions of arm in real-time which would also take large amount of time for programming the same. Thus, we changed to ARDUINO UNO BASED USB 18 SERVO CONTROLLER which consist of Onboard ATMEGA 328p chip with I2C interface for up to 18 servos. This board was easy to program with a GUI for controlling each servo in real-time which eventually saved our time for programming. Also, an ESP8266-01 is interfaced with this controller board for communication with ADAFRUIT cloud. Whenever the order is placed by user the ESP sends signal to controller to begin the cooking process. Software Selection : Solid works – We found the best software till now for creating 3d models is SolidWorks, Hence as we were familiar with it since diploma, we managed well with this software. EasyEDA – The most popular & makers favourite software for creating schematics & playing with PCBs EasyEDA was used to design the schematic of this project. Robokits servo controller software – It is GUI used to control servos in Realtime, means whenever you establish a connection with this board & open their GUI, we can freely move any of the servos among 18 to any angle & also record the steps for future use. This saved our lot of time for programming & also it was literally not possible to admin/upload code, then check whether servo angle is correct or not, vice versa .This software does most of the work done. Arduino IDE - It is an Integrated Development Environment software used to compile and admin/upload codes to any Arduino boards. Google Assistant – It is an artificial intelligence powered virtual assistant developed by Google which can take commands or questions (commands for our project) and respond to it accordingly. In our project it triggers the IFTTT on saying – “Hey Google” and then “Prepare an omelette i am hungry” command. IFTTT – We have used it as a mediator between Google Assistant and Adafruit service. Whenever the Google Assistant receives a predefined command, it triggers the IFTTT which in turn triggers the adafruit io button. Also, we have used the geo location tracking service to trigger the adafruit button when the user enters a predefined area. Adafruit IO – It responds to the trigger of IFTTT by setting the button as HIGH which is further read by ESP01 which is constantly observing the state of buttons.

Circuit Diagram

Automelette Circuit Diagram

Explanation All the servo motors are connected to Arduino via servo control board. But we have used Arduino Uno Based USB 18 Servo Controller board practically which is capable of interfacing servos. Esp8266-01 controller is used to for IOT part of our project, the GPIO 2 pin is connected to D4 pin of Arduino. The pin is high when esp01 receives command from adafruit io which is read by Arduino and further cooking processes are carried out. In order to provide a good user experience an LCD and Voice playback module can be interfaced with the controller. LCD will display the current cooking process being carried out and also the time remaining to complete the cooking process. There is no as such explanation in the circuit diagram, everything else used is open source. An APR33A3 Voice Record & Playback Module can be used to provide audio playback of the cooking process being carried out, also it can inform user after the completion of cooking process.

Code

   


#include <ESP8266WiFi.h>
#include "config.h"
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"

#define ToCont  2
int GoogleAssistant;
int Loc_State;
int GA_State;

#define WLAN_SSID       "AME"   // Your SSID
#define WLAN_PASS       "tushar007"        // Your password

/************************* Adafruit.io Setup *********************************/

#define AIO_SERVER      "io.adafruit.com" //Adafruit Server
#define AIO_SERVERPORT  1883                   
#define AIO_USERNAME    "Automelette"
#define AIO_KEY       "aio_qKuB11XwBqowiAs3nVdxk1cNPPhD"

//WIFI CLIENT
WiFiClient client;

Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);

Adafruit_MQTT_Subscribe Loc = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME"/feeds/Location"); // Feeds name should be same everywhere
Adafruit_MQTT_Subscribe GA = Adafruit_MQTT_Subscribe(&mqtt, AIO_USERNAME"/feeds/google-assistant");

Adafruit_MQTT_Publish photocell = Adafruit_MQTT_Publish(&mqtt, AIO_USERNAME "/feeds/google-assistant");

void MQTT_connect();

void setup() {
  Serial.begin(9600);

  pinMode(ToCont, OUTPUT);
 
  // Connect to WiFi access point.
  Serial.println(); Serial.println();
  Serial.print("Connecting to ");
  Serial.println(WLAN_SSID);

  WiFi.begin(WLAN_SSID, WLAN_PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println();

  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
 
  mqtt.subscribe(&Loc);
  mqtt.subscribe(&GA);
}

uint32_t x=0;
int laststate = 0, currentstate = 0;

void loop() {
 
  MQTT_connect();

  Adafruit_MQTT_Subscribe *subscription;
  while ((subscription = mqtt.readSubscription(20000))) {
    if (subscription == &Loc) {
      Serial.println(F("Got_Loc: "));
      Serial.println((char *)Loc.lastread);
      Loc_State = atoi((char *)Loc.lastread);
    }  
    if (subscription == &GA) {
      Serial.print(F("Got_GA: "));
      Serial.println((char *)GA.lastread);
      GA_State = atoi((char *)GA.lastread);
       Serial.println("Received from GA");
    }  
    Send_Out();
    
  }
   Send_Out();
    
}

void Send_Out() {
   if(GA_State && Loc_State == 1)
  {
    digitalWrite(ToCont,HIGH);
    Serial.println("ToCont HIGH");
  }
  else{
    digitalWrite(ToCont,LOW);
    Serial.println("ToCont LOW");
  }
 
    if(Loc_State == 1)
   {
    currentstate = 1;
   }
   else
   {
    currentstate = 0;
   }

   if(currentstate != laststate)
   {
    
   }

    if( currentstate == 0 && laststate == 1)
   {
      Serial.print(F("\nSending val ToCont"));
      Serial.print(x);
      Serial.print("...");
     if (! photocell.publish(0)) {
         Serial.println(F("Failed"));
         }
     else {
    Serial.println(F("OK!"));
  }
     
   }
   Serial.println("HEre am I");
   Serial.print("Loca State");
  Serial.println(Loc_State);
  Serial.print("Current");
  Serial.println(currentstate);
  Serial.print("Last");
  Serial.println(laststate);
  laststate = currentstate;
}

void MQTT_connect() {
  int8_t ret;

  if (mqtt.connected()) {
    return;
  }

  Serial.print("Connecting to MQTT... ");

  uint8_t retries = 3;
 
  while ((ret = mqtt.connect()) != 0) {
    Serial.println(mqtt.connectErrorString(ret));
    Serial.println("Retrying MQTT connection in 5 seconds...");
    mqtt.disconnect();
    delay(5000);
    retries--;
    if (retries == 0) {
      while (1);
    }
  }
  Serial.println("MQTT Connected!");
 
}

...................................................................................................................................

/*    ROBOKITS ARDUINO USB/BLUETOOTH 18 SERVO CONTROLLER FIRMWARE

This code is generated from Robokits Arduino Bluetooth/USB Servo Controller software.
The hardware consist of 2 AVRs out of which one is Master and has Arduino bootloader. The second one is slave and runs all servo routines.
The master controls slave through I2C and slave operates all servos.
Many pins of the main controller are free and can be used for connecting other peripherals and interfaces.

Firmware contains 2 parts (functions in void loop()) - 1. Run in PC controlled Mode and 2. Run User Code
The code is selected through a Jumper on Servo controller board.
You can change the user code as per your need.


This code is generated for Arduino UNO board profile and the same should be chosed while programming with Arduino IDE.
Arduino IDE veersion 1.6.5 + is recommended.

If the code is moved to other servo base platform like a robot, offsets must be set properly so that servos will move correctly as per program.

*/

#include <Wire.h>

#define servo1  (16>>1)
#define servo2  (18>>1)
#define UART_BAUD_RATE  115200
#define LED 13

#define SERIAL_BUFFER_SIZE  256

void I2C_SERVOSET(unsigned char servo_num,unsigned int servo_pos);
void I2C_SERVOREVERSE(unsigned char servo_num,unsigned char servo_dir);
void I2C_SERVOOFFSET(unsigned char servo_num,int value);
void I2C_SERVOSPEED(unsigned char value);
void I2C_SERVONUTRALSET(unsigned char servo_num,unsigned int servo_pos);
void I2C_SERVOMIN(unsigned char servo_num,unsigned int servo_pos);
void I2C_SERVOMAX(unsigned char servo_num,unsigned int servo_pos);
char I2C_SERVOEND(void);
int I2C_SERVOGET(int servo_num);
int I2C_SERVOGETOFFSET(int servo_num);
void CheckEndMovement(void);
void PCControlledCode(void);
void UserCode(void);
void LEDToggle(void);

volatile int cnt,c,servoval;
volatile char state,servobuf[36],bytecnt;

int interval=100;
unsigned long previousMillis=0;
unsigned long currentMillis = millis();
char runCode =0;  //1 for PC controlled, 0 for user controlled
char LEDState=0;

#define State_Start  0
#define State_Command  1
#define State_Servoposition  2
#define State_Speed  3
#define State_Servomin  4
#define State_Servomax  5
#define State_Servooffset  6
#define State_Servoreverse  7
#define State_Servonutral  8
#define State_ReadOffsets  9

#define ipfromesp D4
int ipstate;

void setup()
{
  cnt=0;
  int i;
  unsigned int x;
  char buffer[10],tmp,tmp1;
  float range;
  Serial.begin(UART_BAUD_RATE);
  Serial.write('S');
  Serial.write('r');
  pinMode(13, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(8, INPUT);
  digitalWrite(8,1);
  delay(500);
  sei();
  Wire.begin();
  TWSR = 3; // no prescaler
  TWBR = 18;  //Set I2C speed lower to suite I2C Servo controller
  pinMode(2,OUTPUT);
  digitalWrite(2,HIGH);
  delay(500);
  state=State_Start;
 
  if (digitalRead(8))
  {
    runCode=1;
  }
  else
  {
    runCode=0;
  }
}

void loop()
{
  if (runCode ==1)
  {
    PCControlledCode();
  }
  else
  {
    UserCode();
  LEDToggle();
  }
}


void PCControlledCode(void)
{
  currentMillis = millis();
  if(Serial.available()>0)
  {
    c=Serial.read();
    previousMillis = currentMillis;
    if(state==State_Start)
    {
      if(c==170)    
        state=State_Command;                     
    }          
    else if(state==State_Servoposition)
    {
      servobuf[bytecnt]=c;
      bytecnt++;
      if(bytecnt==36)
      {
        for(int i=1;i<19;i++)
        {
          servoval=servobuf[(i*2)-1]<<7;
          servoval=servoval+servobuf[(i*2)-2];
          I2C_SERVOSET(i,servoval);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write('r');
        CheckEndMovement();
        previousMillis = currentMillis;
      }
    }                
    else if(state==State_Speed)
    {
      I2C_SERVOSPEED(c);
      state=State_Start;
      Serial.write("rs");
      CheckEndMovement();
    }
    else if(state==State_Servomin)
    {
      servobuf[bytecnt]=c;
      bytecnt++;                    
      if(bytecnt==36)
      {
        for(int i=1;i<19;i++)
        {
          servoval=servobuf[(i*2)-1]<<7;
          servoval=servoval+servobuf[(i*2)-2];
          I2C_SERVOMIN(i,servoval);
          delay(20);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write("rn");
        CheckEndMovement();
        previousMillis = currentMillis;
      }
    }
    else if(state==State_Servomax)
    {
      servobuf[bytecnt]=c;
      bytecnt++;                    
      if(bytecnt==36)
      {
        for(int i=1;i<19;i++)
        {
          servoval=servobuf[(i*2)-1]<<7;
          servoval=servoval+servobuf[(i*2)-2];
          I2C_SERVOMAX(i,servoval);
          delay(20);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write("rx");
        CheckEndMovement();
        previousMillis = currentMillis;
      }  
    }             
    else if(state==State_Servooffset)
    {
      servobuf[bytecnt]=c;
      bytecnt++;                    
      if(bytecnt==36)
      {
        for(int i=1;i<19;i++)
        {
          servoval=servobuf[(i*2)-1]<<7;
          servoval=servoval+servobuf[(i*2)-2];
          I2C_SERVOOFFSET(i,servoval);
          delay(20);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write("ro");
        CheckEndMovement();
        previousMillis = currentMillis;
      }
    }
    else if(state==State_Servoreverse)
    {
      servobuf[bytecnt]=c;
      bytecnt++;                                      
      if(bytecnt==18)
      {
        for(int i=1;i<19;i++)
        {
          I2C_SERVOREVERSE(i,servobuf[(i-1)]);
          delay(20);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write("rv");
        CheckEndMovement();
        previousMillis = currentMillis;
      }
    }           
    else if(state==State_Servonutral)
    {
      servobuf[bytecnt]=c;
      bytecnt++;                    
      if(bytecnt==36)
      {
        for(int i=1;i<19;i++)
        {
          servoval=servobuf[(i*2)-1]<<7;
          servoval=servoval+servobuf[(i*2)-2];
          I2C_SERVONUTRALSET(i,servoval);
        }
        bytecnt=0;
        state=State_Start;
        Serial.write("rn");
        CheckEndMovement();
        previousMillis = currentMillis;
      }  
    }
    else if(state==State_ReadOffsets)
    {
      Serial.write("O");
      for(int i=1;i<19;i++)
      {
        Serial.print(I2C_SERVOGETOFFSET(i));
        Serial.write(",");
        delay(1);
      }
      Serial.write("F");
      state=State_Start;
      Serial.write('r');
      CheckEndMovement();
      previousMillis = currentMillis;
    }
    else if(state==State_Command)
    {
        if(c==8)
        state=State_Speed;
        if(c==10)
        state=State_Servoposition;
        if(c==11)
        state=State_Servomin;
        if(c==12)
        state=State_Servomax;
        if(c==13)
        state=State_Servooffset;
        if(c==14)
        state=State_Servoreverse;                        
        if(c==15)
        state=State_Servonutral;
        if(c==21)
        state=State_ReadOffsets;
        bytecnt=0;                     
    }
    else
    {
      Serial.write('e');
      CheckEndMovement();
    }
  }
  else
  {
    if ((unsigned long)(currentMillis - previousMillis) >= interval)
    {
      Serial.write('i');
      Serial.write('r');
      CheckEndMovement();
      previousMillis = currentMillis;
    }
  }  
}

void CheckEndMovement(void)
{
  if(I2C_SERVOEND())
    Serial.write('X');
  else
    Serial.write('G');

  LEDToggle();
}

void I2C_SERVOSET(unsigned char servo_num,unsigned int servo_pos)
{
  if(servo_pos<500)
    servo_pos = 500;
  else if(servo_pos>2500)
    servo_pos=2500;

  if(servo_pos>501)
    servo_pos=(((servo_pos-2)*2)-1000);
  else
    servo_pos=0;

  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write(servo_num-1);
  Wire.write(servo_pos>>8);
  Wire.write(servo_pos & 0XFF);
  Wire.endTransmission();
}

void I2C_SERVOMIN(unsigned char servo_num,unsigned int servo_pos)
{
  if(servo_pos<500)
    servo_pos = 500;
  else if(servo_pos>2500)
    servo_pos=2500;
  servo_pos=((servo_pos*2)-1000);

  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write((servo_num-1)+(18*4));
  Wire.write(servo_pos>>8);
  Wire.write(servo_pos & 0XFF);
  Wire.endTransmission();
  delay(20);
}

void I2C_SERVOMAX(unsigned char servo_num,unsigned int servo_pos)
{
  if(servo_pos<500)
    servo_pos = 500;
  else if(servo_pos>2500)
    servo_pos=2500;
  servo_pos=((servo_pos*2)-1000);

  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write((servo_num-1)+(18*3));
  Wire.write(servo_pos>>8);
  Wire.write(servo_pos & 0XFF);
  Wire.endTransmission();
  delay(20);
}

void I2C_SERVONUTRALSET(unsigned char servo_num,unsigned int servo_pos)
{
  if(servo_pos<500)
    servo_pos = 500;
  else if(servo_pos>2500)
    servo_pos=2500;
  servo_pos=((servo_pos*2)-1000);

  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write((servo_num-1)+(18*5));
  Wire.write(servo_pos>>8);
  Wire.write(servo_pos & 0XFF);
  Wire.endTransmission();
}

void I2C_SERVOSPEED(unsigned char value)
{
  Wire.beginTransmission(servo1);
  Wire.write(18*2);
  Wire.write(value);
  Wire.write(0);
  Wire.endTransmission();
  Wire.beginTransmission(servo2);
  Wire.write(18*2);
  Wire.write(value);
  Wire.write(0);
  Wire.endTransmission();
  delay(20);
}

void I2C_SERVOOFFSET(unsigned char servo_num,int value)
{
  value=3000-value;
  value=value-1500;

  if (value<-500)
    value=-500;
  else if (value>500)
    value=500;

  if(value>0)
    value=2000+(value*2);
  else if(value<=0)
    value=-value*2;

 
  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write((servo_num-1)+(18*6));
  Wire.write(value>>8);
  Wire.write(value & 0XFF);
  Wire.endTransmission();
  delay(20);
}

void I2C_SERVOREVERSE(unsigned char servo_num,unsigned char servo_dir)
{
  if(servo_dir>0)
    servo_dir=1;
  if(servo_num<19)
    Wire.beginTransmission(servo1);
  else
    Wire.beginTransmission(servo2);
  Wire.write((servo_num-1)+(18*7));
  Wire.write(servo_dir);
  Wire.write(0);
  Wire.endTransmission();
  delay(20);
}

char I2C_SERVOEND(void)
{
  int i, n;
  char buffer;
  Wire.beginTransmission(servo1);
  n = Wire.write(181);
  if (n != 1)
    return (-10);

  n = Wire.endTransmission(false);
  if (n != 0)
    return (n);

  delayMicroseconds(350);
  Wire.requestFrom(servo1, 1, true);
  while(Wire.available())
    buffer=Wire.read();

  return(buffer);
}

int I2C_SERVOGET(int servo_num)
{
  int i, n, error;
  uint8_t buffer[2];
  Wire.beginTransmission(servo1);

  n = Wire.write((servo_num-1)+(18*8));
  if (n != 1)
    return (-10);

  n = Wire.endTransmission(false);
  if (n != 0)
    return (n);

  delayMicroseconds(240);
  Wire.requestFrom(servo1, 2, true);
  i = 0;
  while(Wire.available() && i<2)
  {
    buffer[i++]=Wire.read();
  }
  if ( i != 2)
    return (-11);
  return (((buffer[0]*256 + buffer[1])+4)/2 +500);
}

int I2C_SERVOGETOFFSET(int servo_num)
{
  int i, n, error;
  uint8_t buffer[2];
  Wire.beginTransmission(servo1);

  n = Wire.write((servo_num-1)+(182));
  if (n != 1)
    return (-10);

  n = Wire.endTransmission(false);
  if (n != 0)
    return (n);

  delayMicroseconds(240);
  Wire.requestFrom(servo1, 2, true);
  i = 0;
  while(Wire.available() && i<2)
  {
    buffer[i++]=Wire.read();
  }
  if ( i != 2)
    return (-11);
  i=((buffer[0]*256 + buffer[1]));
  if(i>2000)
    return(3000-(((i-2000)/2)+1500));
  else
    return(3000-((-i/2)+1500));
}


void ServoSetAll(unsigned int Servo1, unsigned int Servo2, unsigned int Servo3, unsigned int Servo4, unsigned int Servo5, unsigned int Servo6, unsigned int Servo7, unsigned int Servo8, unsigned int Servo9, unsigned int Servo10, unsigned int Servo11, unsigned int Servo12, unsigned int Servo13, unsigned int Servo14, unsigned int Servo15, unsigned int Servo16, unsigned int Servo17, unsigned int Servo18)
{
  if (Servo1 >= 500) {I2C_SERVOSET(1,Servo1);}
  if (Servo2 >= 500) {I2C_SERVOSET(2,Servo2);}
  if (Servo3 >= 500) {I2C_SERVOSET(3,Servo3);}
  if (Servo4 >= 500) {I2C_SERVOSET(4,Servo4);}
  if (Servo5 >= 500) {I2C_SERVOSET(5,Servo5);}
  if (Servo6 > 500) {I2C_SERVOSET(6,Servo6);}
  if (Servo7 >= 500) {I2C_SERVOSET(7,Servo7);}
  if (Servo8 >= 500) {I2C_SERVOSET(8,Servo8);}
  if (Servo9 >= 500) {I2C_SERVOSET(9,Servo9);}
  if (Servo10 >= 500) {I2C_SERVOSET(10,Servo10);}
  if (Servo11 >= 500) {I2C_SERVOSET(11,Servo11);}
  if (Servo12 >= 500) {I2C_SERVOSET(12,Servo12);}
  if (Servo13 >= 500) {I2C_SERVOSET(13,Servo13);}
  if (Servo14 >= 500) {I2C_SERVOSET(14,Servo14);}
  if (Servo15 >= 500) {I2C_SERVOSET(15,Servo15);}
  if (Servo16 >= 500) {I2C_SERVOSET(16,Servo16);}
  if (Servo17 >= 500) {I2C_SERVOSET(17,Servo17);}
  if (Servo18 >= 500) {I2C_SERVOSET(18,Servo18);}
  while (!I2C_SERVOEND())
  {
    delay(1);
  }
  LEDToggle();
}

void LEDToggle(void)
{
  if (LEDState == 0)
    LEDState = 1;
  else
    LEDState = 0;
  digitalWrite(LED, LEDState);
}

void MoveServos(void)
{
 
//------------------------------OIL SPREADING PROCESS------------------------------

  delay(1000);    // Line # 0
  ServoSetAll(2195,1246,2169,1350,1842,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);    // Line # 5
  delay(300);   //Default Delay
  delay(800);   // Line # 6
  ServoSetAll(2195,1201,1980,1350,1704,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);   // Line # 7
  delay(300);   //Default Delay
  delay(800);   // Line # 8
  ServoSetAll(2195,1174,1920,1350,1713,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);   // Line # 9
  delay(300);   //Default Delay
  delay(200);   // Line # 10
  ServoSetAll(2195,1174,1920,1350,1713,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2067);   // Line # 11
  delay(300);   //Default Delay
  delay(600);   // Line # 12
  ServoSetAll(2195,1279,1920,1350,1656,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2069);   // Line # 13
  delay(300);   //Default Delay
  delay(600);   // Line # 14
  ServoSetAll(2195,1722,1920,1350,1554,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2069);   // Line # 15
  delay(300);   //Default Delay
  delay(800);   // Line # 16
  ServoSetAll(1688,1546,1743,1350,1401,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2087);   // Line # 17
  delay(300);   //Default Delay
  delay(1000);    // Line # 18
  ServoSetAll(1688,1792,2109,1350,1221,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2087);   // Line # 19
  delay(300);   //Default Delay
  delay(600);   // Line # 20
  ServoSetAll(1622,1711,2070,1350,1203,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2087);   // Line # 21
  delay(300);   //Default Delay
  delay(100);   // Line # 22
  delay(100);   // Line # 23
  delay(100);   // Line # 24
  ServoSetAll(2195,1279,1920,1350,1656,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,2093);   // Line # 25
  delay(300);   //Default Delay
  delay(800);   // Line # 26
  ServoSetAll(2195,1174,1920,1350,1713,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);   // Line # 27
  delay(300);   //Default Delay
  delay(600);   // Line # 28
  ServoSetAll(2195,1246,2169,1350,1842,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);   // Line # 29
  delay(300);   //Default Delay
  delay(800);   // Line # 30
  ServoSetAll(1781,1500,1500,1350,943,1500,1503,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500,1500);    // Line # 31
  delay(300);   //Default Delay
  delay(1000);


  //------------------------------EGG SPREADING PROCESS------------------------------

  delay(1000);    // Line # 0

  I2C_SERVOSPEED(82);   // Line # 7
  ServoSetAll(1500,2045,0,2500,1500,0,955,0,0,500,0,0,0,0,0,0,0,0);   // Line # 8
  delay(300);   //Default Delay
  delay(100);   // Line # 9
  ServoSetAll(554,1742,0,2500,1500,0,1231,0,0,500,0,0,0,0,0,0,0,0);   // Line # 10
  delay(300);   //Default Delay
  delay(100);   // Line # 11
  ServoSetAll(569,1335,0,2500,1500,0,1582,0,0,500,0,0,0,0,0,0,0,0);   // Line # 12
  delay(300);   //Default Delay
  delay(100);   // Line # 13
  ServoSetAll(590,1281,0,2374,1500,0,1543,0,0,500,0,0,0,0,0,0,0,0);   // Line # 14
  delay(300);   //Default Delay
  delay(100);   // Line # 15
  ServoSetAll(569,1254,0,2332,1500,0,1594,0,0,500,0,0,0,0,0,0,0,0);   // Line # 16
  delay(300);   //Default Delay
  delay(100);   // Line # 17
  ServoSetAll(569,1254,0,2332,1500,0,1594,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 18
  delay(300);   //Default Delay
  delay(100);   // Line # 19
  ServoSetAll(569,1458,0,2383,1500,0,1429,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 20
  delay(300);   //Default Delay
  delay(2000);    // Line # 21
  I2C_SERVOSPEED(20);   // Line # 22
  delay(2000);    // Line # 23
  ServoSetAll(611,1824,0,2134,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 24
  delay(300);   //Default Delay
  delay(2000);    // Line # 25
  delay(2000);    // Line # 26
  ServoSetAll(1539,1824,0,2134,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 27
  delay(300);   //Default Delay
  delay(100);   // Line # 28
  ServoSetAll(1539,1431,0,1729,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 29
  delay(300);   //Default Delay
  delay(100);   // Line # 30
  I2C_SERVOSPEED(80);   // Line # 31
  ServoSetAll(1710,1350,0,1829,500,0,835,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 32
  delay(300);   //Default Delay
  delay(2000);    // Line # 33
  delay(100);   // Line # 34
  ServoSetAll(1539,1431,0,1729,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 35
  delay(300);   //Default Delay
  delay(100);   // Line # 36
  ServoSetAll(1539,1824,0,2134,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 37
  delay(300);   //Default Delay
  delay(100);   // Line # 38
  ServoSetAll(611,1824,0,2134,1500,0,835,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 39
  delay(300);   //Default Delay
  delay(100);   // Line # 40
  ServoSetAll(569,1458,0,2383,1500,0,1429,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 41
  delay(300);   //Default Delay
  delay(100);   // Line # 42
  ServoSetAll(587,1284,0,2335,1500,0,1528,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 43
  delay(300);   //Default Delay
  delay(100);   // Line # 44
  ServoSetAll(587,1272,0,2362,1500,0,1528,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 45
  delay(300);   //Default Delay
  ServoSetAll(587,1245,0,2362,1500,0,1528,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 46
  delay(300);   //Default Delay
  delay(100);   // Line # 47
  ServoSetAll(587,1245,0,2362,1500,0,1528,0,0,500,0,0,0,0,0,0,0,0);   // Line # 48
  delay(300);   //Default Delay
  delay(100);   // Line # 49
  ServoSetAll(584,1269,0,2380,1500,0,1576,0,0,500,0,0,0,0,0,0,0,0);   // Line # 50
  delay(300);   //Default Delay
  delay(100);   // Line # 51
  ServoSetAll(584,1293,0,2410,1500,0,1546,0,0,500,0,0,0,0,0,0,0,0);   // Line # 52
  delay(300);   //Default Delay
  delay(100);   // Line # 53
  ServoSetAll(569,1335,0,2500,1500,0,1582,0,0,500,0,0,0,0,0,0,0,0);   // Line # 54
  delay(300);   //Default Delay
  delay(100);   // Line # 55
  ServoSetAll(1500,2045,0,2500,1500,0,955,0,0,500,0,0,0,0,0,0,0,0);   // Line # 56
  delay(300);   //Default Delay
  delay(1000);

//------------------------------SPATULA PICKING PROCESS------------------------------

  delay(1000);    // Line # 0
  I2C_SERVOSPEED(75);   // Line # 7
  ServoSetAll(1500,2045,0,2500,1500,0,955,0,0,500,0,0,0,0,0,0,0,0);   // Line # 8
  delay(300);   //Default Delay
  delay(100);   // Line # 9
  ServoSetAll(2159,1715,0,2500,1500,0,1231,0,0,500,0,0,0,0,0,0,0,0);    // Line # 10
  delay(300);   //Default Delay
  delay(100);   // Line # 11
  ServoSetAll(2192,1398,0,2500,1500,0,1626,0,0,500,0,0,0,0,0,0,0,0);    // Line # 12
  delay(300);   //Default Delay
  delay(100);   // Line # 13
  ServoSetAll(2192,1248,0,2338,1500,0,1582,0,0,500,0,0,0,0,0,0,0,0);    // Line # 14
  delay(300);   //Default Delay
  delay(100);   // Line # 15
  ServoSetAll(2192,1230,0,2287,1500,0,1552,0,0,500,0,0,0,0,0,0,0,0);    // Line # 16
  delay(300);   //Default Delay
  delay(100);   // Line # 17
  ServoSetAll(2192,1230,0,2287,1500,0,1552,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 18
  delay(300);   //Default Delay
  delay(100);   // Line # 19
  ServoSetAll(2192,1302,0,2311,1500,0,1540,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 20
  delay(300);   //Default Delay
  I2C_SERVOSPEED(30);   // Line # 21
  delay(100);   // Line # 22
  ServoSetAll(2192,1779,0,2500,1500,0,1390,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 23
  delay(300);   //Default Delay
  I2C_SERVOSPEED(50);   // Line # 24
  delay(100);   // Line # 25
  ServoSetAll(1645,1779,0,2500,1500,0,1390,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 26
  delay(300);   //Default Delay
  delay(100);   // Line # 27
  I2C_SERVOSPEED(30);   // Line # 28
  ServoSetAll(1666,2180,0,2500,1500,0,527,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 29
  delay(300);   //Default Delay
  delay(100);   // Line # 30
  ServoSetAll(1666,2000,0,2500,1500,0,527,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 31
  delay(300);   //Default Delay
  ServoSetAll(1666,1973,0,2500,1500,0,617,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 32
  delay(300);   //Default Delay
  ServoSetAll(1666,1856,0,2377,1500,0,662,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 33
  delay(300);   //Default Delay
  ServoSetAll(1666,1823,0,2371,1500,0,698,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 34
  delay(300);   //Default Delay
  ServoSetAll(1666,1745,0,2386,1500,0,998,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 35
  delay(300);   //Default Delay
  ServoSetAll(1666,1685,0,2386,1500,0,1040,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 36
  delay(300);   //Default Delay
  delay(100);   // Line # 37
  ServoSetAll(1639,1514,0,2212,1461,0,1163,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 38
  delay(300);   //Default Delay
  delay(100);   // Line # 39
  ServoSetAll(1639,1679,0,2212,1461,0,1115,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 40
  delay(300);   //Default Delay
  delay(100);   // Line # 41
  ServoSetAll(940,1679,0,2212,1461,0,1115,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 42
  delay(300);   //Default Delay
  delay(100);   // Line # 43
  I2C_SERVOSPEED(45);   // Line # 44
  ServoSetAll(940,1451,0,2212,1461,0,893,0,0,1178,0,0,0,0,0,0,0,0);   // Line # 45
  delay(300);   //Default Delay
  delay(100);   // Line # 46
  ServoSetAll(940,1679,0,2212,1461,0,1115,0,0,1178,0,0,0,0,0,0,0,0);    // Line # 47
  delay(300);   //Default Delay
  delay(100);   // Line # 48
  ServoSetAll(1500,2045,0,2500,1500,0,955,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 49
  delay(300);   //Default Delay
  ServoSetAll(2184,2045,0,2500,1500,0,955,0,0,1194,0,0,0,0,0,0,0,0);    // Line # 50
  delay(300);   //Default Delay
  delay(100);   // Line # 51
  ServoSetAll(2192,1241,0,2317,1500,0,1564,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 52
  delay(300);   //Default Delay
  delay(100);   // Line # 53
  ServoSetAll(2192,1274,0,2317,1500,0,1564,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 54
  delay(300);   //Default Delay
  delay(100);   // Line # 55
  ServoSetAll(2192,1241,0,2317,1500,0,1564,0,0,1194,0,0,0,0,0,0,0,0);   // Line # 56
  delay(300);   //Default Delay
  delay(100);   // Line # 57
  ServoSetAll(2192,1241,0,2317,1500,0,1564,0,0,500,0,0,0,0,0,0,0,0);    // Line # 58
  delay(300);   //Default Delay
  delay(100);   // Line # 59
  ServoSetAll(2192,1271,0,2395,1500,0,1555,0,0,500,0,0,0,0,0,0,0,0);    // Line # 60
  delay(300);   //Default Delay
  delay(100);   // Line # 61
  ServoSetAll(1500,2045,0,2500,1500,0,955,0,0,500,0,0,0,0,0,0,0,0);   // Line # 62
  delay(300);   //Default Delay
  delay(1000);
}

void UserCode(void)
{

  //------------------------------Configuration------------------------------

  I2C_SERVOMAX(1,2500); I2C_SERVOMAX(2,2500); I2C_SERVOMAX(3,2500); I2C_SERVOMAX(4,2500); I2C_SERVOMAX(5,2500); I2C_SERVOMAX(6,2500); I2C_SERVOMAX(7,2500); I2C_SERVOMAX(8,2500); I2C_SERVOMAX(9,2500); I2C_SERVOMAX(10,1194); I2C_SERVOMAX(11,2500); I2C_SERVOMAX(12,2500); I2C_SERVOMAX(13,2500); I2C_SERVOMAX(14,2500); I2C_SERVOMAX(15,2500); I2C_SERVOMAX(16,2500); I2C_SERVOMAX(17,2500); I2C_SERVOMAX(18,2500);    //Maximum Values

  I2C_SERVOMIN(1,500); I2C_SERVOMIN(2,500); I2C_SERVOMIN(3,500); I2C_SERVOMIN(4,500); I2C_SERVOMIN(5,500); I2C_SERVOMIN(6,500); I2C_SERVOMIN(7,500); I2C_SERVOMIN(8,500); I2C_SERVOMIN(9,500); I2C_SERVOMIN(10,500); I2C_SERVOMIN(11,500); I2C_SERVOMIN(12,500); I2C_SERVOMIN(13,500); I2C_SERVOMIN(14,500); I2C_SERVOMIN(15,500); I2C_SERVOMIN(16,500); I2C_SERVOMIN(17,500); I2C_SERVOMIN(18,500);    //Minimum Values

  I2C_SERVOOFFSET(1,1500); I2C_SERVOOFFSET(2,1500); I2C_SERVOOFFSET(3,1500); I2C_SERVOOFFSET(4,1500); I2C_SERVOOFFSET(5,1500); I2C_SERVOOFFSET(6,1500); I2C_SERVOOFFSET(7,1500); I2C_SERVOOFFSET(8,1500); I2C_SERVOOFFSET(9,1500); I2C_SERVOOFFSET(10,1500); I2C_SERVOOFFSET(11,1500); I2C_SERVOOFFSET(12,1500); I2C_SERVOOFFSET(13,1500); I2C_SERVOOFFSET(14,1500); I2C_SERVOOFFSET(15,1500); I2C_SERVOOFFSET(16,1500); I2C_SERVOOFFSET(17,1500); I2C_SERVOOFFSET(18,1500);    //Offset Values

  I2C_SERVOREVERSE(1,0); I2C_SERVOREVERSE(2,0); I2C_SERVOREVERSE(3,0); I2C_SERVOREVERSE(4,0); I2C_SERVOREVERSE(5,0); I2C_SERVOREVERSE(6,0); I2C_SERVOREVERSE(7,0); I2C_SERVOREVERSE(8,0); I2C_SERVOREVERSE(9,0); I2C_SERVOREVERSE(10,0); I2C_SERVOREVERSE(11,0); I2C_SERVOREVERSE(12,0); I2C_SERVOREVERSE(13,0); I2C_SERVOREVERSE(14,0); I2C_SERVOREVERSE(15,0); I2C_SERVOREVERSE(16,0); I2C_SERVOREVERSE(17,0); I2C_SERVOREVERSE(18,0);    //Directions (Servo Reverse)

 
  // Comment or remove the next 5 lines to run code in loop...
  while(1)
  {
    ipstate = digitalRead(ipfromesp);
    if(ipstate == HIGH)
    {
      MoveServos();
    }
    LEDToggle();
    delay(100);
  }

}