Diff: main.cpp

Revision:

0:9aa960516df7

Child:

1:a9b3fdab473e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 20 04:28:42 2017 +0000
@@ -0,0 +1,477 @@
+#include "mbed.h"
+#include "math.h"
+#include "cstdlib"
+#define MAXPOS 255
+#define SS_TIME 500
+#define POSDRAW 300
+#define BUFF_SIZE 6
+#define COMM_N 0
+#define INITPARAMETER  1
+#define DEBUG 1
+#define BUFF_POS 6
+ 
+PwmOut myServoX(PB_3); //MOTOR EJE X
+PwmOut myServoY(PB_4); //MOTOR EJE Y
+PwmOut myServoZ(PB_10);//MOTOR EJE Z
+
+Serial command(USBTX, USBRX); //COMUNICACION SERIAL
+Serial Sdebug(USBTX, USBRX);
+DigitalOut led(LED1);          //DECLARACION LED DE LA PLACA
+InterruptIn mybutton(USER_BUTTON); 
+
+// COMANDOS 
+#define LED 0
+#define PUNTO 1
+#define LINE 2
+#define RECTANGLE 3
+#define CIRCLE 4
+#define STOP 5
+#define PAUSE 6
+#define REANUDAR 7
+#define RESOLUCION 8
+#define HOME 9
+#define TIEMPO_DE_PASO 10
+
+Ticker ticker1;
+Timeout timeout;
+
+int rap;
+int RSTEP = 5;
+double abajo=7;
+double arriba=0;
+double k,l,r,tp=50,paso=0;
+int p=0;
+
+//*****************************************************************************
+//  COMANDO  MOVER MOTOR
+//  |POS 1|POS 2|POS 3|POS 4| POS 5| POS 6| POS 7|
+//  |  <  | #C  | a   | b   |  c   |  d   |  >   |
+//
+// #C -> indica el comando
+// a,b,c,d parametros del comando
+// <,> -> inicio,  y fin de comando 
+//  el inicio de comando no se almacena en el buffer
+//*****************************************************************************
+// VARIABLES PARA DEFINIR  EL  COMMANDO
+
+uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};
+float buffer_POS [BUFF_POS]= {0,0,0,0,0,0};
+
+void savep();
+void interrupcion();
+
+int coord2us(float coord)
+    {
+        if(0 <= coord <= MAXPOS)
+        return int(750+coord*1900/50);// u6
+        return 750;
+    }
+    void print_num(uint8_t val)
+    {
+        if (val <10)
+            command.putc(val+0x30);
+        else 
+            command.putc(val-9+0x40);
+    }  
+void print_bin2hex (uint8_t val)
+    {
+        command.printf(" 0x");
+        print_num(val>>4);
+        print_num(val&0x0f); 
+    }
+void Read_command()
+    {
+        for (uint8_t i=0; i<BUFF_SIZE;i++)
+        {
+            buffer_command[i]=command.getc();
+                
+        }
+    }
+    
+void echo_command()
+    {
+        for (uint8_t i=0; i<BUFF_SIZE;i++)
+        {
+            print_bin2hex(buffer_command[i]);
+        }
+    }
+uint8_t check_command()
+    {
+       if (buffer_command[BUFF_SIZE-1]== '>')
+             return 1;
+             return 0;        
+    }
+void command_led(uint8_t tm)
+    {
+        #if DEBUG
+            command.printf("%i, segundos", tm);
+        #endif
+        led=1;  
+        wait(tm);
+        led=0;            
+    }
+void draw()
+    {
+        myServoZ.pulsewidth_us(POSDRAW);
+        wait(1);
+    }
+void nodraw()
+    {
+        myServoZ.pulsewidth_us(MAXPOS);
+        wait(1);
+    }
+    uint8_t SSTIME = 100;  
+void tiempo_de_paso(int SST) 
+    {
+    SSTIME=SST;     
+    #if DEBUG
+    command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
+    #endif              
+    }
+void vertex2d(float x, float y, float z)
+    {
+        int pulseX = coord2us(x);
+        int pulseY = coord2us(y);
+        int pulseZ = coord2us(z);
+          myServoX.pulsewidth_us(pulseX);
+          myServoY.pulsewidth_us(pulseY);
+          myServoZ.pulsewidth_us(pulseZ);
+        wait_ms(SSTIME);
+    }
+
+uint8_t  posx_old=0;     // posición anterior del eje X
+uint8_t  posy_old=0;     // posición anterior del eje Y
+uint8_t  posz_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 sstime(uint8_t x, uint8_t y, uint8_t z)
+{
+    double dx=abs(x-posx_old);
+    double dy=abs(y-posy_old);
+    double dz=abs(z-posz_old);
+    double dist= sqrt(dx*dx+dy*dy+dz*dz);
+    wait_ms((int)(ss_time*dist));
+    posx_old=x;
+    posy_old=y;
+    posy_old=z;
+ }
+void initdraw(float x, float y)
+    {
+        #if DEBUG
+            Sdebug.printf("inicia dibujo \n");
+        #endif
+        vertex2d (x,y,abajo);
+        wait(1);
+        draw();
+    }
+
+void resolucion(int res){
+    RSTEP = res;
+    #if DEBUG
+    command.printf("Resolucion definida en = %i \n", RSTEP);
+    #endif
+    }
+void home()
+    {
+        vertex2d(0,0,0);
+        wait_ms(rap); 
+    }    
+void maxpos()
+    {
+        vertex2d(250,250,150);
+    }
+void punto(uint8_t x, uint8_t y)
+  {  
+    command.printf("Coord x= %i, coord y=%i \n",x,y);
+    vertex2d(x,y,0);
+    wait_ms(1000);
+    vertex2d(x,y,11);
+    wait_ms(1000);
+    vertex2d(x,y,0);
+    wait_ms(1000);
+    home();
+  }   
+  
+void line(float xi, float xf, float yi, float yf)
+    {
+ // los parametros son coordenadas iniciales  y finales de la linea en milimetros   
+        #if DEBUG
+            Sdebug.printf("linea  Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf);
+        #endif
+        float xp; float yp;
+        float m= (yf-yi)/(xf-xi);
+        float b=(yf-m*xf);
+        vertex2d(0,0,4);
+        wait(1);
+        vertex2d(xi,yi,abajo);
+        wait(1);
+        for (xp =xi;xp<=xf;xp+=RSTEP)
+            {
+                yp =m*xp+b;
+                vertex2d(xp,yp,abajo);
+               // savep( buffer_POS[INITPARAMETER==02]buffer_POS[INITPARAMETER+1==xp], buffer_POS[INITPARAMETER+2==xf],buffer_POS[INITPARAMETER+3==yp], buffer_POS[INITPARAMETER+4==yf]);
+                
+         #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp);
+         #endif
+        }
+        nodraw();
+        home();
+    } 
+ void rectangle(uint8_t xi, uint8_t yi,uint8_t a, uint8_t h)
+{
+ int x,y;
+ x = a+ xi;
+ y = h+ yi;
+ 
+ vertex2d(xi,yi,arriba);
+ wait_ms(600);
+ vertex2d(xi,yi,abajo);
+ wait_ms(600);
+ vertex2d(x,yi,abajo);  
+ wait_ms(600);
+ vertex2d(x,y,abajo); 
+ wait_ms(600);
+ vertex2d(xi,y,abajo); 
+ wait_ms(600);
+ vertex2d(xi,yi,abajo); 
+ wait_ms(600);
+ home();
+ }
+ void circle(uint8_t x1, uint8_t y1,uint8_t r)
+  {
+  double  x2,y2,i,j;
+  vertex2d(x1 ,y1,arriba);
+  wait(1);
+  wait_ms(rap);
+ 
+  for (  x2=0;x2<=r;x2++){
+      y2= pow(((r*r)-(x2*x2)),0.5);
+      i=  x2+x1;  
+      j=y1+y2;
+      vertex2d(i ,j,abajo) ;
+      wait_ms(30);
+      #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+         #endif
+      }
+       
+    for (  x2=r;x2>=0;x2--){
+      y2= pow(((r*r)-(x2*x2)),0.5);   
+      i=  x2+x1;  
+      j= y1 - y2;
+      vertex2d(i ,j,abajo) ;
+      wait_ms(30);
+      #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+         #endif
+       }
+  
+  for (  x2=0;x2<=r;x2++){
+      y2= pow(((r*r)-(x2*x2)),0.5);   
+      i=  x1- x2;  
+      j= y1 - y2;
+      vertex2d(i ,j,abajo) ;
+      wait_ms(30);
+      #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+         #endif
+       }
+  for (  x2=r;x2>=0;x2--){
+      y2= pow(((r*r)-(x2*x2)),0.5);   
+      i=  x1-x2;  
+      j=y1+y2;
+      vertex2d(i ,j,abajo) ;
+      wait_ms(30);
+      #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+         #endif
+       }
+      vertex2d(i ,j,arriba) ;
+      wait_ms(rap);
+      #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f \n", i,j);
+         #endif
+      
+      if(x2==r)
+      {
+      wait(1);  
+    }
+    home();
+    }
+    
+    
+    
+    void savep(float pos){
+         for (uint8_t i=0; i<BUFF_POS;i++)
+        {
+            buffer_command[i]=command.getc();
+        
+        }
+        
+        }
+        
+    void reanudar(){
+        led =0;
+        p=0;
+        
+        
+        }
+        
+    
+void pause(){
+       p=1;
+       led = !led;                            
+    timeout.attach(&pause, 300);
+    // while(1);
+    
+    
+    }
+            
+void stop() //Funcion para detener el programa  
+    {
+    exit(0);
+    home();           
+    }  
+
+void command_exe()
+    {
+        #if DEBUG
+            command.printf("Ejecutando comando: ");
+        #endif  
+        switch (buffer_command[COMM_N])
+        {
+            case (LED): 
+                #if DEBUG
+                    command.printf("LED on\n");
+                #endif
+                command_led(buffer_command[INITPARAMETER]);
+            break;
+        
+            case (PUNTO): 
+                punto(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);    
+                #if DEBUG
+                command.printf("Dibujando Punto");
+                #endif
+            break;
+
+            case LINE:
+                #if DEBUG
+                    command.printf("Dibujando Linea\n");
+                #endif
+                line(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+                
+                
+            break;
+
+            case RECTANGLE: 
+                #if DEBUG
+                    command.printf("Dibujando Rectangulo\n");
+                #endif
+                rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+            break;
+            
+            case  (CIRCLE): 
+                #if DEBUG
+                    command.printf("Dibujando Circulo\n");
+                #endif
+                circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
+            break;
+            
+            case  (STOP): 
+                 #if DEBUG
+                 command.printf("    STOP\n");
+                 #endif
+             break;
+            
+            case  (PAUSE): 
+               #if DEBUG
+                    command.printf("programa pausado\n");
+              #endif
+                pause();
+             break;
+            
+           case  (REANUDAR): 
+              #if DEBUG
+                   command.printf("draw reanudar programa\n");
+                #endif
+                reanudar();
+           break;
+            
+            case  (RESOLUCION): 
+                  resolucion(buffer_command[INITPARAMETER]);
+                  #if DEBUG
+                  command.printf("Resolucion definida");
+                  #endif
+            break;
+            case  (HOME): 
+                 home();
+                #if DEBUG
+                    command.printf("Posicion Home");
+                #endif
+            break;
+            
+            case  (TIEMPO_DE_PASO): 
+                #if DEBUG
+                    command.printf("Nuevo tiempo para paso:\res");
+                #endif
+                tiempo_de_paso(buffer_command[INITPARAMETER]);
+            break;
+            
+            default:
+                #if DEBUG
+                    command.printf("comando  no encontrado\n");
+                #endif
+       
+        }
+}
+
+
+    
+
+
+    
+        
+     
+     
+
+int main() {
+    #if DEBUG
+    command.printf("inicio con debug\n");
+    #else
+    command.printf("inicio sin debug\n");
+    #endif
+     uint8_t val;
+   // mybutton.rise(&stop);
+    while(1){
+    val=command.getc();
+        if (val== '<'){
+            Read_command();
+          //  ticker1.attach(&Read_command,05);
+            if (check_command()){
+                command_exe();
+            }
+            else{
+                #if DEBUG
+                    command.printf("\n ERROR COMANDO -> ");
+                    echo_command();
+                #endif
+            }        
+        }
+        else{
+                #if DEBUG
+                 command.printf("error de inicio de trama: ");
+                 command.putc(val);
+                #endif
+                }
+                
+            }
+        
+                
+    //ticker1.attach(&main,05);
+    
+
+} 
\ No newline at end of file