50_135_507_38_Insper / Mbed OS Teste_2Motores_Joystick_copy

2 Motores + Joystick

Dependencies:   X_NUCLEO_IHM01A1 TextLCD

Fork of HelloWorld_IHM01A1_2Motors_mbedOS by ST

Files at this revision

API Documentation at this revision

Revision 45:8fab21ab1251, committed 2018-05-16

Comitter:
leogrotti
Date:
Wed May 16 18:32:22 2018 +0000
Parent:
44:eecdc0b60f14
Child:
46:63b15a56cbe5
Commit message:
salva 3 pontos + vai automatico - corre??es de erro

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Wed May 16 15:40:31 2018 +0000
+++ b/main.cpp	Wed May 16 18:32:22 2018 +0000
@@ -191,8 +191,11 @@
  */
  int correcao_passo (int passo){
      int passo_verdade;
+     if (passo != 0){
      passo_verdade = passo/1.0892 - 15,56;
+     }
      return (passo_verdade);
+     
      }
      
 int main()
@@ -438,31 +441,31 @@
                         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){
+        if (correcao_passo(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));
+        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 (dif_posY1 >0){
+        if (correcao_passo(dif_posY1) >0){
             motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
             }
-        if (dif_posY1 <0){
-            dif_posY1 = -dif_posY1;
-            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+        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 (dif_posZ1 >0){
+        if (correcao_passo(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));
+        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();
             }
                 
@@ -490,31 +493,31 @@
                         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){
+        if (correcao_passo(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));
+        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 (dif_posY1 >0){
+        if (correcao_passo(dif_posY1) >0){
             motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
             }
-        if (dif_posY1 <0){
-            dif_posY1 = -dif_posY1;
-            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+        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 (dif_posZ1 >0){
+        if (correcao_passo(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));
+        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();
             }
                 
@@ -527,33 +530,42 @@
                         wait_ms(DELAY_2);    
                  dif_posX2 = position2X - positionX_agora2; 
                  dif_posY2 = position2Y - positionY_agora2; 
-                 dif_posZ2 = position2Z - positionZ_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 (dif_posX2 >0){
+        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 (dif_posX2 <0){
-            dif_posX2 = -dif_posX2;
-            motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2));
+        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 (dif_posY2 >0){
+        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 (dif_posY2 <0){
-            dif_posY2 = -dif_posY2;
-            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2));
+        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 (dif_posZ2 >0){
+        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 (dif_posZ2 <0){
-            dif_posZ2 = -dif_posZ2;
-            motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2));
+        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();
@@ -581,31 +593,31 @@
                         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){
+        if (correcao_passo(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));
+        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 (dif_posY1 >0){
+        if (correcao_passo(dif_posY1) >0){
             motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
             motor1->wait_while_active();
             }
-        if (dif_posY1 <0){
-            dif_posY1 = -dif_posY1;
-            motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
+        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 (dif_posZ1 >0){
+        if (correcao_passo(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));
+        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();
             }
                 
@@ -620,31 +632,31 @@
                  dif_posY2 = position2Y - positionY_agora2; 
                  dif_posZ2 = position2Z - positionZ_agora2;     
                  
-        if (dif_posX2 >0){
+        if (correcao_passo(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));
+        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 (dif_posY2 >0){
+        if (correcao_passo(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));
+        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 (dif_posZ2 >0){
+        if (correcao_passo(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));
+        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();
@@ -666,31 +678,31 @@
                  dif_posY3 = position3Y - positionY_agora3; 
                  dif_posZ3 = position3Z - positionZ_agora3;     
                  
-        if (dif_posX3 >0){
+        if (correcao_passo(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));
+        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 (dif_posY3 >0){
+        if (correcao_passo(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));
+        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 (dif_posZ3 >0){
+        if (correcao_passo(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));
+        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();