void setup(){
ini_serial();
}
void loop() {
requestPosition();
// Parse the string when a newline arrives:
if (stringComplete) {
parse_and_store();
// clear the string:
line = "";
stringComplete = false;
delay(100);
}
delay(30);
}
void ini_serial(){
// Open serial communications and wait for port to open:
Serial.begin(115200);
while (!Serial) {
; // wait for serial port to connect. Needed for Leonardo only
}
line.reserve(64);//reserve at least 64 bytes for the incoming string
}
//Request position to GRBL
void requestPosition(){
Serial.println("?");
}
void serialEvent() {
while (Serial.available()>0) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
line += inChar;
// if the incoming character is a newline, set a flag
// so the main loop can do something about it:
if (inChar == '\n') {
stringComplete = true;
}
}
}
void parse_and_store(){
int parseState = line.indexOf('<');
int parseState1 = line.indexOf('|');
Serial.println(line.substring(parseState+1,parseState1));
State = line.substring(parseState + 1,parseState1);
int parseMPos = line.indexOf('MPos:');
Serial.println(parseMPos);
int xparseMPos = line.indexOf(',');
int yparseMPos = line.indexOf(',',xparseMPos + 1);
int z_parseMPos = line.indexOf('|');
int zparseMPos = line.indexOf('|',z_parseMPos + 1);
x_Value = line.substring(parseMPos+1,xparseMPos);
y_Value = line.substring(xparseMPos+1,yparseMPos);
z_Value = line.substring(yparseMPos+1,zparseMPos);
/*Serial.println( line.substring(parseMPos+1,xparseMPos));
Serial.println( line.substring(xparseMPos+1,yparseMPos));
Serial.println( line.substring(yparseMPos+1,zparseMPos));*/
int parseWPos = line.indexOf('WCO'); //find WCO
Serial.println(parseWPos);
if ( parseWPos != -1 ){
wline = line.substring(parseWPos); //get the string after WCO: O:0.000,0.000,0.000>
Serial.println(wline);
int wparseWPos = wline.indexOf(':');
//Serial.println(wparseWPos);
int xparseWPos = wline.indexOf(',');
int yparseWPos = wline.indexOf(',',xparseWPos + 1);
int zparseWPos = wline.indexOf('>');
X_WCO = (wline.substring(wparseWPos + 1,xparseWPos));
Y_WCO = (wline.substring(xparseWPos + 1,yparseWPos));
Z_WCO = (wline.substring(yparseWPos + 1,zparseWPos));
/*Serial.println(wline.substring(wparseWPos + 1,xparseWPos));
Serial.println(wline.substring(xparseWPos + 1,yparseWPos));
Serial.println(wline.substring(yparseWPos + 1,zparseWPos));*/
if ( wline.indexOf('Ov') == -1)
{
Fx_Value = x_Value.toFloat(); //change x_Value from string to float
Fy_Value = y_Value.toFloat();
Fz_Value = z_Value.toFloat();
FX_WCO = X_WCO.toFloat();
FY_WCO = Y_WCO.toFloat();
FZ_WCO = Z_WCO.toFloat();
X_Value = Fx_Value - FX_WCO; // WPos = MPos - WCO;
Y_Value = Fy_Value - FY_WCO;
Z_Value = Fz_Value - FZ_WCO;
/* Serial.println(Fx_Value - FX_WCO,3);
Serial.println(Fy_Value - FY_WCO,3);
Serial.println(Fz_Value - FZ_WCO,3);*/ //debug
}
}
}
wiring diagram:
https://drive.google.com/file/d/0B5MJn_ytEg7lUUVtLWdDd2QyLU0/view?usp=sharing
display like this:
https://drive.google.com/file/d/0B5MJn_ytEg7lNW50T1BwMUltcjg/view?usp=sharing
https://drive.google.com/file/d/0B5MJn_ytEg7ldzNGV2V4dmVxQTQ/view?usp=sharing