File content as of revision 0:da21a18f1ee8:

#include "mbed.h"
#include "mbed.h"
#include "mbed.h"
#define MAXPOS 255

Serial command(USBTX, USBRX);
DigitalOut led(LED1);
#define DEBUG 1

//*****************************************************************************
//  COMANDO  MOVER MOTOR
//  |POS 1|POS 2|POS 3|POS 4| POS 5|
//  |  <  | #C  | a   | b   |  >   |
//
// #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
#define BUFF_SIZE 6
#define COMM_N 0
#define INITPARAMETER  1


// COMANDOS 
#define PUNTO 0
#define LINEA 1
#define CUADRADO 2
#define CIRCULO 3
#define HOME 4

PwmOut servoX(PB_3);
PwmOut servoY(PB_5);
PwmOut servoZ(PB_4);


DigitalIn botoncirculo(PA_0);
DigitalIn botonpunto(PB_0);
DigitalIn botonlinea(PC_1);
DigitalIn botoncuadrado(PC_0);
float xvalue, yvalue, xiline,yiline,xfline,yfline;
int xisquare, yisquare,lenght,xlado,ylado;
double i,j,r;
 
 
 int coord2pulse(float coord)
{
    if(0 <= coord <= MAXPOS)
    return int(coord*1000/150+750);
    return 750;
}
 
 
void vertex2d(float x, float y, float z){
 
    int pulseX = coord2pulse(x);
    int pulseY = coord2pulse(y);
    int pulseZ = coord2pulse(z);
    servoX.pulsewidth_us(pulseX);
    servoY.pulsewidth_us(pulseY);
    servoZ.pulsewidth_us(pulseZ);
}

uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0};

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);
        
        
}

// TODO : TIMEOUT UART SERIAL
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)
{
  
  //EJEMPLO DE COMANDO 
      #if DEBUG
        command.printf("%i, segundos", tm);
    #endif
              led=1;  
                wait(tm);
                led=0;   
                 
}

  
  
  void linea(uint8_t xi, uint8_t yi,uint8_t xf,uint8_t yf)
{      
    
    vertex2d(xi,yi,240);
    wait_ms(600);
    vertex2d(xf,yf,240);
    wait_ms(600);
   
}       


void command_exe()
{
    #if DEBUG
        command.printf("Ejecutando comando: ");
    #endif
    
switch (buffer_command[COMM_N]){

case (PUNTO): 
    linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2], buffer_command[INITPARAMETER+3]);   
    #if DEBUG
        command.printf("Dibujado Punto\n");
    #endif
    
    
break;

case (LINEA):
    #if DEBUG
    command.printf("Dibujando Linea\n");
    #endif  

break;

case  CUADRADO:
    #if DEBUG
    command.printf("Dibujando Cuadrado\n");
    #endif


break;

case CIRCULO: 
    #if DEBUG
    command.printf("Dibujando Circulo\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
    uint8_t val;
    while(1){
        val=command.getc();
        if (val== '<'){
            Read_command();
            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
        }
        
    }
}