File content as of revision 3:c14aa9395c2a:

//---------------------------------LIBRERIAS--------------------------------------------
#include "mbed.h"
#include "math.h"
#include "cstdlib"
#include "InterruptIn.h"
 
//--------------------------DECLARACON DE CONTANTES-------------------------------
#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
 
//--------------------------DECLARACION DE PUERTOS I/O---------------------------- 
PwmOut myServoX(PB_3); //MOTOR EJE X
PwmOut myServoY(PB_4); //MOTOR EJE Y
PwmOut myServoZ(PB_10);//MOTOR EJE Z
 
InterruptIn boton(PC_0);
InterruptIn mybutton(USER_BUTTON);
Serial command(USBTX, USBRX); //COMUNICACION SERIAL
Serial Sdebug(USBTX, USBRX);  //DEBUG SERIAL
DigitalOut led(LED1);          //DECLARACION LED DE LA PLACA
 
//---------------------------DEFINICION COMANDOS POR CONSOLA---------------------- 
#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
 
//--------------------------LLAMADO RUTINA DE INTERRUPCIONES----------------------
//Ticker ticker1; //Variable para rutina de interupciones
//Timeout timeout;//Llamado de variable por conteo para manejo de pausa
 
//-------------------------VARIABLES GLOBALES-------------------------------------
int rap;//Pendiente 
int RSTEP = 5;
double abajo=7;
double arriba=0;
double k,l,r,tp=50,paso=0;
int p=0;
uint8_t estado = 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
//*****************************************************************************
 
//--------------------------DETERMINACION TAMAÑO DE BUFFER------------------------
uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};
float buffer_POS [BUFF_POS]= {0,0,0,0,0,0};
 
//--------------------FUNCIONES PARA COMANDO DE BUFFER----------------------------
 
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) 
    {
   
   
//-------------------------CONFIURACION SSTIME/VERTEX-----------------------------
     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);
    }
 
//--------------------------DECLARACION MEMORIA DE SERVOS-------------------------
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
//-------------------------VARIABLES SSTIME/RESOLUCION----------------------------
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
    }
    
//---------------------------------FUNCION HOME-----------------------------------
void home()
    {
        vertex2d(0,0,0);
        wait_ms(rap); 
    }    
void maxpos()
    {
        vertex2d(250,250,150);
    }
    
//---------------------------------FUNCION DOT-----------------------------------
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();
  }   
  
  
//------------------------------FUNCION LINE--------------------------------------
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);
         #if DEBUG
            Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp);
         #endif
        }
        nodraw();
        home();
    } 
    
//-----------------------------FUNCION RECTANGULO---------------------------------    
 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();
 }
 
 //-----------------------------FUNCION CIRCULO-----------------------------------
 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();
    }
        
}
//-------------------------------FUNCION REANUDAR---------------------------------
    void reanudar(){
        
        command.printf("piccolo reanudando \n");
        wait (1);
        return;
    
        }
//------------------------------FUNCION PAUSA------------------------------------        
void pause(){
       
       led = !led;                            
       command.printf("piccolo en pausa \n");
       wait (5);
       //return; 
       
    }
       
//-------------------------------FUNCION PARE-------------------------------------            
void stop() //Funcion para detener el programa  
    {
    command.printf("piccolo en Stop \n");
    home();  
    exit(0);
    
             
    }  
 
void command_exe()
    {
        
//-------------------------ASIGNACION EN SWTICH CASE------------------------------        
        #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
       
        }
}
 
//--------------------------***PENDIENTE***---------------------------------------
    void Rx_interrupcion(){
        
        char p = command.getc();
    
        command.printf("picolo en pausa %c\n",p);
      wait_ms(2000);
     return ;
           
        }    
        
    
//----------------------------------MAIN DE PROGRAMA------------------------------
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();
            mybutton.fall(&pause);
            mybutton.rise(&reanudar);
            boton.fall(&stop);
            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
        
           }
          }
                     
            }