File content as of revision 0:6b90993ae9ec:

#include "mbed.h"
#include "math.h"

#define MEM_SIZE 10
#define MEM_TYPE uint8_t
#define MAXPOS 50       // en milimetros
#define SS_TIME 100     // en microsegundos
#define POSDRAW 10
#define CM_EJECUTAR 0xff
#define CM_GUARDAR 0xfe
#define CM_VERTEX2D 0xfd
#define CM_DRAW 0xfc 
#define CM_NODRAW 0xfb
#define CM_STOP 0xfa
#define CM_END 0xf0


PwmOut myServoX(PB_3);
PwmOut myServoY(PB_4);
PwmOut myServoZ(PB_5);

int mem_head = 0;
int mem_tail = 0;
uint8_t full = 0;
uint8_t  posx_old=0;     // posición anterior del eje X
uint8_t  posy_old=0;     // posición anterior del eje Y
uint8_t  ss_time=100;     // tiempo  de espera para moverse 1 mm en microsegundos

MEM_TYPE buffer[MEM_SIZE];
Serial command(USBTX, USBRX);

void put_sstime(uint8_t vtime){
    ss_time=vtime;
    
}

int coord2us(float coord)
{
    if(0 <= coord <= MAXPOS)
        return int(750+coord*1900/50);// u6
    return 750;

}

void sstime(uint8_t x, uint8_t y)
{
    double dx=abs(x-posx_old);
    double dy=abs(y-posy_old);
    double dist= sqrt(dx*dx+dy*dy);
    wait_ms((int)(ss_time*dist));
    posx_old =x;
    posy_old=y;
    
 }
 
void draw(){
myServoZ.pulsewidth_us(POSDRAW);
wait_ms(ss_time*2);
}

void nodraw(){
myServoZ.pulsewidth_us(MAXPOS);
wait_ms(ss_time*2);
}


void vertex2d(uint8_t x, uint8_t y){

    int pulseX = coord2us(x);
    int pulseY = coord2us(y);
    
    myServoX.pulsewidth_us(pulseX);
    myServoY.pulsewidth_us(pulseY);
   
    sstime(x,y); 

}

void initdraw(float x, float y){
    vertex2d (x,y);
    draw();
}

void home(){
    nodraw();
    vertex2d(0 ,0);
}    

void init_servo()
{
   myServoX.period_ms(20);
   myServoY.period_ms(20);
   myServoZ.period_ms(20);

    
}
void ejecutar(){
    command.printf("se esta ejecutando el dibujo...");    
    // ubicar acà el codigo
}
void guardar(){
    command.printf("se inicia el comado de guardar..");    
    // ubicar acà el codigo
 
}   

void init_serial()
{
    command.baud(9600);    
}
int main() {
    init_servo();
    init_serial();
    home();
    char read_cc;
    pc.printf("Dibujando linea");
    valor= pc.getc();
    if(valor=='FF')
    ejecutar();
          
            default: command.printf("error de comando");break ;      
        }
    }
 }