Diff: main.cpp

Revision:

0:da21a18f1ee8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 12 02:16:21 2017 +0000
@@ -0,0 +1,215 @@
+#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
+        }
+        
+    }
+}
+