File content as of revision 2:b0ed0b16b9c8:

#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 device(D1, D0, 9600); // tx rx
Serial command(D8,D2);
DigitalOut led(LED1);

 
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;
uint8_t estado = 1;
// 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){
    device.putc(val+0x30);
    command.putc(val+0x30);
    pc.putc(val+0x30);}
    else {
    device.putc(val-9+0x40);    
    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();
       }
     /*for (uint8_t i=0; i<BUFF_SIZE;i++){
    buffer_command[i]=device.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(xi,yi);//se cambio xp,yp por xi,yi
           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(xi,yi);//se cambio xp,yp por xi,yi
            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);//se cambio xp,yp por xi,yi
            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(xi,yi);//se cambio xp,yp por xi,yi
            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    
        }  
    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);    
    x=radio*cos(0*3.1416/180);
      y=radio*sin(0*3.1416/180);
      vertex2d(cx+x,cy+y);
    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(){
   estado = 0;
   wait (2);
}

void reanudar(){
 
    estado = 1;
    #if DEBUG
        command.printf("reanuda piccolo ");
    #endif   
}     

//*********Interrupcion*******//

void interrup(){
        //if(estado == 1){
        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() {
    
    //command.baud(9600);
    //device.baud(9600);
    #if DEBUG
    command.printf("inicio con debug\n");
    #else
    command.printf("inicio sin debug\n");
    #endif
   command.attach(&interrup,Serial::RxIrq);
    //command.attach(&interrup);
    while(1) {
        if(command.readable()) {
            command.putc(command.getc());
            command.attach(&interrup,Serial::RxIrq);
        }
        /*if(uart.readable()) {
            pc.putc(uart.getc());
            command.attach(&interrup,Serial::RxIrq);
        }*/
    }
    
}