//atc must also control spindle on/off
//carousel homed at tool 0 postion
//steps between tools must be equal between all tools and must physically be an integer you need to work it out depending on how many steps your system has.

//required!
//STARTUP TEXT
//external drawbar contorl
//tray in switch
//profile setting from pathpilot
//help
//spindle inhibit
//drawbat state switches
//spindle in motion pulsed and level
//error handling...all of it
//








#include <EEPROM.h>



#define carousel_step_pin 12
#define carousel_dir_pin 13
#define tray_step_pin 10
#define tray_dir_pin 11
#define carousel_ref_pin 9
#define tray_ref_pin 8
#define air_blast_pin 7
#define drawbar_pin 6
#define spindle_stop_pin 5
#define sol4_pin 4


const byte numChars = 32;
char receivedChars[numChars];   // an array to store the received data

bool newData = false;

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

pinMode(carousel_step_pin, OUTPUT);
digitalWrite(carousel_step_pin, LOW);
pinMode(carousel_dir_pin, OUTPUT);
digitalWrite(carousel_dir_pin, LOW);
pinMode(tray_step_pin, OUTPUT);
digitalWrite(tray_step_pin, LOW);
pinMode(tray_dir_pin, OUTPUT);
pinMode(air_blast_pin, OUTPUT);
digitalWrite(air_blast_pin, LOW);
pinMode(drawbar_pin, OUTPUT);
digitalWrite(drawbar_pin, LOW);
pinMode(spindle_stop_pin, OUTPUT);
digitalWrite(spindle_stop_pin, LOW);
pinMode(sol4_pin, OUTPUT);
digitalWrite(sol4_pin, LOW);









digitalWrite(tray_dir_pin, LOW);
pinMode(carousel_ref_pin,INPUT_PULLUP);
pinMode(tray_ref_pin,INPUT_PULLUP);

  
read_mem();


    
}

int profile;


char sol1stat='-';
char sol2stat='-';
char sol3stat='-';
char sol4stat='-';
char blasterstat='-';
char drawbarstat='-';
char traystat='-';
char homestat='-';  
char pendantstat='-';
char rpmstat='0'; //seems to have no effect
char vfd_running='+'; //+=spindle moving,-=stationary
char airstat='-';

bool vfd_detect_mode=1;//vfd detect mode, 1= level,0=pulsed
bool set_sol1=0;
bool set_sol2=0;
bool set_sol3=0;
bool set_sol4=0;
bool set_drawbar=0;
bool set_carousel_forward=0;
bool set_carousel_reverse=0;
bool set_tray_forward=0;
bool set_tray_reverse=0;
bool set_find_home=0;
bool set_air_blast=0;
bool set_tray_find_ref=0;
bool set_carousel_find_ref=0;
bool tray_homed=0;
bool carousel_homed=0;

float tray_ramp_steps;
float carousel_ramp_steps;
float ramp_unit;
float start_step_delay=2000;  
float step_delay;
long tray_distance;
long carousel_distance;
long current_pos=0;
long value=0; 

int required_tool;
int carousel_max_speed;
int carousel_speed;
int carousel_ref_speed;
int tool=0;
int tray_steps;
int carousel_steps;
int tray_max_speed;
int tray_speed;
int tray_ref_speed;
int tray_ref_offset;
int data_array[7];
int data;
int carousel_ref_offset;
int carousel_ref_off;
int tray_ref_off;
int number_of_tools;










void loop() {
//force ok
pendantstat='-'; //no pendant
rpmstat='+'; //no effect
vfd_running='-'; //spindle not moving
airstat='-'; //air ok is a minus
vfd_detect_mode=1;




recvWithEndMarker();
showNewData();
 

if (set_find_home==1){
homestat='-';
tray_homed=0;
carousel_homed=0;
if (!digitalRead(tray_ref_pin)) {
tray_distance=tray_ref_off;
tray_forward();
}
tray_distance=tray_steps*2;
tray_ref();
tray_distance=tray_ref_offset;
tray_reverse();
if (!digitalRead(carousel_ref_pin)) {
carousel_distance=carousel_ref_off;
carousel_forward();
}
carousel_distance=carousel_steps*11;
carousel_ref();
if (carousel_homed==1){
if (carousel_ref_offset > 0){
carousel_distance=carousel_ref_offset;
carousel_forward();
}
if (carousel_ref_offset < 0){
carousel_distance=-carousel_ref_offset;
carousel_reverse();
}
tool=0;
}
if (tray_homed==1 && carousel_homed==1){homestat='+';}
set_find_home=0;
}

if (set_drawbar==1){
drawbarstat='+'; //drawbar actual state is set to keep pathpilot happy...need switch 
digitalWrite(drawbar_pin, HIGH);
}

if (set_drawbar==0){
drawbarstat='-'; //drawbar actual state is un set to keep pathpilot happy...need switch
digitalWrite(drawbar_pin, LOW);
}

if (set_tray_forward==1){ 
sol1stat='+';  
tray_distance=tray_steps;
tray_speed=tray_max_speed;
tray_forward();
traystat='+'; 
set_tray_forward=0;
}

if (set_tray_reverse==1){
if (digitalRead(tray_ref_pin)){
sol1stat='-';   
tray_distance=tray_steps;
tray_speed=tray_max_speed;
tray_reverse();
set_tray_reverse=0; 
}
}
if (!digitalRead(tray_ref_pin)) { 
traystat='-';  
set_tray_reverse=0;
}


if (!digitalRead(tray_ref_pin)) { 
traystat='-';  
}

if (!set_carousel_forward && !set_carousel_reverse){tool_calc();}


if (set_carousel_forward==1){
carousel_speed=carousel_max_speed;
carousel_forward();
tool=required_tool;
set_carousel_forward=0;
}

if (set_carousel_reverse==1){
carousel_speed=carousel_max_speed;  
carousel_reverse();
tool=required_tool;
set_carousel_reverse=0;
}

if (set_air_blast==1){
blasterstat='+';
digitalWrite(air_blast_pin, HIGH);
}

if (set_air_blast==0){
blasterstat='-';
digitalWrite(air_blast_pin, LOW);
}

//spindle stop


    
}//end loop

//data in
void recvWithEndMarker() {
static byte ndx = 0;
char endMarker = '\r';
char rc;
while (Serial.available() > 0 && newData == false) {
rc = Serial.read();
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}

void showNewData() {
if (newData == true) {

if (!set_tray_forward && !set_tray_reverse){
if (receivedChars[0]=='S' && receivedChars[1]=='T'){  //if "ST" then send the data below...
  
Serial.print("T");
Serial.print(tool);
Serial.print(", ");

Serial.print("H");
Serial.print(homestat);
Serial.print(", ");
         
Serial.print("D");   
Serial.print(drawbarstat);      
Serial.print(", ");
         
Serial.print("C");
Serial.print(traystat);
Serial.print(", ");
         
Serial.print("P");
Serial.print(airstat);
Serial.print(", ");
         
Serial.print("A");
Serial.print(blasterstat);
Serial.print(", ");
         
Serial.print("V");
Serial.print(vfd_running);
Serial.print(", ");
         
Serial.print("B");
Serial.print(pendantstat);
Serial.print(", ");
         
Serial.print("1");
Serial.print(sol1stat);
Serial.print(", ");
         
Serial.print("4");
Serial.print(sol4stat);
Serial.print(", ");

Serial.print("X");
Serial.print("-"); 
Serial.print(", ");

Serial.print("Y");
Serial.print("-"); 
Serial.print(", ");
         
Serial.print("R");
Serial.print(rpmstat); 
Serial.print(", ");

Serial.print("\r\n");

}

}else{
Serial.print('.');
Serial.print("\r\n");  
}
  
//if "VE" then send the data below... data must match ini data..otherwise pp will send commands to change the profile details...
//the profile setting is currently hard coded but should be set via pp...this has not been finished.

if (receivedChars[0]=='V' && receivedChars[1]=='E'){   
Serial.print("Z-Bot Automatic Tool Changer II ");
Serial.print("3.0.8");
if (vfd_detect_mode==1){Serial.print(" - VFD: Level");}
Serial.print(" TOOLS: ");
Serial.print(number_of_tools);
Serial.print("\r\n");  


}

//if "VE" then send the data below... data must match ini data..otherwise pp will send commands to change the profile details...
//the profile setting is currently hard coded but should be set via pp...this has not been finished.


if (receivedChars[0]=='V' && receivedChars[1]=='L'){
Serial.print ("Z-Bot Automatic Tool Changer II ""3.0.8""\r\n");  
Serial.print ("PROFILE: ");
Serial.print("10-TOOL-VL");  //need to read from profile memory 
Serial.print("\r\n"); 
Serial.print("TOOLS: ");
Serial.print(number_of_tools);  //need to read from profile memory
Serial.print("\r\n");             
Serial.print("MAXSPEED: ");
Serial.print(carousel_max_speed);
Serial.print("\r\n");       
Serial.print("MINSPEED: ""125""\r\n");        //need to read from profile memory                 
Serial.print("ACCEL: ""30""\r\n");            //need to read from profile memory           
Serial.print("DECEL: ""30""\r\n");            //need to read from profile memory
Serial.print("CIRCUMFERENCE: ""300""\r\n");   //not required
Serial.print("HOMEOFFSET: ");
Serial.print(carousel_ref_offset);
Serial.print("\r\n");    
Serial.print("VFD: ");
if (vfd_detect_mode==1){Serial.print("LEVEL");}
if (vfd_detect_mode==0){Serial.print("PULSED");}
Serial.print("\r\n");          
Serial.print("DRIVER: ""ON-BOARD""\r\n");     //need to read from profile memory
Serial.print("Copyright (c) 6019 Z-Bot, LLC\r\n""\r\n");     
}

//solenoids

         if (receivedChars[0]=='1' && receivedChars[1]=='+'){
         Serial.print(". \r\n");
         set_tray_forward=1;
         }
         
         if (receivedChars[0]=='1' && receivedChars[1]=='-'){
         Serial.print(". \r\n");
         set_tray_reverse=1;
         }

         if (receivedChars[0]=='2' && receivedChars[1]=='+'){
         Serial.print(". \r\n");
         set_air_blast=1;
         }

         if (receivedChars[0]=='2' && receivedChars[1]=='-'){
         Serial.print(". \r\n");
         set_air_blast=0;
         }

         //if (receivedChars[0]=='3' && receivedChars[1]=='+'){
         //Serial.print(". \r\n");
         //set_drawbar=1;
         //}

         //if (receivedChars[0]=='3' && receivedChars[1]=='-'){
         //Serial.print(". \r\n");
         //set_drawbar=0;
         //}

         if (receivedChars[0]=='4' && receivedChars[1]=='+'){
         Serial.print(". \r\n");
         sol4stat='+';
         }

         if (receivedChars[0]=='4' && receivedChars[1]=='-'){
         Serial.print(". \r\n");;          
         sol4stat='-';
         }

         if (receivedChars[0]=='D' && receivedChars[1]=='+'){
         Serial.print(". \r\n");
         set_drawbar=1;
         }

         if (receivedChars[0]=='D' && receivedChars[1]=='-'){
         Serial.print(". \r\n");
         set_drawbar=0;
         }

         if (receivedChars[0]=='F' && receivedChars[1]=='H'){
         Serial.print(". \r\n");
         set_find_home=1;
         }         
         
         if (receivedChars[0]=='H' && receivedChars[1]=='+'){
         Serial.print(". \r\n"); 
         digitalWrite(carousel_step_pin,1);
         delayMicroseconds(15);  
         digitalWrite(carousel_step_pin,0);
         delayMicroseconds(step_delay); 
         carousel_ref_offset++;
         EEPROM.put(48,carousel_ref_offset);
         }

         if (receivedChars[0]=='H' && receivedChars[1]=='-'){
         Serial.print(". \r\n");
         digitalWrite(carousel_dir_pin,1); 
         delayMicroseconds(100);
         digitalWrite(carousel_step_pin,1);
         delayMicroseconds(15);  
         digitalWrite(carousel_step_pin,0);
         delayMicroseconds(step_delay); 
         delayMicroseconds(100);
         digitalWrite(carousel_dir_pin,0);
         carousel_ref_offset--;
         EEPROM.put(48,carousel_ref_offset);
         }

//tooL

         if (receivedChars[0]=='T'){
          
         if (receivedChars[1]=='0'){
         required_tool=0;   
         Serial.print(". \r\n");          
         }          
          
         if (receivedChars[1]=='1' && receivedChars[2]==0){
         required_tool=1;   
         Serial.print(". \r\n");         
         }
         
         if (receivedChars[1]=='2'){
         required_tool=2; 
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='3'){
         required_tool=3;       
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='4'){
         required_tool=4; 
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='5'){
         required_tool=5;      
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='6'){
         required_tool=6;
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='7'){
         required_tool=7;
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='8'){
         required_tool=8;
         Serial.print(". \r\n");          
         }
         
         if (receivedChars[1]=='9'){
         required_tool=9;
         Serial.print(". \r\n");          
         }

         if (receivedChars[1]=='1' && receivedChars[2]=='0'){
         required_tool=10;
         Serial.print(". \r\n");  
         }  
               
         if (receivedChars[1]=='1' && receivedChars[2]=='1'){
         required_tool=11;
         Serial.print(". \r\n");  
         }  

         if (receivedChars[1]=='1' && receivedChars[2]=='2'){
         required_tool=12;
         Serial.print(". \r\n");  
         }

         if (receivedChars[1]=='1' && receivedChars[2]=='3'){
         required_tool=13;
         Serial.print(". \r\n");  
         }         

         if (receivedChars[1]=='1' && receivedChars[2]=='4'){
         required_tool=14;
         Serial.print(". \r\n");  
         }

         if (receivedChars[1]=='1' && receivedChars[2]=='5'){
         required_tool=15;
         Serial.print(". \r\n");  
         }
         
         
         }











//****************************DATA ENTRY VIA SERIAL**********************
//MUST BE FIVE DIGITS E.G 00460 or 12345
//if enter is pressed without a number being entered then data is read only

//tray steps
if (receivedChars[0]=='t' && receivedChars[1]=='s'){
Serial.print("\r\n");
if (receivedChars[2]<48){   //numbers only
Serial.print ("tray steps: "); 
Serial.print (tray_steps);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_steps=value;
value=0;
Serial.print("tray steps new value: ");
Serial.print(tray_steps);
Serial.print("\r\n");
EEPROM.put(0,tray_steps);
}   
} 

//tray speed
if (receivedChars[0]=='t' && receivedChars[1]=='v'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("tray velocity: "); 
Serial.print (100000/tray_max_speed);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_max_speed=100000/value;
value=0;
Serial.print("tray velocity new value: ");
Serial.print(100000/tray_max_speed);
Serial.print("\r\n");
EEPROM.put(8,tray_max_speed);
}   
}

//tray accel
if (receivedChars[0]=='t' && receivedChars[1]=='a'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("tray accel: "); 
Serial.print (100000/tray_ramp_steps);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_ramp_steps=100000/value;
value=0;
Serial.print("tray accel new value: ");
Serial.print(100000/tray_ramp_steps);
Serial.print("\r\n");
EEPROM.put(16,tray_ramp_steps);
}   
}

//carousel steps
if (receivedChars[0]=='c' && receivedChars[1]=='s'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("carousel_steps: "); 
Serial.print (carousel_steps);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
carousel_steps=value;
value=0;
Serial.print("carousel steps new value: ");
Serial.print(carousel_steps);
Serial.print("\r\n");
EEPROM.put(24,carousel_steps);
}   
} 

//carousel speed
if (receivedChars[0]=='c' && receivedChars[1]=='v'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("carousel velocity: "); 
Serial.print (100000/carousel_max_speed);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
}
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
carousel_max_speed=100000/value;
value=0;
Serial.print("carousel velocity new value: ");
Serial.print(100000/carousel_max_speed);
Serial.print("\r\n");
EEPROM.put(32,carousel_max_speed);
}   
} 

//carousel accel
if (receivedChars[0]=='c' && receivedChars[1]=='a'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("carousel_accel: "); 
Serial.print (100000/carousel_ramp_steps);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
carousel_ramp_steps=100000/value;
value=0;
Serial.print("carousel accel new value: ");
Serial.print(100000/carousel_ramp_steps);
Serial.print("\r\n");
EEPROM.put(40,carousel_ramp_steps);
}   
} 

//carousel ref speed
if (receivedChars[0]=='c' && receivedChars[1]=='r'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("carousel ref speed: "); 
Serial.print (100000/carousel_ref_speed);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
carousel_ref_speed=100000/value;
value=0;
Serial.print("carousel ref speed new value: ");
Serial.print(100000/carousel_ref_speed);
Serial.print("\r\n");
EEPROM.put(56,carousel_ref_speed);
}   
} 

//carousel ref off
if (receivedChars[0]=='c' && receivedChars[1]=='b'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("carousel ref back off steps: "); 
Serial.print (carousel_ref_off);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
carousel_ref_off=value;
value=0;
Serial.print("carousel ref back off steps new value: ");
Serial.print(carousel_ref_off);
Serial.print("\r\n");
EEPROM.put(64,carousel_ref_off);
}   
}




//tray ref speed
if (receivedChars[0]=='t' && receivedChars[1]=='r'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("tray ref speed: "); 
Serial.print (100000/tray_ref_speed);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_ref_speed=100000/value;
value=0;
Serial.print("tray ref speed new value: ");
Serial.print(100000/tray_ref_speed);
Serial.print("\r\n");
EEPROM.put(72,tray_ref_speed);
}   
} 

//tray ref off
if (receivedChars[0]=='t' && receivedChars[1]=='b'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("tray ref back off steps: "); 
Serial.print (tray_ref_off);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_ref_off=value;
value=0;
Serial.print("tray ref back off steps new value: ");
Serial.print(tray_ref_off);
Serial.print("\r\n");
EEPROM.put(86,tray_ref_off);
}   
}

//tray ref offset
if (receivedChars[0]=='t' && receivedChars[1]=='o'){
Serial.print("\r\n");
if (receivedChars[2]<48){ //numbers only
Serial.print ("tray ref offset steps: "); 
Serial.print (tray_ref_offset);  
Serial.print("\r\n");
}else{  
for (int x = 2; x < 7; x++){
data=((receivedChars[x])-48);  
if( data>-1 && data< 9){ 
data_array[x-2]=data;                               
receivedChars[x]=0;
}   
} 
value=value+(data_array[4]*1);
value=value+(data_array[3]*10);
value=value+(data_array[2]*100);
value=value+(data_array[1]*1000);
value=value+(data_array[0]*10000);
if (value<1){value=1;}
if (value>20000){value=20000;}
tray_ref_offset=value;
value=0;
Serial.print("tray ref offset steps new value: ");
Serial.print(tray_ref_offset);
Serial.print("\r\n");
EEPROM.put(94,tray_ref_offset);
}   
}














newData = false;
}
}

//***********************************MOTION*********************************


void carousel_forward(){
         int ramp_steps;         
         step_delay=start_step_delay;
         ramp_steps=carousel_ramp_steps;
         if (carousel_distance < (carousel_ramp_steps*2)){ramp_steps=(carousel_distance/2);} 
         ramp_unit=(start_step_delay-carousel_speed)/ramp_steps;
         //start motion
         digitalWrite(carousel_dir_pin,0); 
         delayMicroseconds(100);        
         for (int x = 0; x < carousel_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > carousel_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }   
         digitalWrite(carousel_step_pin,1);
         delayMicroseconds(15);  
         digitalWrite(carousel_step_pin,0);
         if (set_find_home==1){step_delay=carousel_ref_speed;}
         delayMicroseconds(step_delay); 
         recvWithEndMarker();
         showNewData();              
         if ((carousel_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(carousel_dir_pin,0);
}

void carousel_reverse(){
         int ramp_steps;
         step_delay=start_step_delay;
         ramp_steps=carousel_ramp_steps;
         if (carousel_distance < (carousel_ramp_steps*2)){ramp_steps=(carousel_distance/2);} 
         ramp_unit=(start_step_delay-carousel_speed)/ramp_steps;
         //start motion
         digitalWrite(carousel_dir_pin,1); 
         delayMicroseconds(100);        
         for (int x = 0; x < carousel_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > carousel_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }
         digitalWrite(carousel_step_pin,1);
         delayMicroseconds(15);  
         digitalWrite(carousel_step_pin,0);
         if (set_find_home==1){step_delay=carousel_ref_speed;}
         delayMicroseconds(step_delay); 
         recvWithEndMarker();
         showNewData();              
         if ((carousel_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(carousel_dir_pin,0);
}

void carousel_ref(){  
         int ramp_steps;    
         step_delay=start_step_delay;
         ramp_steps=tray_ramp_steps;
         if (carousel_distance < (carousel_ramp_steps*2)){ramp_steps=(carousel_distance/2);} 
         ramp_unit=(start_step_delay-carousel_ref_speed)/ramp_steps;
         //start motion
         digitalWrite(carousel_dir_pin,1); 
         delayMicroseconds(100);        
         for (int x = 0; x < carousel_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > carousel_ref_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }
         digitalWrite(carousel_step_pin,1);
         delayMicroseconds(15);  
         recvWithEndMarker();
         showNewData();         
         digitalWrite(carousel_step_pin,0);
         if (!digitalRead(carousel_ref_pin)){
         x=carousel_distance;
         carousel_homed=1;
         }
         if (set_find_home==1){step_delay=carousel_ref_speed;}
         delayMicroseconds(step_delay); 
         if ((carousel_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(carousel_dir_pin,0);
}




void tray_forward(){
         int ramp_steps;
         step_delay=start_step_delay;
         ramp_steps=tray_ramp_steps;
         if (tray_distance < (tray_ramp_steps*2)){ramp_steps=(tray_distance/2);} 
         ramp_unit=(start_step_delay-tray_speed)/ramp_steps;
         //start motion
         digitalWrite(tray_dir_pin,0); 
         delayMicroseconds(100);        
         for (int x = 0; x < tray_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > tray_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }
         digitalWrite(tray_step_pin,1);
         delayMicroseconds(15);  
         recvWithEndMarker();
         showNewData();       
         digitalWrite(tray_step_pin,0);
         if (set_find_home==1){step_delay=tray_ref_speed;}
         delayMicroseconds(step_delay);
         if ((tray_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(tray_dir_pin,0);
}


void tray_reverse(){
         int ramp_steps;
         step_delay=start_step_delay;
         ramp_steps=tray_ramp_steps;
         if (tray_distance < (tray_ramp_steps*2)){ramp_steps=(tray_distance/2);} 
         ramp_unit=(start_step_delay-tray_speed)/ramp_steps;
         //start motion
         digitalWrite(tray_dir_pin,1); 
         delayMicroseconds(100);        
         for (int x = 0; x < tray_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > tray_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }
         digitalWrite(tray_step_pin,1);
         delayMicroseconds(15);  
         recvWithEndMarker();
         showNewData();       
         digitalWrite(tray_step_pin,0);
         if (set_find_home==1){step_delay=tray_ref_speed;}
         delayMicroseconds(step_delay); 
         if ((tray_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(tray_dir_pin,0);
}

void tray_ref(){  
         int ramp_steps;    
         step_delay=start_step_delay;
         ramp_steps=tray_ramp_steps;
         if (tray_distance < (tray_ramp_steps*2)){ramp_steps=(tray_distance/2);} 
         ramp_unit=(start_step_delay-tray_ref_speed)/ramp_steps;
         //start motion
         digitalWrite(tray_dir_pin,1); 
         delayMicroseconds(100);        
         for (int x = 0; x < tray_distance; x++){
         if (x < ramp_steps){ 
         if (step_delay > tray_ref_speed){
         step_delay = (step_delay - ramp_unit);
         }
         }
         digitalWrite(tray_step_pin,1);
         delayMicroseconds(15);  
         recvWithEndMarker();
         showNewData();         
         digitalWrite(tray_step_pin,0);
         if (!digitalRead(tray_ref_pin)){
         x=tray_distance;
         tray_homed=1;
         }
         if (set_find_home==1){step_delay=tray_ref_speed;}
         delayMicroseconds(step_delay); 
         if ((tray_distance - x) < ramp_steps){
         step_delay = step_delay+ramp_unit;
         }
         }
         digitalWrite(tray_dir_pin,0);
}






//**************************END MOTION*******************************

void tool_calc(){

         if(required_tool > number_of_tools){required_tool = 0;}
         
         if (required_tool > tool){
         if (required_tool==(number_of_tools-1) and tool==0){
         carousel_distance=carousel_steps;
         set_carousel_reverse=1;
         }else{
         carousel_distance=((required_tool-tool)*carousel_steps); 
         set_carousel_forward=1;
         }
         }else{
         if (required_tool==0 and tool==(number_of_tools-1)){
         carousel_distance=carousel_steps;
         set_carousel_forward=1;
         } else{
         carousel_distance=((tool-required_tool)*carousel_steps); 
         if (tool==9 and required_tool==0){carousel_distance=carousel_steps;}
         set_carousel_reverse=1;
         }
         }
         }
//*********************************MEMORY*****************************************
         

void read_mem(){


EEPROM.get(0,tray_steps);
EEPROM.get(8,tray_max_speed);
EEPROM.get(16,tray_ramp_steps);
EEPROM.get(24,carousel_steps);
EEPROM.get(32,carousel_max_speed);
EEPROM.get(40,carousel_ramp_steps);
EEPROM.get(48,carousel_ref_offset);
EEPROM.get(56,carousel_ref_speed);
EEPROM.get(64,carousel_ref_off);
EEPROM.get(72,tray_ref_speed);
EEPROM.get(86,tray_ref_off);
EEPROM.get(94,tray_ref_offset);
EEPROM.get(102,number_of_tools);
EEPROM.get(110,profile);

//profile deinitions
if (profile==3){
number_of_tools=10;
vfd_detect_mode=1;
}

Serial.print("memory read ok");//
Serial.print(profile);

}