File content as of revision 17:a0b884f78802:

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

DigitalOut led(LED1);

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

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

void put_sstime(uint8_t vtime){
    ss_time=vtime;
    
}
void led_on(uint16_t ts, uint16_t tms )
{

                led=1;  
                wait(ts);wait_ms(tms);
                led=0;   
                 
}

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);
   
    wait_ms(ss_time); //TODO: CALCULAR EL TIEMPO DE DONDE  VIENE 

}

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

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

void line_n(float xi, float yi, float xf,  float yf)
{ 
  float xp; float yp;
    float m;  float b;
    int  st=1 ;
    int run=1;
    m=(float)(yf-yi)/(xf-xi);
                
    if (abs(m)<=1){    
        b=(float)yf-(float)(m*xf);
  //      command.printf(" m =  %f b= %f \n", m,b);
        xp =xi;
        while(run)
        {
            yp =m*xp+b;
            vertex2d(xp ,yp);
            //print_bin2hex(xp);  print_bin2hex(yp);  
            //command.printf(" \n");
            if (xf>xi){ 
                xp=xp+st;
                if (xp>xf)run=0;
            }else {
                xp-=st;   
                if (xp<xf)run=0;
            }
        
        }
    }else{ 
        m=(float)(xf-xi)/(yf-yi);
        b=(float)xf-(float)(m*yf);
  //      command.printf(" m =  %f b= %f \n", m,b);
        yp =yi;
        while(run)
        {
            xp =m*yp+b;
            vertex2d(xp ,yp);
         //   print_bin2hex(xp);  print_bin2hex(yp);  
         //  command.printf(" \n");
            if (yf>yi){ 
                yp=yp+st;
                if (yp>yf)run=0;
            }else {
                yp-=st;   
                if (yp<yf)run=0;
            }
        
        }
    }
}
void line(float xi, float yi, float xf,  float yf)
{   
    initdraw(xi,yi);
    line_n(xi, yi, xf,  yf);
    nodraw();
 
}


void rectangle(float x, float y, float width,float height) {
    
    initdraw(x,y);
 
    line_n(x,y,x+width,y);
    line_n(x+width,y,x+width,y+height);
    line_n(x+width,y+height,x,y+height);
    line_n(x,y+height,x,y);
    

    nodraw();
 
}

void circle(float x, float y, float radio)
{
 initdraw(x+radio,y);
 // coloque codigo 
 nodraw();

}