Piccolo / Mbed 2 deprecated Piccolo_10

Dependencies:   mbed

Revision:
0:02b166bd1478
Child:
1:c0ff30bf8db2
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 18 14:53:56 2017 +0000
@@ -0,0 +1,541 @@
+#include "mbed.h"
+#include "InterruptIn.h"
+#include "stm32f4xx_hal.h"
+
+//Serial blue(PA_9, PA_10);  //PA_10=TX , PA_9=RX  ... D8 y D2 respectivamente en la nucleo tx1
+Serial pc(PA_9, PA_10);
+Serial command(USBTX, USBRX ,9600);
+DigitalOut led(LED1);
+
+
+void pausa(); 
+void command_led();
+void punto();
+void linea();
+void rectangle();
+void circle();
+void home();
+void resolucion();
+void tiempo_pasos();
+void stop();
+void pausa();
+void reanudar();
+void mover();
+void recibeByte();
+void stepmotor();
+void command_exe();
+
+#define DEBUG 1 // Servomotores
+PwmOut myServoX(PB_3);
+PwmOut myServoY(PB_5);
+PwmOut myServoZ(PB_4);
+//*****************************************************************************
+//  COMANDO  MOVER MOTOR
+//  |POS 1|POS 2|POS 3|POS 4| POS 5|
+//  |  <  | #C  | a   | b   |  >   |
+//  |  3C |0 al A| D1 | D2 | D3 | D4 | 3E |
+// #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal para enviar comando desde techado de 0 a A) 
+// 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
+#define BUFF_SIZE 6
+#define COMM_N 0
+#define INITPARAMETER  1
+#define MAXPOS 50
+#define POSDRAW 00 
+#define SS_TIME 500 
+#define PI 3.1415926
+int RSTEP = 1;         // en milimetros
+int cont = 1;
+int tiempo = 5;
+//int time = 0;
+int pause = 1;          // Tiempo en us (tiempo entre pasos) nos da la velocidad 
+uint8_t val;
+// COMANDOS         
+#define LED_NC          0   //ok
+#define DOT_NC          1   //ok
+#define LINE_NC         2   //ok 
+#define RECTANGLE_NC    3   //ok
+#define CICLE_NC        4   //ok
+#define HOME_NC         5   //ok
+#define RESOLUCION_NC   6   //ok
+#define TIEMPOPASOS_NC  7
+#define STOP_NC         8
+#define PAUSA_NC        9
+#define REANUDAR_NC     10
+#define MOVER_CN        11
+
+//////////////////Funciones para mover los servos/////////////////////
+
+int coord2pulsex(float coord)
+{   if(0 <= coord <= MAXPOS)
+    return int(800+coord*1850/50);// u6
+    return 750;  }
+    
+int coord2pulsey(float coord)
+{   if(0 <= coord <= MAXPOS)
+    return int(750+coord*1400/42);// u6
+    return 750;  }
+    
+void vertex2d(float x, float y){ // Funcion para enviarle la posicion a (x,y)
+
+    int pulseX = coord2pulsex(x);
+    int pulseY = coord2pulsey(y);
+    
+    myServoX.pulsewidth_us(pulseX);
+    myServoY.pulsewidth_us(pulseY+8);  }
+
+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
+uint8_t  buffer_command[BUFF_SIZE]={0,0,0,0,0,0}; // Matriz del Comando enviado
+uint8_t  buffer[BUFF_SIZE]={0,0,0,0,0,0};
+
+void put_sstime(uint8_t vtime){
+    ss_time=vtime;  }
+    
+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(){
+    wait(2);
+    myServoZ.pulsewidth_ms(2);
+    wait_ms(ss_time*2); }
+    
+void nodraw(){
+    myServoZ.pulsewidth_ms(1);
+    wait_ms(ss_time*2); 
+    wait(1);
+    //home();
+    }
+    
+    
+void initdraw(float x, float y)//ok
+    {
+    vertex2d(x,y);
+    wait_ms(ss_time);
+    draw();
+}
+
+///////////////FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO///////////////////
+    
+void print_num(uint8_t val)
+{   if (val <10){
+    command.putc(val+0x30);
+    pc.putc(val+0x30);}
+    else {
+    command.putc(val-9+0x40); 
+    pc.putc(val-9+0x40);  }}
+    
+void print_bin2hex (uint8_t val)// Imprimir el comando enviado en Hexadecimal
+{
+    command.printf(" 0x");
+    print_num(val>>4);
+    print_num(val&0x0f); }
+ 
+// TODO : TIMEOUT UART SERIAL
+
+void Read_command() // Leer el comando que se digito en CoolTerm
+{
+    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() //Verifica el ultimo valor del comando enviado '>'
+{
+       if (buffer_command[BUFF_SIZE-1]== '>'){
+       
+        #if DEBUG
+        command.printf("\nComando:");
+        print_bin2hex(buffer_command[COMM_N]);
+        command.printf(" -> ");
+        #endif
+        return 1;
+        }
+        #if DEBUG
+        command.printf("\n ERROR COMANDO -> ");
+        echo_command();
+        #endif
+        return 0;       }
+        
+///////////////Funciones para dibujar////////////////
+   
+void command_led(int tm){
+                
+    led=1;  
+    wait(tm);
+    led=0;   }
+ 
+void punto(uint8_t x, uint8_t y){
+    
+    vertex2d(x,y);
+    wait(1);
+    draw();
+    wait(1);
+    nodraw();
+    #if DEBUG
+    command.printf("Coord x= %i, coord y=%i \n",x,y);
+    #endif
+    
+    }
+    
+void linea(float xi, float yi,  float  xf, float yf)
+    { 
+    #if DEBUG
+    command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP);
+    #endif
+    float xp,yp;
+    float m=(yf-yi)/(xf-xi);
+    float b=yf-(m*xf);
+    #if DEBUG
+    command.printf("\n b =%f, m=%f \n", b,m);
+    #endif
+    float nstep =(m/RSTEP);
+    //nstep=RSTEP;
+    #if DEBUG
+    command.printf("\nstep = %f \n", nstep);
+    #endif
+     if ((abs(xf-xi))>abs(yf-yi)){
+        if (xf>xi){
+           initdraw(xp,yp);
+           for (xp=xi; xp<=xf; xp+=RSTEP){
+            yp =m*xp+b;
+            vertex2d(xp,yp);
+            #if DEBUG
+            command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
+            #endif    
+             } }
+        else{
+            float temp = xi;
+            xi = xf;
+            xf = temp;
+            initdraw(xp,yp);
+            for (xp=xi; xp<=xf; xp+=RSTEP){
+            yp =m*xp+b;
+            vertex2d(xp,yp);
+            #if DEBUG
+            command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
+            #endif    
+            }}}
+        else {
+            if (yf>yi){
+            initdraw(xp,yp);
+            for (yp=yi; yp<=yf; yp+=RSTEP){
+            xp=(yp-b)/m;
+            vertex2d(xp,yp);
+            #if DEBUG
+            command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
+            #endif    
+            }}
+        else{
+            float tempo = yi;
+            yi = yf;
+            yf = tempo;
+            initdraw(xp,yp);
+            for (yp=yi; yp<=yf; yp+=RSTEP){
+            xp=(yp-b)/m;
+            vertex2d(xp,yp);
+            #if DEBUG
+            command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
+            #endif      
+             }                                  
+         }   
+        }
+    nodraw(); 
+    }
+    
+void rectangle(uint8_t x, uint8_t y, uint8_t a, uint8_t h)
+    {
+    #if DEBUG
+    command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP);  
+    #endif    
+    uint8_t A=x+a;
+    uint8_t B=y+h;     
+    initdraw(x,y);
+ 
+    for(uint8_t xi=x; xi<=(x+a); xi+=RSTEP){
+        vertex2d(xi,y);
+        #if DEBUG
+        command.printf("Coordenadas x=%i,y=%i for 1\n", xi,y);
+        #endif    
+        }  
+        wait(10);
+    for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){
+        vertex2d(x+a,yi);
+        #if DEBUG
+        command.printf("Coordenadas x=%i,y=%i for 2\n", x+a,yi);
+        #endif    
+        }
+    for(uint8_t xf=A; xf>x; xf= xf - RSTEP){
+         vertex2d(xf,B);
+        #if DEBUG
+        command.printf("Coordenadas x=%i,y=%i for 3\n", xf,B);
+        #endif    
+        }  
+    for (uint8_t yf=(y+h); yf>y; yf-=RSTEP){
+        vertex2d(x,yf);
+        #if DEBUG
+        command.printf("Coordenadas x=%i,y=%i for 4\n", x,yf);
+        #endif    
+        } 
+        vertex2d(x,y);
+        #if DEBUG
+        command.printf("Coordenadas x=%i,y=%i for 4\n", x,y);
+        #endif 
+    nodraw(); 
+    }
+void circle(float cx, float cy, float radio)    
+    { 
+    int y;
+    int x;    
+    vertex2d(cx,cy);    
+    draw();
+    wait(1);
+     for(double i=0; i<=90 ;i+=RSTEP)   
+     {
+      x=radio*cos(i*3.1416/180);
+      y=radio*sin(i*3.1416/180);
+      vertex2d(cx+x,cy+y);
+      #if DEBUG
+      command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
+      #endif
+      wait_ms(10);
+      }  
+      for(double i=90; i<=180 ;i+=RSTEP)
+     {
+      x=radio*cos(i*3.1416/180);
+      y=radio*sin(i*3.1416/180);
+      vertex2d(cx+x,cy+y);
+      #if DEBUG
+      command.printf("position x =%li ; position y =%li \n",x,y);
+      #endif
+      wait_ms(10);
+      }  
+      for(double i=180; i<=270 ;i+=RSTEP) 
+     {
+      x=radio*cos(i*3.1416/180);
+      y=radio*sin(i*3.1416/180);
+      vertex2d(cx+x,cy+y);
+      #if DEBUG
+      command.printf("position x =%li ; position y =%li\n",x,y);
+      #endif
+      wait_ms(10);
+      }
+       
+      for(double i=270; i<=360 ;i+=RSTEP) 
+     {
+      x=radio*cos(i*3.1416/180);
+      y=radio*sin(i*3.1416/180);
+      vertex2d(cx+x,cy+y);
+      #if DEBUG
+      command.printf("position x =%li , position y =%li \n",x,y);
+      #endif
+      wait_ms(10);
+      } 
+      wait(1);        
+      nodraw();
+ }
+ 
+void home(){
+    vertex2d(0,0);
+    nodraw();
+    
+    }
+    
+void resolucion(int res){
+    
+    RSTEP = res;
+    #if DEBUG
+    command.printf("Resolucion definida en =%i \n", RSTEP);
+    #endif 
+}
+
+void command_sstime(){
+    #if DEBUG
+    command.printf("config tiempo entre movimientos de los servos: ");
+    command.printf("SSTIME = %i\n", buffer_command[1]);
+    #endif
+    put_sstime(buffer_command[1]);
+     }    
+     
+void stop(){
+     
+    #if DEBUG
+    command.printf("stop");
+    #endif
+    exit(0);
+     }
+    
+void pausa(){
+   
+   // while(pause == 0){
+        
+        wait(10000);
+        //}
+          /*  #if DEBUG
+    command.printf("piccolo en pausa ");
+    #endif
+    led=0;
+    wait(2);*/
+}
+
+void reanudar(){
+ 
+    while(pause == 1){
+        
+        wait(0);
+        }
+    #if DEBUG
+        command.printf("reanuda piccolo ");
+    #endif   
+}
+//********MOTOR PASO A PASO*********//
+
+/*      Paso  A   B   C   D      Paso  E   F   G   H 
+          1   1   1   0   0        1   1   1   0   0
+          2   0   1   1   0        2   0   1   1   0
+          3   0   0   1   1        3   0   0   1   1
+          4   1   0   0   1        4   1   0   0   1
+*/
+     
+
+//*********Interrupcion*******//
+
+void interrup(){
+        val=command.getc();
+        if (val== '<'){
+            Read_command();
+        if (check_command()){
+                command_exe();
+                #if DEBUG
+                 echo_command();
+                #endif
+                command.printf("%s", buffer_command);
+                
+        }
+        }
+        else command.printf("error inicio trama: %d ",val);
+        command.putc(val);
+    return;
+    
+ }
+ 
+/////////CASOS PARA LA SELECCION DE COMANDOS/////////////
+ 
+void command_exe()
+    {
+    #if DEBUG
+    command.printf("Ejecutando comando: \n");
+    #endif
+    
+switch (buffer_command[COMM_N]){
+ 
+case (LED_NC): 
+    #if DEBUG
+    command.printf("led on 7 off\n");
+    #endif
+    command_led(buffer_command[INITPARAMETER]);
+break;
+
+case (DOT_NC):
+    #if DEBUG
+    command.printf("Punto \n");
+    #endif
+    punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]);
+break;
+ 
+case (LINE_NC):
+    #if DEBUG
+    command.printf("draw line\n");
+    #endif
+    linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+ 
+ 
+ 
+ 
+break;
+ 
+case (RECTANGLE_NC): 
+    #if DEBUG
+    command.printf("cuadrado \n");
+    #endif
+    rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
+ 
+break;
+case (CICLE_NC):
+    circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
+    #if DEBUG 
+    command.printf("Circulo\n");
+    #endif
+break;
+case (HOME_NC):
+    #if DEBUG
+    command.printf("Yendo a Home \n");
+    #endif
+    home();
+break;
+case (RESOLUCION_NC):
+    resolucion(buffer_command[INITPARAMETER]);
+    #if DEBUG
+    command.printf("Tiempo pasos \n");
+    #endif
+break;
+case (TIEMPOPASOS_NC):
+    sstime(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);
+    #if DEBUG
+    command.printf("Tiempo pasos \n");
+    #endif
+break;
+case (STOP_NC):
+    #if DEBUG
+    command.printf("Stop \n");
+    #endif
+break;
+
+case (PAUSA_NC):
+    #if DEBUG
+    command.printf("Pausa \n");
+    #endif
+    pausa();
+break;
+case (REANUDAR_NC):
+    #if DEBUG
+    command.printf("Reanudar \n");
+    #endif
+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
+    command.attach(&interrup);
+    /*while(1){
+   
+    //command.printf("%d",buffer_command[0]);
+      wait(1);
+    }*/
+    
+}