Revision 43:d08a3f6b65b5, committed 2018-05-15

Comitter:

leogrotti

Date:

Tue May 15 18:47:13 2018 +0000

Parent:

42:a04bff02f231

Child:

44:eecdc0b60f14

Commit message:

tentativa go_to;

Changed in this revision

--- a/main.cpp	Tue May 15 14:23:01 2018 +0000
+++ b/main.cpp	Tue May 15 18:47:13 2018 +0000
@@ -56,8 +56,8 @@
 
 /* Delay in milliseconds. */
 #define DELAY_1 20
-#define DELAY_2 6000
-#define DELAY_3 8000
+#define DELAY_2 200
+#define DELAY_3 2000
 
 /* Speed in pps (Pulses Per Second).
    In Full Step mode: 1 pps = 1 step/s).
@@ -80,6 +80,10 @@
 AnalogIn eixo_ZU(A0);
 AnalogIn eixo_ZD(A1);
 
+DigitalIn salva_pos (PC_1);
+DigitalIn vai_cami (PC_0);
+
+
 
 
 float x;
@@ -88,8 +92,22 @@
 float u;
 float zu;
 float zd;
+float a;
+float pos;
+float sp;
+float vc;
 
 int count;
+int position1Y ;
+int position1X ;
+int position1Z ;
+int position2Y ;
+int position2X ;
+int position2Z ;
+
+int position3Y ;
+int position3X ;
+int position3Z ;
 
 
 bool flagX1=0; 
@@ -192,6 +210,10 @@
     //sensorY1.rise(&paraY1);
     //sensorY1.fall(&continuaY1);
     
+a = 0;
+pos = 0;
+
+
 
     while(true) {
         count = count +1;
@@ -200,45 +222,39 @@
         w = eixo_Y.read();
         zu = eixo_ZU.read();
         zd = eixo_ZD.read();
-        if (count == 10000) {
-        printf("zu =%f \r\n\n", zu);
-        printf("zd =%f \r\n\n", zd);
-        count = 0;
-        }
-        
-    //printf("X =%i \r\n\n", flagX1);
-   // printf("Y =%i \r\n\n", flagY1);
+        sp = salva_pos.read();
+        vc = vai_cami.read();
+        if (count == 9999999999999999) {
+            printf("zu =%f \r\n\n", zu);
+            printf("zd =%f \r\n\n", zd);
+            printf("sp =%f \r\n\n", sp);
+            printf("vc =%f \r\n\n", vc);
+            count = 0;
+            }
+
     
  // Movimentando eixo Z cima caso botão 1 apertado
     if (zu >0.7) {
         motor3->run(StepperMotor::FWD);
-        //wait_ms(DELAY_1);
         speed3 = motor3->get_speed();
-        //wait_ms(DELAY_1);
         }
                 
   // Parando eixo Z caso botão 1 e 2 liberado              
         if (zu< 0.7 && zd < 0.7){
             motor3->hard_stop();
-            //  wait_ms(DELAY_1);
             speed3 = 0;
-            //wait_ms(DELAY_1);
             }
                      
 // Movimentando eixo Z baixo caso botão 2 apertado
     if (zd >0.7) {
         motor3->run(StepperMotor::BWD);
-        //wait_ms(DELAY_1);
         speed3 = motor3->get_speed();
-        //wait_ms(DELAY_1);
         }
                 
   // Parando eixo Z caso botão 1 e 2 liberado              
         if (zu< 0.7 && zd < 0.7){
             motor3->hard_stop();
-            //  wait_ms(DELAY_1);
             speed3 = 0;
-            //wait_ms(DELAY_1);
             }
             
              
@@ -248,47 +264,152 @@
 // Movimentando eixo Y fwd        
         if (u>0.820) {
             motor1->run(StepperMotor::FWD);
-            //wait_ms(DELAY_1);
             speed1 = motor1->get_speed();
-            //wait_ms(DELAY_1);
             }
 // Movimentando eixo Y bwd             
             else{ if(u<0.65) {
                 motor1->run(StepperMotor::BWD);
-                //wait_ms(DELAY_1);
-                speed1 = motor1->get_speed();
-                // wait_ms(DELAY_1); 
+                speed1 = motor1->get_speed(); 
                 }
 // parando eixo Y
             else 
                 motor1->hard_stop();
-                //wait_ms(DELAY_1);
                 speed1 = 0;
-                //wait_ms(DELAY_1);
                 //}
                 }
                 
 // Movimentando eixo X duas direções com joystick
     //if (flagX1 == 0) {
-// Movimentando eixo Y fwd   
+// Movimentando eixo X fwd   
         if (w>0.80) {
             motor2->run(StepperMotor::BWD);
             speed2 = motor2->get_speed();
-            printf("speed2 =%d \r\n\n", speed2);
-            printf("W =%f \r\n\n", w);
+            wait_ms(DELAY_1);
             }
-// Movimentando eixo Y Bwd 
+// Movimentando eixo X Bwd 
             else{ if(w<0.60) {
                 motor2->run(StepperMotor::FWD);
                 speed2 = motor2->get_speed();  
-                printf("speed2 =%d \r\n\n", speed2);
-                printf("W =%f \r\n\n", w);
+                wait_ms(DELAY_1);
                 }
 // parando eixo X
             else
                 motor2->hard_stop();
-                speed2 = 0;
-                printf("speed2 =%d \r\n\n", speed2);
+                speed2 = motor2->get_speed();
+                wait_ms(DELAY_1);
+                
                }
+  /*  if (a==0);{
+        motor2->move(StepperMotor::BWD, 2000);
+        motor2->wait_while_active();
+        a= 1;
+        }
+    if (a==1);{
+        motor2->move(StepperMotor::FWD, 2000);
+        motor2->wait_while_active();
+        a= 0;
+        }
+    */
+    
+    if (salva_pos) {
+            if (pos==0) {
+                int position1Y = motor1->get_position();
+                int position1X = motor2->get_position();
+                int position1Z = motor3->get_position();
+            
+                printf( "pos1Y = %i \n\r", position1Y);
+                printf( "pos1X = %i \n\r", position1X);
+                printf( "pos1Z = %i \n\r", position1Z);
+            
+                pos = 1;
+                wait_ms(DELAY_2);
+                }
+            }
+        
+    
+    if (salva_pos) {
+            if (pos==1) {
+                int position2Y = motor1->get_position();
+                int position2X = motor2->get_position();
+                int position2Z = motor3->get_position();
+            
+                printf( "pos2Y = %i \n\r", position2Y);
+                printf( "pos2X = %i \n\r", position2X);
+                printf( "pos2Z = %i \n\r", position2Z);
+            
+                pos = 2;
+                wait_ms(DELAY_2);
+                }
+            }
+        
+    if (salva_pos) {
+            if (pos==2) {
+                int position3Y = motor1->get_position();
+                int position3X = motor2->get_position();
+                int position3Z = motor3->get_position();
+            
+                printf( "pos3Y = %i \n\r", position3Y);
+                printf( "pos3X = %i \n\r", position3X);
+                printf( "pos3Z = %i \n\r", position3Z);
+            
+                pos = 3;
+                wait_ms(DELAY_2);
+                }
+            }
+                
+    if (vai_cami) {
+        if (pos ==0){
+            printf("sem posicoes salvas");
+            wait_ms(DELAY_2);
+            }
+        if (pos == 1) {
+            motor1->go_to(position1Y);
+            motor2->go_to(position1X);
+            motor3->go_to(position1Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            }
+        if (pos == 2) {
+            motor1->go_to(position1Y);
+            motor2->go_to(position1X);
+            motor3->go_to(position1Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            wait_ms(DELAY_3);
+            motor1->go_to(position2Y);
+            motor2->go_to(position2X);
+            motor3->go_to(position2Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            }
+        if (pos == 3) {
+            motor1->go_to(position1Y);
+            motor2->go_to(position1X);
+            motor3->go_to(position1Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            wait_ms(DELAY_3);
+            motor1->go_to(position2Y);
+            motor2->go_to(position2X);
+            motor3->go_to(position2Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            wait_ms(DELAY_3);
+            motor1->go_to(position3Y);
+            motor2->go_to(position3X);
+            motor3->go_to(position3Z);
+            motor1->wait_while_active();
+            motor2->wait_while_active();
+            motor3->wait_while_active();
+            }
+            }
+            
+            
+            
     }
 }
\ No newline at end of file