Diff: main.cpp

Revision:

44:eecdc0b60f14

Parent:

43:d08a3f6b65b5

Child:

45:8fab21ab1251

--- a/main.cpp	Tue May 15 18:47:13 2018 +0000
+++ b/main.cpp	Wed May 16 15:40:31 2018 +0000
@@ -109,6 +109,27 @@
 int position3X ;
 int position3Z ;
 
+int positionY_agora1;
+int positionX_agora1;
+int positionZ_agora1;
+int positionY_agora2;
+int positionX_agora2;
+int positionZ_agora2;
+int positionY_agora3;
+int positionX_agora3;
+int positionZ_agora3;
+
+int dif_posX1;
+int dif_posY1;
+int dif_posZ1;
+int dif_posX2;
+int dif_posY2;
+int dif_posZ2;
+int dif_posX3;
+int dif_posY3;
+int dif_posZ3;
+
+
 
 bool flagX1=0; 
 bool flagY1=0; 
@@ -168,7 +189,12 @@
     flagY1 = 0;
     }
  */
- 
+ int correcao_passo (int passo){
+     int passo_verdade;
+     passo_verdade = passo/1.0892 - 15,56;
+     return (passo_verdade);
+     }
+     
 int main()
 {
     /*----- Initialization. -----*/
@@ -299,117 +325,386 @@
                 wait_ms(DELAY_1);
                 
                }
-  /*  if (a==0);{
-        motor2->move(StepperMotor::BWD, 2000);
+ /*   if (vai_cami){
+                 position1Y = motor1->get_position();
+                 position1X = motor2->get_position();
+                 position1Z = motor3->get_position();
+            
+                 printf( "pos1Y = %i \n\r", position1Y);
+                 printf( "pos1X = %i \n\r", position1X);
+                 printf( "pos1Z = %i \n\r", position1Z);
+
+                 
+        motor2->move(StepperMotor::BWD, 4574);
         motor2->wait_while_active();
-        a= 1;
+
+                
+
+                 positionY_agora1 = motor1->get_position();
+                 positionX_agora1= motor2->get_position();
+                 positionZ_agora1 = motor3->get_position();
+                printf( "posY_agora1 = %i \n\r", positionY_agora1);
+                printf( "posX_agora1 = %i \n\r", positionX_agora1);
+                printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
+                a= 1;
         }
-    if (a==1);{
-        motor2->move(StepperMotor::FWD, 2000);
+    if (salva_pos){
+                 position2Y = motor1->get_position();
+                 position2X = motor2->get_position();
+                 position2Z = motor3->get_position();
+            
+                 printf( "pos2Y = %i \n\r", position2Y);
+                 printf( "pos2X = %i \n\r", position2X);
+                 printf( "pos2Z = %i \n\r", position2Z);
+        motor2->move(StepperMotor::FWD, 4574);
         motor2->wait_while_active();
+                 positionY_agora2 = motor1->get_position();
+                 positionX_agora2= motor2->get_position();
+                 positionZ_agora2 = motor3->get_position();
+                printf( "posY_agora2 = %i \n\r", positionY_agora2);
+                printf( "posX_agora2 = %i \n\r", positionX_agora2);
+                printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
+        
         a= 0;
         }
+    
     */
     
-    if (salva_pos) {
+  if (salva_pos) {
             if (pos==0) {
-                int position1Y = motor1->get_position();
-                int position1X = motor2->get_position();
-                int position1Z = motor3->get_position();
+                 position1Y = motor1->get_position();
+                 position1X = motor2->get_position();
+                 position1Z = motor3->get_position();
             
                 printf( "pos1Y = %i \n\r", position1Y);
                 printf( "pos1X = %i \n\r", position1X);
                 printf( "pos1Z = %i \n\r", position1Z);
             
                 pos = 1;
+                printf( "pos = %f \n\r", pos);
                 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();
+                 position2Y = motor1->get_position();
+                 position2X = motor2->get_position();
+                 position2Z = motor3->get_position();
             
                 printf( "pos2Y = %i \n\r", position2Y);
                 printf( "pos2X = %i \n\r", position2X);
                 printf( "pos2Z = %i \n\r", position2Z);
             
                 pos = 2;
+                printf( "pos = %f \n\r", pos);
                 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();
+                 position3Y = motor1->get_position();
+                 position3X = motor2->get_position();
+                 position3Z = motor3->get_position();
             
                 printf( "pos3Y = %i \n\r", position3Y);
                 printf( "pos3X = %i \n\r", position3X);
                 printf( "pos3Z = %i \n\r", position3Z);
             
                 pos = 3;
+                printf( "pos = %f \n\r", pos);
                 wait_ms(DELAY_2);
                 }
-            }
-                
+                }
     if (vai_cami) {
+                printf( "pos = %f \n\r", pos);
         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);
+                        printf( "pos1Y = %i \n\r", position1Y);
+                        printf( "pos1X = %i \n\r", position1X);
+                        printf( "pos1Z = %i \n\r", position1Z);
+                 positionY_agora1 = motor1->get_position();
+                 positionX_agora1= motor2->get_position();
+                 positionZ_agora1 = motor3->get_position();
+                        printf( "posY_agora1 = %i \n\r", positionY_agora1);
+                        printf( "posX_agora1 = %i \n\r", positionX_agora1);
+                        printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
+                 dif_posX1 = position1X - positionX_agora1; 
+                 dif_posY1 = position1Y - positionY_agora1; 
+                 dif_posZ1 = position1Z - positionZ_agora1; 
+                 
+                        printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
+                        printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
+                        printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
+        if (dif_posX1 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posX1 <0){
+            dif_posX1 = -dif_posX1;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posY1 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
-            motor2->wait_while_active();
+            }
+        if (dif_posY1 <0){
+            dif_posY1 = -dif_posY1;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ1 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
             motor3->wait_while_active();
             }
+        if (dif_posZ1 <0){
+            dif_posZ1 = -dif_posZ1;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
+            motor3->wait_while_active();
+            }
+                
+                 positionY_agora1 = motor1->get_position();
+                 positionX_agora1= motor2->get_position();
+                 positionZ_agora1 = motor3->get_position();
+                        printf( "posY_agora1 = %i \n\r", positionY_agora1);
+                        printf( "posX_agora1 = %i \n\r", positionX_agora1);
+                        printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
+            }
         if (pos == 2) {
-            motor1->go_to(position1Y);
-            motor2->go_to(position1X);
-            motor3->go_to(position1Z);
+                        printf( "pos1Y = %i \n\r", position1Y);
+                        printf( "pos1X = %i \n\r", position1X);
+                        printf( "pos1Z = %i \n\r", position1Z);
+                 positionY_agora1 = motor1->get_position();
+                 positionX_agora1= motor2->get_position();
+                 positionZ_agora1 = motor3->get_position();
+                        printf( "posY_agora1 = %i \n\r", positionY_agora1);
+                        printf( "posX_agora1 = %i \n\r", positionX_agora1);
+                        printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
+                 dif_posX1 = position1X - positionX_agora1; 
+                 dif_posY1 = position1Y - positionY_agora1; 
+                 dif_posZ1 = position1Z - positionZ_agora1; 
+                 
+                        printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
+                        printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
+                        printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
+        if (dif_posX1 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posX1 <0){
+            dif_posX1 = -dif_posX1;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posY1 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
-            motor2->wait_while_active();
+            }
+        if (dif_posY1 <0){
+            dif_posY1 = -dif_posY1;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ1 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
             motor3->wait_while_active();
-            wait_ms(DELAY_3);
-            motor1->go_to(position2Y);
-            motor2->go_to(position2X);
-            motor3->go_to(position2Z);
+            }
+        if (dif_posZ1 <0){
+            dif_posZ1 = -dif_posZ1;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
+            motor3->wait_while_active();
+            }
+                
+                 positionY_agora2 = motor1->get_position();
+                 positionX_agora2= motor2->get_position();
+                 positionZ_agora2 = motor3->get_position();
+                        printf( "posY_agora2 = %i \n\r", positionY_agora2);
+                        printf( "posX_agora2 = %i \n\r", positionX_agora2);
+                        printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
+                        wait_ms(DELAY_2);    
+                 dif_posX2 = position2X - positionX_agora2; 
+                 dif_posY2 = position2Y - positionY_agora2; 
+                 dif_posZ2 = position2Z - positionZ_agora2;     
+                 
+        if (dif_posX2 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
+            motor2->wait_while_active();
+            }
+        if (dif_posX2 <0){
+            dif_posX2 = -dif_posX2;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2));
+            motor2->wait_while_active();
+            }
+        if (dif_posY2 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
             motor1->wait_while_active();
-            motor2->wait_while_active();
+            }
+        if (dif_posY2 <0){
+            dif_posY2 = -dif_posY2;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ2 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
             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();
+        if (dif_posZ2 <0){
+            dif_posZ2 = -dif_posZ2;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2));
             motor3->wait_while_active();
-            wait_ms(DELAY_3);
-            motor1->go_to(position2Y);
-            motor2->go_to(position2X);
-            motor3->go_to(position2Z);
+            }   
+                 positionY_agora2 = motor1->get_position();
+                 positionX_agora2= motor2->get_position();
+                 positionZ_agora2 = motor3->get_position();
+                        printf( "posY_agora2 = %i \n\r", positionY_agora2);
+                        printf( "posX_agora2 = %i \n\r", positionX_agora2);
+                        printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
+            }
+    
+     if (pos == 3) {
+                        printf( "pos1Y = %i \n\r", position1Y);
+                        printf( "pos1X = %i \n\r", position1X);
+                        printf( "pos1Z = %i \n\r", position1Z);
+                 positionY_agora1 = motor1->get_position();
+                 positionX_agora1= motor2->get_position();
+                 positionZ_agora1 = motor3->get_position();
+                        printf( "posY_agora1 = %i \n\r", positionY_agora1);
+                        printf( "posX_agora1 = %i \n\r", positionX_agora1);
+                        printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
+                 dif_posX1 = position1X - positionX_agora1; 
+                 dif_posY1 = position1Y - positionY_agora1; 
+                 dif_posZ1 = position1Z - positionZ_agora1; 
+                 
+                        printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
+                        printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
+                        printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
+        if (dif_posX1 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posX1 <0){
+            dif_posX1 = -dif_posX1;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
+            motor2->wait_while_active();
+            }
+        if (dif_posY1 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
-            motor2->wait_while_active();
+            }
+        if (dif_posY1 <0){
+            dif_posY1 = -dif_posY1;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ1 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
             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();
+            }
+        if (dif_posZ1 <0){
+            dif_posZ1 = -dif_posZ1;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
             motor3->wait_while_active();
             }
+                
+                 positionY_agora2 = motor1->get_position();
+                 positionX_agora2= motor2->get_position();
+                 positionZ_agora2 = motor3->get_position();
+                        printf( "posY_agora2 = %i \n\r", positionY_agora2);
+                        printf( "posX_agora2 = %i \n\r", positionX_agora2);
+                        printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
+                        wait_ms(DELAY_2);    
+                 dif_posX2 = position2X - positionX_agora2; 
+                 dif_posY2 = position2Y - positionY_agora2; 
+                 dif_posZ2 = position2Z - positionZ_agora2;     
+                 
+        if (dif_posX2 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
+            motor2->wait_while_active();
             }
+        if (dif_posX2 <0){
+            dif_posX2 = -dif_posX2;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2));
+            motor2->wait_while_active();
+            }
+        if (dif_posY2 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
+            motor1->wait_while_active();
+            }
+        if (dif_posY2 <0){
+            dif_posY2 = -dif_posY2;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ2 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
+            motor3->wait_while_active();
+            }
+        if (dif_posZ2 <0){
+            dif_posZ2 = -dif_posZ2;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2));
+            motor3->wait_while_active();
+            }   
+                 positionY_agora2 = motor1->get_position();
+                 positionX_agora2= motor2->get_position();
+                 positionZ_agora2 = motor3->get_position();
+                        printf( "posY_agora2 = %i \n\r", positionY_agora2);
+                        printf( "posX_agora2 = %i \n\r", positionX_agora2);
+                        printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
+            
+            
+                positionY_agora3 = motor1->get_position();
+                 positionX_agora3= motor2->get_position();
+                 positionZ_agora3 = motor3->get_position();
+                        printf( "posY_agora3 = %i \n\r", positionY_agora3);
+                        printf( "posX_agora3 = %i \n\r", positionX_agora3);
+                        printf( "posZ_agora3 = %i \n\r", positionZ_agora3);
+                        wait_ms(DELAY_2);    
+                 dif_posX3 = position3X - positionX_agora3; 
+                 dif_posY3 = position3Y - positionY_agora3; 
+                 dif_posZ3 = position3Z - positionZ_agora3;     
+                 
+        if (dif_posX3 >0){
+            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX3));
+            motor2->wait_while_active();
+            }
+        if (dif_posX3 <0){
+            dif_posX3 = -dif_posX3;
+            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX3));
+            motor2->wait_while_active();
+            }
+        if (dif_posY3 >0){
+            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY3));
+            motor1->wait_while_active();
+            }
+        if (dif_posY3 <0){
+            dif_posY3 = -dif_posY3;
+            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY3));
+            motor1->wait_while_active();
+            }
+        if (dif_posZ3 >0){
+            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ3));
+            motor3->wait_while_active();
+            }
+        if (dif_posZ3 <0){
+            dif_posZ3 = -dif_posZ3;
+            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ3));
+            motor3->wait_while_active();
+            }   
+                 positionY_agora3 = motor1->get_position();
+                 positionX_agora3= motor2->get_position();
+                 positionZ_agora3 = motor3->get_position();
+                        printf( "posY_agora3 = %i \n\r", positionY_agora3);
+                        printf( "posX_agora3 = %i \n\r", positionX_agora3);
+                        printf( "posZ_agora3 = %i \n\r", positionZ_agora3);
+            }
+        }
+        }
             
             
             
     }
-}
\ No newline at end of file
+
+//}