File content as of revision 0:e0c1f3f89b27:

#include "mbed.h"

PwmOut eje_x(PB_3);
PwmOut eje_y(PB_4);
PwmOut eje_z(PB_5);

double i=0,rad=0;
double pulso_us=0,RET_PRU=0,gain=15,b=10;
double grados=0,retardo_for=0.05,retardo=1,diam=70,var_x=0,var_y=0;

    

void REFERENCE(){
    eje_x.pulsewidth_us(1650);
    eje_y.pulsewidth_us(1635);
    eje_z.pulsewidth_us(750);
    wait_ms(500);
    }

    
void SERVO_X(){
    pulso_us=((grados*1700)/(180))+790; 
    eje_x.pulsewidth_us(pulso_us);
    wait_ms(0);
    }
    
void SERVO_Y(){
    pulso_us=((grados*1870)/(180))+700;
    eje_y.pulsewidth_us(pulso_us);
    wait_ms(0);
    }
    
void SERVO_Z(){
    pulso_us=((grados*1800)/(180))+650;  
    eje_z.pulsewidth_us(pulso_us);
    wait_ms(100);
    } 
    
void PruevaX(){
        grados=0;
        SERVO_X();
        wait_ms(RET_PRU);
    for(i=0;i<=90;i=i+10){
        grados=i;
        SERVO_X();
        wait_ms(retardo);
        }
        wait_ms(RET_PRU);
        grados=180;
        SERVO_X();
        wait_ms(RET_PRU);
    }
void PruevaY(){
        grados=0;
        SERVO_Y();
        wait_ms(RET_PRU);
    for(i=0;i<=90;i=i+10){
        grados=i;
        SERVO_Y();
        wait_ms(retardo);
        }
        wait_ms(RET_PRU);
        grados=180;
        SERVO_Y();
        wait_ms(RET_PRU);
    }
    
void ESCRIBIR(){
    grados=120;
    SERVO_Z();
    }

void ESCRIBIR_0(){
    grados=30;
    SERVO_Z();
    }
      
void LINEA_DIAG(){
    REFERENCE();
    wait(2);
    for(i=0;i<=1800;i=i+5){
        grados=i;
        SERVO_X();
        SERVO_Y();
        wait_ms(retardo);
        ESCRIBIR();
        }
    ESCRIBIR_0();
    }
    
void CUADRADO(){
    REFERENCE();
    grados=0;
    SERVO_X();
    SERVO_Y();
    wait(2);
    for(i=0;i<=180;i=i+5){
        grados=i;
        SERVO_Y();
        wait_ms(retardo);
        ESCRIBIR();        
        }
    for(i=0;i<=180;i=i+5){
        grados=i;
        SERVO_X();
        wait_ms(retardo);
        ESCRIBIR();
        }
    for(i=180;i>=0;i=i-5){
        grados=i;
        SERVO_Y();
        wait_ms(retardo);
        ESCRIBIR();
        }
    for(i=180;i>=0;i=i-5){
        grados=i;
        SERVO_X();
        wait_ms(retardo);
        ESCRIBIR();
        }
    for(i=180;i<=0;i=i+10){
        grados=i;
        SERVO_Y();
        wait_ms(retardo);
        ESCRIBIR();        
        }
    ESCRIBIR_0();
        }
void CIRCULO(){
    REFERENCE();
    wait(2);
    for(i=0;i<360;i=i+10){
        rad=(i*2*3.1416)/(180);
        grados=(((cos(rad)+1)*180/2)*140/180)+20;
        SERVO_X();
        grados=(((sin(rad)+1)*180/2)*140/180)+20;
        SERVO_Y();
        wait_ms(retardo);
        ESCRIBIR();
        }
    ESCRIBIR_0();
    }

void DIBUJO(){
    REFERENCE();
    wait(2);
    grados=0+gain;
    SERVO_X();
    SERVO_Y();
    ESCRIBIR();
    wait(retardo);
    
    grados=30+gain;
    SERVO_X();
    wait(retardo);
    
    grados=60+gain;
    SERVO_Y();
    wait(retardo);
    
    grados=90+gain;
    SERVO_X();
    wait(retardo);
    
    grados=120+gain;
    SERVO_Y();
    wait(retardo);
    
    grados=150+gain;
    SERVO_X();
    wait(retardo);
    
    grados=150+gain;
    SERVO_Y();
    wait(retardo);
    
    grados=120+gain-b;
    SERVO_X();
    wait(retardo);
    
    grados=90+gain-b;
    SERVO_Y();
    wait(retardo);
    
    grados=60+gain-b;
    SERVO_X();
    wait(retardo);
    
    grados=30+gain-b;
    SERVO_Y();
    wait(retardo);
    
    grados=0+gain-b;
    SERVO_X();
    wait(retardo);
    
    grados=0+gain-b;
    SERVO_Y();
    wait(retardo);

    ESCRIBIR_0();
    
    grados=150+gain;
    SERVO_Y();
    wait(retardo);
    
    grados=0+gain;
    SERVO_X();
    wait(retardo);
    
    ESCRIBIR();
    for(i=0;i<45;i++){
    grados=i+gain;
    SERVO_X();
    grados=150-i+gain;
    SERVO_Y();
    wait(retardo_for);
    }
    
    ESCRIBIR_0();
    
    grados=150+gain;
    SERVO_X();
    wait(retardo);

    grados=0+gain;
    SERVO_Y();
    wait(retardo);
    
    ESCRIBIR();
    for(i=0;i<45;i++){
    grados=150-i+gain;
    SERVO_X();
    grados=i+gain;
    SERVO_Y();
    wait(retardo_for);
        }
    
    ESCRIBIR_0();
        
    grados=150+gain;
    SERVO_X();
    wait(retardo);

    grados=60+gain;
    SERVO_Y();
    wait(retardo);
    
    for(i=180;i<360;i=i+5){
        rad=(i*3.1416)/(180);
        var_x=((cos(rad)+1)*diam/2)*1.0;
        grados=var_x+30+gain;
        SERVO_X();
        var_y=((sin(rad)+1)*diam/2)*1.5;
        grados=var_y+120+gain;
        SERVO_Y();
        ESCRIBIR();
        }
    ESCRIBIR_0();
    }
    
    
int main() {
        eje_x.period_us(20000);
        eje_y.period_us(20000);
        eje_z.period_us(20000);        
        REFERENCE();  
    
    while(1) { 
    
        DIBUJO(); 
        
        

}
}