Diff: main.cpp

Revision:

0:e0c1f3f89b27

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Sep 16 14:22:04 2017 +0000
@@ -0,0 +1,276 @@
+#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(); 
+        
+        
+
+}
+}
\ No newline at end of file