Diff: main.cpp

Revision:

1:14cd9d276832

Parent:

0:efeca84ca0a5

--- a/main.cpp	Mon Nov 18 13:40:34 2019 +0000
+++ b/main.cpp	Fri Mar 06 18:54:17 2020 +0000
@@ -3,56 +3,182 @@
 DigitalIn BT(BUTTON1);
 //DigitalOut led(LED1);
 
-DigitalOut IN1_MOTOR1(D13);
-DigitalOut IN2_MOTOR1(D12);
-DigitalOut EN_MOTOR1(D11);
+DigitalOut IN1_MOTOR1(D2);
+DigitalOut IN2_MOTOR1(D4);
+
 
-DigitalOut IN1_MOTOR2(D8);
-DigitalOut IN2_MOTOR2(D10);
-DigitalOut EN_MOTOR2(D9);
+DigitalOut IN1_MOTOR2(D7);
+DigitalOut IN2_MOTOR2(D8);
+
+DigitalOut EN_MOTOR_X(D3);
+DigitalOut EN_MOTOR_Y(D5);
+DigitalOut EN_MOTOR_Z(D6);
 
 Serial pc(USBTX, USBRX); // tx, rx
 
+void steps_motor(int STEPS_AMOUNT, int DIRECTION, int SPEED, int AXIS); //speed = passos por segundo
+
+int step = 1;
 
 int main()
 {
+    pc.baud(115200);
     pc.printf("Iniciando...\n\r");
-    IN1_MOTOR1 = 0;
-    IN2_MOTOR1 = 0;
-    IN1_MOTOR2 = 0;
-    IN2_MOTOR2 = 0;
+    wait_ms(1000);
     
-    EN_MOTOR1 = 1;
-    EN_MOTOR2 = 1;
+    steps_motor(200, 0, 50, 1);
+    steps_motor(200, 1, 50, 2);
+    steps_motor(200, 0, 50, 3);
+
     
-    wait_ms(2000);
-
     while(1) 
-    {
-        pc.printf("CW\n\r");
-        IN1_MOTOR1 = 0;
-        IN2_MOTOR1 = 1;
-        IN1_MOTOR2 = 0;
-        IN2_MOTOR2 = 1;
-        wait_ms(4000);
-        
-        IN1_MOTOR1 = 0;
-        IN2_MOTOR1 = 0;
-        IN1_MOTOR2 = 0;
-        IN2_MOTOR2 = 0;
-        wait_ms(500);
-        
-        pc.printf("CCW\n\r");
-        IN1_MOTOR1 = 1;
-        IN2_MOTOR1 = 0;
-        IN1_MOTOR2 = 1;
-        IN2_MOTOR2 = 0;
-        wait_ms(4000); 
-        
-        IN1_MOTOR1 = 0;
-        IN2_MOTOR1 = 0;
-        IN1_MOTOR2 = 0;
-        IN2_MOTOR2 = 0;
-        wait_ms(500);      
+    { 
+        if(BT == 0)
+        {
+            steps_motor(1, 0, 10, 3);
+            wait(0.1); 
+        }
     }
 }
+
+
+
+void steps_motor(int STEPS_AMOUNT, int DIRECTION, int SPEED, int AXIS)
+{
+    if(AXIS == 1)
+    {
+        EN_MOTOR_X = 1;  
+    }
+    
+    if(AXIS == 2)
+    {
+        EN_MOTOR_Y = 1; 
+    }
+    
+    if(AXIS == 3)
+    {
+        EN_MOTOR_Z = 1; 
+    }
+    
+    SPEED = 1000 / SPEED; 
+    SPEED = SPEED / 4;
+    
+    if(DIRECTION == 0)
+    {
+        while(STEPS_AMOUNT != 0)
+        {
+            if((step == 1) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 1;
+                IN2_MOTOR1 = 0;
+                IN1_MOTOR2 = 1;
+                IN2_MOTOR2 = 0;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+                pc.printf("\n\r STEPS_AMOUNT 1: %i",STEPS_AMOUNT);
+            }
+                        
+            if((step == 2) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 0;
+                IN2_MOTOR1 = 1;
+                IN1_MOTOR2 = 1;
+                IN2_MOTOR2 = 0;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+                pc.printf("\n\r STEPS_AMOUNT 2: %i",STEPS_AMOUNT);
+            }
+                    
+            if((step == 3) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 0;
+                IN2_MOTOR1 = 1;
+                IN1_MOTOR2 = 0;
+                IN2_MOTOR2 = 1;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+                pc.printf("\n\r STEPS_AMOUNT 3: %i",STEPS_AMOUNT);
+            }
+                  
+            if((step == 4) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 1;
+                IN2_MOTOR1 = 0;
+                IN1_MOTOR2 = 0;
+                IN2_MOTOR2 = 1;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+                pc.printf("\n\r STEPS_AMOUNT 4: %i",STEPS_AMOUNT);
+            }
+            
+            if(step > 4)
+            {
+                step = 1;
+            }   
+        }
+        pc.printf("\n\r Acabei");
+    }
+
+    if(DIRECTION == 1)
+    {
+        while(STEPS_AMOUNT > 0)
+        {
+            if((step == 1) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 1;
+                IN2_MOTOR1 = 0;
+                IN1_MOTOR2 = 0;
+                IN2_MOTOR2 = 1;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+            }
+                        
+            if((step == 2) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 0;
+                IN2_MOTOR1 = 1;
+                IN1_MOTOR2 = 0;
+                IN2_MOTOR2 = 1;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+            }
+                    
+            if((step == 3) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 0;
+                IN2_MOTOR1 = 1;
+                IN1_MOTOR2 = 1;
+                IN2_MOTOR2 = 0;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+            }
+                  
+            if((step == 4) && (STEPS_AMOUNT > 0))
+            {
+                IN1_MOTOR1 = 1;
+                IN2_MOTOR1 = 0;
+                IN1_MOTOR2 = 1;
+                IN2_MOTOR2 = 0;
+                wait_ms(SPEED);
+                step++;
+                STEPS_AMOUNT--;
+            }
+            
+            if(step > 4)
+            {
+                step = 1;
+            }
+        
+        }
+    }
+    EN_MOTOR_X = 0;
+    EN_MOTOR_Y = 0;
+    EN_MOTOR_Z = 0;
+}