Revision 47:b5dc79d3fb35, committed 2018-05-17

Comitter:

leogrotti

Date:

Thu May 17 17:18:48 2018 +0000

Parent:

46:63b15a56cbe5

Child:

48:76ccfa408a33

Commit message:

salva ate 10 pontos com lista + tentativa interpola??o

Changed in this revision

--- a/main.cpp	Wed May 16 22:15:06 2018 +0000
+++ b/main.cpp	Thu May 17 17:18:48 2018 +0000
@@ -203,6 +203,9 @@
      if (passo != 0){
      passo_verdade = passo/1.0892 - 15,56;
      }
+     else {
+         passo_verdade = 0;
+         }
      return (passo_verdade);
      
      }
@@ -216,22 +219,31 @@
     int calcdeltaY = correcao_passo(listapontosY[0] - PYatual);
     int calcdeltaZ = correcao_passo(listapontosZ[0] - PZatual);
      
-    if (calcdeltaX > 0) {
+    if (calcdeltaX > 0 and calcdeltaY > 0 ) {
         motor2->move(StepperMotor::FWD, calcdeltaX);
+        motor1->move(StepperMotor::FWD, calcdeltaY);
         motor2->wait_while_active();
+        motor1->wait_while_active();
         }
-       else {
-            motor2->move(StepperMotor::BWD, -calcdeltaX);
-            motor2->wait_while_active();
-            }
-    if (calcdeltaY > 0) {
-        motor1->move(StepperMotor::FWD, calcdeltaY);
+    if (calcdeltaX > 0 and calcdeltaY < 0 ) {
+        motor2->move(StepperMotor::FWD, calcdeltaX);
+        motor1->move(StepperMotor::BWD, -calcdeltaY);
+        motor2->wait_while_active();
         motor1->wait_while_active();
         }
-        else {
-            motor1->move(StepperMotor::BWD, -calcdeltaY);
-            motor1->wait_while_active();
-            }
+    if (calcdeltaX < 0 and calcdeltaY > 0 ) {
+        motor2->move(StepperMotor::BWD, -calcdeltaX);
+        motor1->move(StepperMotor::FWD, calcdeltaY);
+        motor2->wait_while_active();
+        motor1->wait_while_active();
+        }
+    if (calcdeltaX < 0 and calcdeltaY < 0 ) {
+        motor2->move(StepperMotor::BWD, -calcdeltaX);
+        motor1->move(StepperMotor::BWD, -calcdeltaY);
+        motor2->wait_while_active();
+        motor1->wait_while_active();
+        }
+       
     if (calcdeltaZ > 0) {
         motor3->move(StepperMotor::FWD, calcdeltaZ);
         motor3->wait_while_active();
@@ -418,50 +430,7 @@
                 wait_ms(DELAY_1);
                 
                }
- /*   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();
-
-                
-
-                 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 (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) {
             listaX[index] = motor2->get_position();
@@ -477,347 +446,10 @@
            } 
             
             
-            /*if (pos==0) {
-                 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) {
-                 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) {
-                 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) {
         vai_caminho_desejado(listaX, listaY, listaZ, index);
         printf("Vai caminho pressionado %d \r\n", index);
         wait_ms(DELAY_2);
-        /*        printf( "pos = %f \n\r", pos);
-        if (pos ==0){
-            printf("sem posicoes salvas");
-            wait_ms(DELAY_2);
-            }
-        if (pos == 1) {
-                        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 (correcao_passo(dif_posX1) >0){
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX1) <0){
-           // correcao_passo(dif_posX1) = -correcao_passo(dif_posX1);
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) >0){
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) <0){
-            //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1);
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) >0){
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) <0){
-            //correcao_passo(dif_posZ1) = -correcao_passo(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) {
-                        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 (correcao_passo(dif_posX1) >0){
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX1) <0){
-            //correcao_passo(dif_posX1) = -correcao_passo(dif_posX1);
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) >0){
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) <0){
-            //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1);
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) >0){
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) <0){
-            //correcao_passo(dif_posZ1) = -correcao_passo(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;  
-                        printf( "difY2 = %i \n\r", correcao_passo(dif_posY2));
-                        printf( "difX2 = %i \n\r", correcao_passo(dif_posX2));
-                        printf( "difZ2 = %i \n\r", correcao_passo(dif_posZ2));   
-                 
-        if (correcao_passo(dif_posX2) >0){
-            printf( "fwd difX2 = %i \n\r", correcao_passo(dif_posX2));
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX2) <0){
-            //correcao_passo(dif_posX2) = -correcao_passo(dif_posX2);
-                                    printf( "bwd difX2 = %i \n\r", correcao_passo(dif_posX2));
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX2));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY2) >0){
-                                    printf( "fwd difY2 = %i \n\r", correcao_passo(dif_posY2));
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY2) <0){
-            //correcao_passo(dif_posY2) = -correcao_passo(dif_posY2);
-            printf( "bwd difY2 = %i \n\r", correcao_passo(dif_posY2));
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY2));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ2) >0){
-            printf( "fwd difZ2 = %i \n\r", correcao_passo(dif_posZ2));
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ2) <0){
-            //correcao_passo(dif_posZ2) = -correcao_passo(dif_posZ2);
-            printf( "bwd difZ2 = %i \n\r", correcao_passo(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);
-            }
-    
-     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 (correcao_passo(dif_posX1) >0){
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX1) <0){
-            //correcao_passo(dif_posX1) = -correcao_passo(dif_posX1);
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) >0){
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY1) <0){
-            //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1);
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) >0){
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ1) <0){
-            //correcao_passo(dif_posZ1) = -correcao_passo(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 (correcao_passo(dif_posX2) >0){
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX2) <0){
-            //correcao_passo(dif_posX2) = -correcao_passo(dif_posX2);
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX2));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY2) >0){
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY2) <0){
-            //correcao_passo(dif_posY2) = -correcao_passo(dif_posY2);
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY2));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ2) >0){
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ2) <0){
-            //correcao_passo(dif_posZ2) = -correcao_passo(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 (correcao_passo(dif_posX3) >0){
-            motor2->move(StepperMotor::FWD, correcao_passo(dif_posX3));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posX3) <0){
-            //correcao_passo(dif_posX3) = -correcao_passo(dif_posX3);
-            motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX3));
-            motor2->wait_while_active();
-            }
-        if (correcao_passo(dif_posY3) >0){
-            motor1->move(StepperMotor::FWD, correcao_passo(dif_posY3));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posY3) <0){
-            //correcao_passo(dif_posY3) = -correcao_passo(dif_posY3);
-            motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY3));
-            motor1->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ3) >0){
-            motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ3));
-            motor3->wait_while_active();
-            }
-        if (correcao_passo(dif_posZ3) <0){
-            //correcao_passo(dif_posZ3) = -correcao_passo(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);
-            }*/
         }
         }