Diff: main.cpp

Revision:

8:38ae341e2a4f

Parent:

7:fab201aa45b7

Child:

9:5008f9501dbf

--- a/main.cpp	Sat Sep 23 14:44:47 2017 +0000
+++ b/main.cpp	Sat Sep 30 02:32:24 2017 +0000
@@ -4,7 +4,9 @@
 Serial command(USBTX, USBRX);
 DigitalOut led(LED1);
 #define DEBUG 1
-
+PwmOut myServoX(PB_3);
+PwmOut myServoY(PB_6);
+PwmOut myServoZ(PB_4);
 //*****************************************************************************
 //  COMANDO  MOVER MOTOR
 //  |POS 1|POS 2|POS 3|POS 4| POS 5|
@@ -26,11 +28,25 @@
 #define LED_NC 0
 #define DOT_NC 1
 #define LINE_NC 2
-#define RECTANGLE_NC 3
+#define CUADRADO 3
 #define CICLE_NC 4
 #define HOME_NC 5
 
-
+int coord2pulse(float coord)
+{
+     if(0 <= coord <= MAXPOS)
+        return int(750+coord*1900/50);// u6
+    return 750;
+ 
+}
+void draw(){
+    myServoZ.pulsewidth_us(POSDRAW);
+   // myServoY.pulsewidth_us(750);
+ }
+void nodraw(){
+    myServoZ.pulsewidth_us(MAXPOS);
+   // myServoY.pulsewidth_us(2000);
+ }
 
 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0};
 
@@ -96,6 +112,39 @@
                 led=0;   
                  
 }
+void cuadrado(float x, float y, float lado){
+        float a = x+lado;
+        float b = y+lado;
+        
+       #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f, lado = %f \n", x,y,lado);
+        #endif
+        
+       vertex2d(x ,y);
+       draw();
+       vertex2d(x ,b);
+       #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f, lado = %f \n", x,b,lado);
+        #endif
+       vertex2d(a ,b);
+       #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f, lado = %f \n", a,b,lado);
+        #endif
+       vertex2d(a ,y);
+       #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f, lado = %f \n", a,y,lado);
+        #endif
+       vertex2d(x ,y);
+       #if DEBUG
+            Sdebug.printf("coord X = %f,coord Y = %f, lado = %f \n", x,y,lado);
+        #endif
+        wait_ms(SS_TIME);
+
+ }
+ void linea(float x1, float y1, float x2, float y2){
+     vertex2d(x1 ,y1);
+     vertex2d(x2 ,y2);
+     }
 void command_exe()
 {
     
@@ -118,10 +167,10 @@
 
 break;
 
-case RECTANGLE_NC: 
-    #if DEBUG
-    command.printf("draw rectabgle\n");
-    #endif
+case CUADRADO: 
+    
+    cuadrado(buffer_command[INITPARAMETER]);
+    
 
 
 break;