File content as of revision 30:27838f6fdfd6:

#define tx PC_10
#define rx PC_11

#define sendTime 0.5

Serial ihm(tx, rx); //tx e rx (D1 e D0)

int saved = 0;
const int maxPoints = 100;
float points[3][maxPoints]; // [0] X; [1] Y; [2] Z;
float path[3][maxPoints]; // [0] mode [1] speed; [2] glue;
int resting_points = 0;

class IHM { 
    int n;
    string buffer;
    char buffer_char[8];
    
    int gotX;
    int gotY;
    int gotZ;
    int gotF;
    int gotG;
    int gotM;
    
    public:
        int debug;
    
        // PUBLIC FUNCTIONS ----------------------------------------------------------------------------------------------------------------------------------------
        void send_position(float X, float Y, float Z){
            //printf("Position sent\n\r");
            buffer = "";
            
            buffer.append("p");
            
            n = sprintf(buffer_char, "X%.2f", X);
            buffer.append(buffer_char);    
            n = sprintf(buffer_char, "Y%.2f", Y);
            buffer.append(buffer_char);  
            n = sprintf(buffer_char, "Z%.2f", Z);
            buffer.append(buffer_char); 
            
            send(buffer); 
        }
        
        void action_complete(void){
            send("a");    
        }
        
        void read_gcode(string gcode){
            string buffer;
            char c;
            saved = 0;
            
            printf("\nGcode: %s\n\n\r", gcode);
            
            for(int i = 0; i < gcode.size(); i++){
                c = gcode[i];
                if(c == '\r'){
                    translate_gcode(buffer, saved);
                    saved++;
                    buffer = "";
                } else if(c == ';'){
                    buffer.push_back(c);
                    translate_gcode(buffer, saved);
                    break;
                } else if(c != '\n'){
                    buffer.push_back(c);
                }
            }
        }
        
        void translate_gcode(string line, int line_number){
            char c;
            string buffer;
            int toAdd[6] = {1, 1, 1, 1, 1, 1} ;
            
            line.append(" ");
            
            gotX = 0;
            gotY = 0;
            gotZ = 0;
            gotF = 0;
            gotG = 0;
            gotM = 0; 
            
            //printf("Line: %s             No: %d\r\n", line, line_number); 
            for(int i = 0; i < line.size(); i++){
                c = line[i];
                
                if(c == 'G'){
                    set_variables(1, 0, 0, 0, 0, 0);
                }
                if(c == 'X'){
                    set_variables(0, 1, 0, 0, 0, 0);
                }
                if(c == 'Y'){
                    set_variables(0, 0, 1, 0, 0, 0);
                }
                if(c == 'Z'){
                    set_variables(0, 0, 0, 1, 0, 0);
                }
                if(c == 'F'){
                    set_variables(0, 0, 0, 0, 1, 0);
                }
                if(c == 'M'){
                    set_variables(0, 0, 0, 0, 0, 1);
                }
                
                if(gotG && c != 'G'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[0]){
                        path[0][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[0] = 0;
                        //printf("G: %f\r\n", path[0][line_number]);
                    }
                }
                
                if(gotX && c != 'X'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[1]){
                        points[0][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[1] = 0;
                        //printf("X: %f\r\n", points[0][line_number]);
                    }
                }
                
                if(gotY && c != 'Y'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[2]){
                        points[1][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[2] = 0;
                        //printf("Y: %f\r\n", points[1][line_number]);
                    }
                }
                
                if(gotZ && c != 'Z'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[3]){
                        points[2][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[3] = 0;
                        //printf("Z: %f\r\n", points[2][line_number]);
                    }
                }
                
                if(gotF && c != 'F'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[4]){
                        path[1][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[4] = 0;
                        //printf("F: %f\r\n", path[1][line_number]);
                    }
                }
                
                if(gotM && c != 'M'){
                    if(c != ' ' && c != ';'){
                        buffer.push_back(c);
                    } else if(toAdd[5]){
                        path[2][line_number] = atof(buffer.c_str());
                        buffer = "";
                        toAdd[5] = 0;
                        //printf("M: %f\r\n", path[2][line_number]);
                    }
                }                     
            }  
            
            if(toAdd[1]){
                points[0][line_number] = points[0][line_number-1];
            }
            
            if(toAdd[2]){
                points[1][line_number] = points[1][line_number-1];
            }
            
            if(toAdd[3]){
                points[2][line_number] = points[2][line_number-1];
            }
            
            if(toAdd[4]){
                path[1][line_number] = path[1][line_number-1];
            }
            
            if(toAdd[5]){
                path[2][line_number] = path[2][line_number-1];
            }
            
            printf("G: %.2f, X: %.2f, Y: %.2f, Z: %.2f, F: %.2f, M: %.2f\n\r", path[0][line_number], points[0][line_number], points[1][line_number], points[2][line_number], path[1][line_number], path[2][line_number]);
        }
        
        void set_variables(int G, int X_val, int Y_val, int Z_val, int F, int M){
            gotX = X_val;
            gotY = Y_val;
            gotZ = Z_val;
            gotF = F;
            gotG = G;
            gotM = M;   
        }
        
        void sendSaved(void){
            string buffer = "u";
            char buffer_char[8];
            int n;
            
            for(int i = 0; i < saved; i++){
                buffer.append("G");
                n = sprintf(buffer_char, "%d ", (int)path[0][i]);
                buffer.append(buffer_char);
                
                buffer.append("X");
                n = sprintf(buffer_char, "%.3f ", points[0][i]);
                buffer.append(buffer_char);
                
                buffer.append("Y");
                n = sprintf(buffer_char, "%.3f ", points[1][i]);
                buffer.append(buffer_char);
                
                buffer.append("Z");
                n = sprintf(buffer_char, "%.3f ", points[2][i]);
                buffer.append(buffer_char);
                
                buffer.append("F");
                n = sprintf(buffer_char, "%.2f ", path[1][i]);
                buffer.append(buffer_char);
                
                buffer.append("M");
                n = sprintf(buffer_char, "%d ", (int)path[2][i]);
                buffer.append(buffer_char);
                
                if(i == (saved-1)){
                    buffer.append(";");
                } else {
                    buffer.append("\r\n");
                }
            } 
            
            printf("Sending: %s\n\r", buffer);
            send(buffer);   
        }

    private:
        
        // PRIVATE FUNCTIONS ----------------------------------------------------------------------------------------------------------------------------------------
        void send(string msg){
            msg.append("x");
            
            for(int i = 0; i < msg.length(); i++){
                ihm.putc(msg[i]);
                wait_ms(10);   
            }
            printf("Sent\n\r");
        }       
};