Revision 50:58a2df31077a, committed 2018-05-18

Comitter:

leogrotti

Date:

Fri May 18 15:28:53 2018 +0000

Parent:

48:76ccfa408a33

Child:

51:3f0e4e641054

Commit message:

C?digo Funcional com fim de curso

Changed in this revision

--- a/main.cpp	Thu May 17 17:22:35 2018 +0000
+++ b/main.cpp	Fri May 18 15:28:53 2018 +0000
@@ -70,9 +70,9 @@
 
 
 /* Variables -----------------------------------------------------------------*/
-//InterruptIn sensorX1(PC_9); 
-//InterruptIn sensorY1(PA_14); 
 
+DigitalIn fim_curso_X (PD_2);
+DigitalIn fim_curso_Y (PC_11);
 
 
 Serial pc(USBTX, USBRX);
@@ -85,9 +85,6 @@
 DigitalIn salva_pos (PC_1);
 DigitalIn vai_cami (PC_0);
 
-
-
-
 float x;
 float y;
 float w;
@@ -99,27 +96,11 @@
 float sp;
 float vc;
 
-int count;
-int position1Y ;
-int position1X ;
-int position1Z ;
-int position2Y ;
-int position2X ;
-int position2Z ;
+int Dir_X;
+int Dir_Y;
+int Dir_Z;
 
-int position3Y ;
-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 count;
 
 int dif_posX1;
 int dif_posY1;
@@ -135,15 +116,11 @@
 int listaY[10];
 int listaZ[10];
 
-
 int index = 0;
 
-
-
 bool flagX1=0; 
 bool flagY1=0; 
 
-
 unsigned int minspeed = 1300;
 
 //int step = 0x08;
@@ -162,42 +139,9 @@
 
 
 /* Main ----------------------------------------------------------------------*/
-/*
-void released_1()
-{
-    motor3->hard_stop();
-    printf ("parou \r\n");
 
-}
-
-void released_2()
-{
-    motor3->hard_stop();
-    printf ("parou \r\n");
-
-}
-
-void paraX1() {
-    flagX1 = 1;   
-    motor2->hard_stop();
-    
-    }
-    
-    void continuaX1() {
-    flagX1 = 0;
-    }
  
  
- void paraY1() {
-    flagY1 = 1;
-    motor1->hard_stop();
-    
-    }
-    
-    void continuaY1() {
-    flagY1 = 0;
-    }
- */
  int correcao_passo (int passo){
      int passo_verdade;
      if (passo != 0){
@@ -210,6 +154,7 @@
      
      }
  
+
  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();
@@ -219,19 +164,19 @@
     int calcdeltaY = correcao_passo(listapontosY[0] - PYatual);
     int calcdeltaZ = correcao_passo(listapontosZ[0] - PZatual);
      
-    if (calcdeltaX > 0 and calcdeltaY > 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();
         }
-    if (calcdeltaX > 0 and calcdeltaY < 0 ) {
+    if (calcdeltaX >= 0 and calcdeltaY < 0 ) {
         motor2->move(StepperMotor::FWD, calcdeltaX);
         motor1->move(StepperMotor::BWD, -calcdeltaY);
         motor2->wait_while_active();
         motor1->wait_while_active();
         }
-    if (calcdeltaX < 0 and calcdeltaY > 0 ) {
+    if (calcdeltaX < 0 and calcdeltaY >= 0 ) {
         motor2->move(StepperMotor::BWD, -calcdeltaX);
         motor1->move(StepperMotor::FWD, calcdeltaY);
         motor2->wait_while_active();
@@ -252,6 +197,7 @@
             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] );
@@ -270,19 +216,19 @@
         printf("Y vai andar: %d \r\n", calcdeltaY);
         printf("Z vai andar: %d \r\n", calcdeltaZ);
         
-        if (calcdeltaX > 0 and calcdeltaY > 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();
         }
-        if (calcdeltaX > 0 and calcdeltaY < 0) {
+        if (calcdeltaX >= 0 and calcdeltaY < 0) {
         motor2->move(StepperMotor::FWD, calcdeltaX);
         motor1->move(StepperMotor::BWD, -calcdeltaY);
         motor2->wait_while_active();
         motor1->wait_while_active();
         }
-        if (calcdeltaX < 0 and calcdeltaY > 0) {
+        if (calcdeltaX < 0 and calcdeltaY >= 0) {
         motor2->move(StepperMotor::BWD, -calcdeltaX);
         motor1->move(StepperMotor::FWD, calcdeltaY);
         motor2->wait_while_active();
@@ -306,9 +252,6 @@
         }
     }
         
-        
-        
-        
 int main() 
 {
     /*----- Initialization. -----*/
@@ -330,30 +273,6 @@
         exit(EXIT_FAILURE);
     }
 
- 
-
-
-
-    /* Attaching and enabling interrupt handlers. 
-    motor3->attach_flag_irq(&flag_irq_handler3);
-    motor3->enable_flag_irq();
-    motor1->attach_flag_irq(&flag_irq_handler1);
-    motor1->enable_flag_irq();
-    motor2->attach_flag_irq(&flag_irq_handler2);
-    motor2->enable_flag_irq();
-    */
-    
-    //sensorX1.rise(&paraX1);
-   // sensorX1.fall(&continuaX1);
-
-    
-    //sensorY1.rise(&paraY1);
-    //sensorY1.fall(&continuaY1);
-    
-//a = 0;
-//pos = 0;
-
-
 
     while(true) {
         count = count +1;
@@ -377,70 +296,93 @@
     if (zu >0.7) {
         motor3->run(StepperMotor::FWD);
         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();
-            speed3 = 0;
+            wait_ms(DELAY_1);
             }
                      
 // Movimentando eixo Z baixo caso botão 2 apertado
     if (zd >0.7) {
         motor3->run(StepperMotor::BWD);
         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();
-            speed3 = 0;
+            wait_ms(DELAY_1);
             }
             
              
 // Movimentando eixo Y duas direções com joystick
     //if (flagY1 == 0 ) {
         
+        
 // Movimentando eixo Y fwd        
-        if (u>0.820) {
+        if (u>0.820 && !fim_curso_Y) {
             motor1->run(StepperMotor::FWD);
-            speed1 = motor1->get_speed();
+            wait_ms(DELAY_1);
+            Dir_Y = 1;
             }
 // Movimentando eixo Y bwd             
-            else{ if(u<0.65) {
+            else if(u<0.65 && !fim_curso_Y) {
                 motor1->run(StepperMotor::BWD);
-                speed1 = motor1->get_speed(); 
+                wait_ms(DELAY_1);
+                Dir_Y = 0;
                 }
+                
 // parando eixo Y
-            else 
+            else {
                 motor1->hard_stop();
-                speed1 = 0;
-                //}
+                wait_ms(DELAY_1);
+                if (fim_curso_Y) {
+                    if (Dir_Y == 1){
+                        motor1->move(StepperMotor::BWD, 1000);
+                        motor1->wait_while_active();
+                        }
+                    if (Dir_Y == 0){
+                        motor1->move(StepperMotor::FWD, 1000);
+                        motor1->wait_while_active();
+                        }
+                    }
                 }
                 
 // Movimentando eixo X duas direções com joystick
     //if (flagX1 == 0) {
 // Movimentando eixo X fwd   
-        if (w>0.80) {
+        if (w>0.80 && !fim_curso_X) {
             motor2->run(StepperMotor::BWD);
-            speed2 = motor2->get_speed();
             wait_ms(DELAY_1);
-            }
+            Dir_X = 1;
+                }
+            
 // Movimentando eixo X Bwd 
-            else{ if(w<0.60) {
-                motor2->run(StepperMotor::FWD);
-                speed2 = motor2->get_speed();  
+            else if(w<0.60 && !fim_curso_X) {
+                motor2->run(StepperMotor::FWD); 
                 wait_ms(DELAY_1);
+                Dir_X = 0;
                 }
 // parando eixo X
-            else
+            else{
                 motor2->hard_stop();
-                speed2 = motor2->get_speed();
-                wait_ms(DELAY_1);
-                
+                wait_ms(DELAY_1);        
+                if (fim_curso_X) {
+                    if (Dir_X == 1){
+                        motor2->move(StepperMotor::FWD, 1000);
+                        motor2->wait_while_active();
+                        }
+                    if (Dir_X == 0){
+                        motor2->move(StepperMotor::BWD, 1000);
+                        motor2->wait_while_active();
+                        }
+                    }
                }
-
-    
+                  
   if (salva_pos) {
             listaX[index] = motor2->get_position();
             listaY[index] = motor1->get_position();
@@ -453,17 +395,12 @@
             index = index+1;
             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);
         }
-        }
-            
-            
-            
-    
+        }   
 
 }