File content as of revision 17:53c6058d2021:

//*****************************LIBRERIAS***************************************/

#include "mbed.h"
#define DEBUG 1

/**************PUERTOS I/O DE SISTEMA EMBEBIDO NUCLEO-F411RE*******************/

Serial command(USBTX, USBRX);
DigitalOut led(LED1);
PwmOut myServoX(PB_3); //Servo X
PwmOut myServoY(PB_4); //Servo Y
PwmOut myServoZ(PC_7); //Servo Z
InterruptIn button(USER_BUTTON); // interrupcion boton stop
volatile bool button1_pressed = false;
volatile bool button1_enabled = true;

//*****************************************************************************
//                          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) 
// a,b,c,d parametros del comando
// <,> -> inicio,  y fin de comando 
//  el inicio de comando no se almacena en el buffer
//*****************************************************************************

//***************************VARIABLES DEFINIDAS*******************************/

#define BUFF_SIZE 6         // tamaño del comando enviado desde CoolTerm
#define COMM_N 0            //
#define INITPARAMETER  1    // 
#define MAXPOS 50           // Posicion maxima de la matriz de dibujo  x,y en mm
#define POSDRAW 30          // Posicion del servomotor Z  para dibujar
//#define SS_TIME 100         // no puedo degarla declarada cte. pues se debe mod por teclado
#define PI 3.1415926        //
uint8_t RSTEP = 5;              // Ini. variable de Resolucion para el dibujo 
uint8_t DET=1;
uint8_t  posx_old=0;     // posición anterior del eje X
uint8_t  posy_old=0;     // posición anterior del eje Y
uint8_t SSTIME = 100;  

/****************************COMANDOS DE TECLADO*******************************/     
    
#define LED_NC          0   //ok
#define DOT_NC          1   //ok
#define LINE_NC         2   //ok 
#define RECTANGULO_NC   3   //ok
#define CIRCLE_NC       4   //ok
#define HOME_NC         5   //ok
#define RESOLUCION_NC   6   //ok 
#define TIEMPOPASOS_NC  7   // No se a que se refiere. 
#define STOP_NC         8   // Pendiente
#define PAUSA_NC        9   // Pendiente
#define REANUDAR_NC     10  // Pendiente
//#define MOVER_NC        11  //Se refiere a el movimiento del motor paso a paso 


/**************FUNCIONES PARA MOVER LOS SERVOS X, Y, Z*************************/

int coord2pulse(float coord) // 
    {
     if(0 <= coord <= MAXPOS)
        return int(750+coord*1900/50);// u6
   return 750; 
    }
    
void vertex2d(float x, float y) // Funcion para enviarle la posicion a (x,y)
    { 
    wait_ms(SSTIME);
    int pulseY = coord2pulse(y);
    int pulseX = coord2pulse(x);
    
    myServoY.pulsewidth_us(pulseY);
    myServoX.pulsewidth_us(pulseX);
    
    }
    
void draw() // Funcion para enviarle la posicion  de dibujo a z. 
    {
    wait_ms(SSTIME*10);
    int pulseZ=coord2pulse(POSDRAW);    
    myServoZ.pulsewidth_us(pulseZ);
    wait_ms(SSTIME);
    }
void nodraw() // Funcion para enviarle la posicion de no dibujar a  z. 
    {     
    wait_ms(SSTIME*10); 
    int pulseZ=coord2pulse(0);    
    myServoZ.pulsewidth_us(pulseZ);
    
    }
    
void initdraw(float x, float y)//ok
    {
    vertex2d(x,y);
    wait_ms(SSTIME);
    draw();
}

/*****************FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO*****************/

uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0}; // Matriz del Comando enviado

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) // Imprimir el comando enviado en Hexadecimal
    {
    command.printf(" 0x");
    print_num(val>>4);
    print_num(val&0x0f);    
    }

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 EN EL COMANDO!!!!!!!!!!!!!! -> ");
        echo_command();
        #endif
    return 0;         
    }

/********************FUNCIONES PARA DIBUJAR EN EL PICOLO************************/
 
void Led(int tm) //Funcion para definir el tiempo de led en milisegundos 
    { 
    #if DEBUG
    command.printf("\nTiempo led: %i seg\n", tm);
    #endif
    led=1;  
    wait(tm);
    led=0;                    
    }
    
void punto(uint8_t x, uint8_t y) //Funcion para dibujar un punto 
    {
    #if DEBUG
    command.printf("\nCoordenadas  x=%i, y=%i\n",x,y);
    #endif
    initdraw(x,y);
    nodraw();
    }         
    
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 Rectangulo(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    
        }  
    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(uint8_t cx, uint8_t cy, uint8_t radio)    
    { 
    int y;
    int x;    
    vertex2d(cx,cy);
    #if DEBUG
      command.printf("\nCoordenadas xc =%i, yc =%i, Radio=%i \n",cx,cy,radio);
     #endif    
     
     for(double i=0; i<=PI/2 ;i+=((PI/2)/RSTEP))   
      {
      x=radio*cos(i);
      y=radio*sin(i);
      initdraw(x+cx,y+cy);
      #if DEBUG
      command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
      #endif
      }  
       #if DEBUG
      command.printf("\n");
      #endif
  for(double i=PI/2; i<=PI ;i+=((PI/2)/RSTEP))   
      {
      x=radio*cos(i);
      y=radio*sin(i);
      vertex2d(x+cx,y+cy);
      #if DEBUG
      command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio, ((PI/2)/RSTEP));
      #endif
      }    
      #if DEBUG
      command.printf("\n");
      #endif
  for(double i=PI; i<=((3*PI)/2) ;i+=((PI/2)/RSTEP))   
      {
      x=radio*cos(i);
      y=radio*sin(i);
      initdraw(x+cx,y+cy);
      #if DEBUG
      command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
      #endif
      }  
      #if DEBUG
      command.printf("\n");
      #endif
  for(double i=((3*PI)/2); i<=(2*PI) ;i+=((PI/2)/RSTEP))   
      {
      x=radio*cos(i);
      y=radio*sin(i);
      initdraw(x+cx,y+cy);
      #if DEBUG
      command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
      #endif
     }
 nodraw();
 }

void home()
    {
    nodraw();
    vertex2d(0,0);
     #if DEBUG
    command.printf("\nCoordenada HOME x=0, y =0");
    #endif
    }
        
void resolucion(int res) // Funcion para definir la resolucion del dibujo 
    {
    RSTEP = res;
    #if DEBUG
    command.printf("\nResolucion definida en=%i \n", RSTEP);
    #endif
    }    
 
void TiempoPasos(int SST) //Funcion para definir el tiempo de led
    {
    SSTIME=SST;     
    #if DEBUG
    command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
    #endif              
    }
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)(SSTIME*dist));
    posx_old =x;
    posy_old=y;
   
 }
 
void Stop() //Funcion para detener el programa  
    {
    exit(0);
        
  /*  if(ST == 0){
         home();
       #if DEBUG
        command.printf("\n...Parada total...\n", ST);
        #endif   
       exit(ST);
        }  */         
    }    

void Pausa(int p) //Funcion para definir el tiempo de led
    {   
    #if DEBUG
    command.printf("\n...Pausado...\n");
    #endif               
     }
    
void Reanudar(int det) //Funcion para definir el tiempo de led
    { 
    #if DEBUG
    command.printf("\nReanudando imagen...%i/n", DET);
    #endif    
         
    }
    
/*void Mover(int tm) //Funcion para definir el tiempo de led
    { 
    #if DEBUG
    command.printf("\nMover a: %i/n", tm);
    #endif               
    }  
*/

/*********************** CASOS DE USO PARA SELECCION DE COMANDOS***************/

void command_exe()
{   
    switch (buffer_command[COMM_N]){

        case (LED_NC): 
                        #if DEBUG
                        command.printf("    LED ON/OFF\n");
                        #endif
                        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("    LINEA\n");
                        #endif
                        linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);   
                    break;

        case (RECTANGULO_NC): 
                        #if DEBUG
                        command.printf("    RECTANGULO\n");
                        #endif
                        Rectangulo(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
                    break;
                    
         case (CIRCLE_NC):
                        #if DEBUG 
                        command.printf("    CIRCULO\n");
                        #endif
                        circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
                    break;

        case (HOME_NC):
                        #if DEBUG
                        command.printf("    HOME\n");
                        #endif
                        home();
                    break;
                    
        case (RESOLUCION_NC):
                        #if DEBUG
                        command.printf("    RESOLUCION\n");
                        #endif
                        resolucion(buffer_command[INITPARAMETER]);
                    break;
        
       case (TIEMPOPASOS_NC):
                        #if DEBUG
                        command.printf("    TIEMPO EN PASOS\n");
                        #endif
                        TiempoPasos(buffer_command[INITPARAMETER]);
                    break;
                   
        case (STOP_NC):
                        #if DEBUG
                        command.printf("    STOP\n");
                        #endif
                      //  Stop(buffer_command[INITPARAMETER]);
                        
                    break;
                                        
        case (PAUSA_NC):
                        #if DEBUG
                        command.printf("    PAUSA\n");
                        #endif
                        Pausa(buffer_command[INITPARAMETER]);
                    break;  
                    
        case (REANUDAR_NC):
                        #if DEBUG
                        command.printf("    REANUDAR\n");
                        #endif
                        Reanudar(buffer_command[INITPARAMETER]);
                    break;                             
                    
 /*       case (MOVER_NC):
                        #if DEBUG
                        command.printf("       MOVER\n");
                        #endif
                        Mover(buffer_command[INITPARAMETER]);
                    break;
  */                                       
    default:
        #if DEBUG
        command.printf("Comando  no encontrado\n");
        #endif        
    }
}

int main() 
{
    myServoX.period_ms(20);
    myServoY.period_ms(20);
    myServoZ.period_ms(20);
    
    #if DEBUG
    command.printf("inicio con debug\n");
    #else
    command.printf("inicio sin debug\n");
    #endif
    uint8_t val;
    button.fall(&Stop);
    while(1){
        val=command.getc();
        
        if (val== '<'){
            Read_command();         
            if (check_command()){
                command_exe();
                #if DEBUG
                echo_command();
                #endif              
                if(button1_pressed ==false){                  
    }
                                }
                        }   
           }
}