File content as of revision 1:f246d2321c06:

#include "mbed.h"
#define MAXPOS 50
#define SS_TIME 500
 
PwmOut myServoX(PB_3);
PwmOut myServoY(PB_4);
PwmOut myServoZ(PB_10);
 
 
int coord2us(float coord)
    {
    if(0 <= coord <= MAXPOS)
        return int(750+coord*1900/50);// u6
    return 750;
    }
 
void vertex2d(float x, float y) 
    {
    int pulseX = coord2us(x);
    int pulseY = coord2us(y);
    
    myServoX.pulsewidth_us(pulseX);
    myServoY.pulsewidth_us(pulseY);
        wait_ms(SS_TIME);
    }
    
  
    
void home()
    {

    vertex2d(0,0);
       myServoZ.pulsewidth_us(MAXPOS);
    }    
 
void maxpos()
    {
    vertex2d(MAXPOS,MAXPOS);
    }    
 
void diag()
    {
  /*  wait(2);vertex2d(0,0);
    wait(2);vertex2d(10,10);
    wait(2);vertex2d(20,20);
    wait(2);vertex2d(30,30);
    wait(2);vertex2d(40,40);
    
    
    
    wait(1);vertex2d(0,0);   
    wait(1);vertex2d(40,0);   
    wait(1);vertex2d(40,40);   
    wait(1);vertex2d(0,40);
    wait(1);vertex2d(0,0);   */
        }
        
void cuadrado(int x, int y, int lado)
    {
        float l=lado;
        float a=x;
        float b=y;
        
        myServoZ.pulsewidth_us(MAXPOS);
        vertex2d(a,b);     
        myServoX.pulsewidth_us(100);  
        a=a+l;
        vertex2d(a,b);
        b=b+l;
        vertex2d(a,b);
        a=a-l;
        vertex2d(a,b);
        b=b-l;
        vertex2d(a,b);
         }        
 
void line(int x0, int y0, int x1, int y1) {        
        home();
        wait(2);
        vertex2d(x0,y0);
bool steep = abs(y1 - y0) > abs(x1 - x0);
int temp, deltax, deltay, error, y, x;
    if (steep) {
        temp = y0;
        y0 = x0;
        x0 = temp;
        temp = y1;
        y1 = x1;
        x1 = temp;
    }
    if (x0 > x1) {
        temp = x1;
        x1 = x0;
        x0 = temp;
        temp = y1;
        y1 = y0;
        y0 = temp;
    }
    
    deltax = x1 - x0;  
    deltay = abs(y1 - y0);
    error = deltax / 2;
    y = y0;
    /*if (y0 < y1) {
        ystep = 1;
    } else {
        ystep = -1;
    }*/
    for (x=x0; x<=x1; x++) {
        if (steep) {
           vertex2d(y,x);
        } else {
            vertex2d(x,y);
        }
        }
    }

void circle(int cx, int cy, int radius)    {
        
    int x = 0;
    int y = radius;
    int d = 3 - (2 * radius);
        
        home();
        wait(2);
        vertex2d(cx,cy);
        myServoX.pulsewidth_us(100); 
        myServoZ.pulsewidth_us(MAXPOS);
        /*cx=cx+radius;
        vertex2d(cx,cy);*/
        myServoX.pulsewidth_us(100);       
        
        while (x <= y) {
        vertex2d(cx + x, cy + y);
        vertex2d(cx + y, cy + x);
        vertex2d(cx - x, cy + y);
        vertex2d(cx + y, cy - x);
        vertex2d(cx - x, cy - y);
        vertex2d(cx - y, cy - x);
        vertex2d(cx + x, cy - y);
        vertex2d(cx - y, cy + x);
 
        if (d<0){
            d += (4 * x) + 6;
        } else
        {
            d += (4 * (x - y)) + 10;
            y--;
        }
        x++;
    }
         }
 
 int main() 
 {
     // configuracion de  periodo
    myServoX.period_ms(20);
    myServoY.period_ms(20);
    myServoZ.period_ms(20);
    
    while(1)
    {
     home();
     wait(3);
    maxpos();    
     wait(3);
     //diag();
     cuadrado(10,10,20);
     wait(3);
     line(20,20,40,40);
     wait(3);
     circle(20,20,20);
          wait(3);
     
     }
 
}