Revision 46:63b15a56cbe5, committed 2018-05-16

Comitter:

leogrotti

Date:

Wed May 16 22:15:06 2018 +0000

Parent:

45:8fab21ab1251

Child:

47:b5dc79d3fb35

Commit message:

teste l?gica com lista;

Changed in this revision

--- a/main.cpp	Wed May 16 18:32:22 2018 +0000
+++ b/main.cpp	Wed May 16 22:15:06 2018 +0000
@@ -48,6 +48,8 @@
 /* Component specific header files. */
 #include "L6474.h"
 
+#include <list>
+
 
 /* Definitions ---------------------------------------------------------------*/
 
@@ -129,6 +131,13 @@
 int dif_posY3;
 int dif_posZ3;
 
+int listaX[10];
+int listaY[10];
+int listaZ[10];
+
+
+int index = 0;
+
 
 
 bool flagX1=0; 
@@ -197,8 +206,89 @@
      return (passo_verdade);
      
      }
+ 
+ void vai_caminho_desejado (int listapontosX[10], int listapontosY[10], int listapontosZ[10], int index_local) {
+    int PYatual = motor1->get_position();
+    int PXatual = motor2->get_position();
+    int PZatual = motor3->get_position();
      
-int main()
+    int calcdeltaX = correcao_passo(listapontosX[0] - PXatual);
+    int calcdeltaY = correcao_passo(listapontosY[0] - PYatual);
+    int calcdeltaZ = correcao_passo(listapontosZ[0] - PZatual);
+     
+    if (calcdeltaX > 0) {
+        motor2->move(StepperMotor::FWD, calcdeltaX);
+        motor2->wait_while_active();
+        }
+       else {
+            motor2->move(StepperMotor::BWD, -calcdeltaX);
+            motor2->wait_while_active();
+            }
+    if (calcdeltaY > 0) {
+        motor1->move(StepperMotor::FWD, calcdeltaY);
+        motor1->wait_while_active();
+        }
+        else {
+            motor1->move(StepperMotor::BWD, -calcdeltaY);
+            motor1->wait_while_active();
+            }
+    if (calcdeltaZ > 0) {
+        motor3->move(StepperMotor::FWD, calcdeltaZ);
+        motor3->wait_while_active();
+        }
+        else {
+            motor3->move(StepperMotor::BWD, -calcdeltaZ);
+            motor3->wait_while_active();
+            }
+    printf("O for vai rodar: %d \r\n", index_local-1);
+    for (int i = 1; i< index_local ; i++) {
+        printf("X deveria estar : %d \r\n", listapontosX[i-1] );
+        printf("Y deveria estar: %d \r\n", listapontosY[i-1]);
+        printf("Z deveria estar: %d \r\n", listapontosZ[i-1]);
+        
+        printf("X quer ir : %d \r\n", listapontosX[i] );
+        printf("Y quer ir: %d \r\n", listapontosY[i]);
+        printf("Z quer ir: %d \r\n", listapontosZ[i]);
+        
+        calcdeltaX = correcao_passo(listapontosX[i] - listapontosX[i-1]);
+        calcdeltaY = correcao_passo(listapontosY[i] - listapontosY[i-1]);
+        calcdeltaZ = correcao_passo(listapontosZ[i] - listapontosZ[i-1]);
+        
+        printf("X vai andar: %d \r\n", calcdeltaX);
+        printf("Y vai andar: %d \r\n", calcdeltaY);
+        printf("Z vai andar: %d \r\n", calcdeltaZ);
+        
+        if (calcdeltaX > 0) {
+        motor2->move(StepperMotor::FWD, calcdeltaX);
+        motor2->wait_while_active();
+        }
+            else {
+            motor2->move(StepperMotor::BWD, -calcdeltaX);
+            motor2->wait_while_active();
+            }
+        if (calcdeltaY > 0) {
+        motor1->move(StepperMotor::FWD, calcdeltaY);
+        motor1->wait_while_active();
+        }
+            else {
+            motor1->move(StepperMotor::BWD, -calcdeltaY);
+            motor1->wait_while_active();
+            }
+        if (calcdeltaZ > 0) {
+        motor3->move(StepperMotor::FWD, calcdeltaZ);
+        motor3->wait_while_active();
+        }
+            else {
+            motor3->move(StepperMotor::BWD, -calcdeltaZ);
+            motor3->wait_while_active();
+            }
+        }
+    }
+        
+        
+        
+        
+int main() 
 {
     /*----- Initialization. -----*/
 
@@ -239,8 +329,8 @@
     //sensorY1.rise(&paraY1);
     //sensorY1.fall(&continuaY1);
     
-a = 0;
-pos = 0;
+//a = 0;
+//pos = 0;
 
 
 
@@ -374,7 +464,20 @@
     */
     
   if (salva_pos) {
-            if (pos==0) {
+            listaX[index] = motor2->get_position();
+            listaY[index] = motor1->get_position();
+            listaZ[index] = motor3->get_position();
+            for (int j=0; j<=index; j++) {
+                printf("posX%d %d: \r\n", j, listaX[j]);
+                printf("posY%d %d: \r\n", j, listaY[j]);
+                printf("posZ%d %d: \r\n", j, listaZ[j]);
+                }
+            index = index+1;
+            wait_ms(DELAY_2);
+           } 
+            
+            
+            /*if (pos==0) {
                  position1Y = motor1->get_position();
                  position1X = motor2->get_position();
                  position1Z = motor3->get_position();
@@ -417,9 +520,12 @@
                 printf( "pos = %f \n\r", pos);
                 wait_ms(DELAY_2);
                 }
-                }
+                }*/
     if (vai_cami) {
-                printf( "pos = %f \n\r", pos);
+        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);
@@ -711,12 +817,12 @@
                         printf( "posY_agora3 = %i \n\r", positionY_agora3);
                         printf( "posX_agora3 = %i \n\r", positionX_agora3);
                         printf( "posZ_agora3 = %i \n\r", positionZ_agora3);
-            }
+            }*/
         }
         }
             
             
             
-    }
+    
 
-//}
+}