cotrol on of

src/main.cpp

@@ -0,0 +1,117 @@
+#include <Arduino.h>
+#include <Wire.h>
+#include <MPU6050_tockn.h>
+TaskHandle_t TareaProcesador1;
+void loop1(void *pvParameters);
+volatile int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
+volatile int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
+volatile int dir = 0;
+bool FWD = HIGH;
+bool BWD = LOW;
+
+#define STEP_1 13
+#define DIR_1 25
+#define STEP_2 33
+#define DIR_2 14
+MPU6050 mpu6050(Wire);
+
+float ax, ay, az;
+long timer = 0;
+void setup()
+{
+  pinMode(23, OUTPUT);
+  pinMode(STEP_1, OUTPUT);
+  pinMode(STEP_2, OUTPUT);
+  pinMode(DIR_1, OUTPUT);
+  pinMode(DIR_2, OUTPUT);
+  digitalWrite(DIR_1, LOW);
+  digitalWrite(DIR_2, LOW);
+  Wire.begin();
+  Serial.begin(115200);
+  Wire.begin();
+  xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);
+
+  delay(2000); // Pause for 2 seconds
+
+  mpu6050.begin();
+  mpu6050.calcGyroOffsets(false);
+}
+
+void loop()
+{
+  mpu6050.update();
+  if (millis() - timer > 10)
+  {
+
+    // ax = mpu6050.getAngleX();
+    ay = mpu6050.getAngleY();
+    //az = mpu6050.getAngleZ();
+
+    /*   Serial.print(ax), Serial.print(",");
+    Serial.print(ay), Serial.print(",");
+    Serial.println(az);*/
+    if (ay > 4)
+    {
+      dir = -1;
+    }
+    else if (ay < -4)
+    {
+      dir = 1;
+    }
+    else
+    {
+      dir = 0;
+    }
+    throttle_left_motor = 6 * dir;
+    throttle_right_motor = 6 * dir;
+
+    timer = millis();
+  }
+}
+
+void loop1(void *pvParameters)
+{
+  for (;;)
+  {
+
+    throttle_counter_left_motor++; //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
+    if (throttle_counter_left_motor > throttle_left_motor_memory)
+    {                                                   //If the number of loops is larger then the throttle_left_motor_memory variable
+      throttle_counter_left_motor = 0;                  //Reset the throttle_counter_left_motor variable
+      throttle_left_motor_memory = throttle_left_motor; //Load the next throttle_left_motor variable
+      if (throttle_left_motor_memory < 0)
+      {                                   //If the throttle_left_motor_memory is negative
+        digitalWrite(DIR_1, BWD);         //Set output 3 low to reverse the direction of the stepper controller
+        throttle_left_motor_memory *= -1; //Invert the throttle_left_motor_memory variable
+      }
+      else
+        digitalWrite(DIR_1, FWD); //Set output 3 high for a forward direction of the stepper motor
+    }
+    else if (throttle_counter_left_motor == 1)
+      digitalWrite(STEP_1, HIGH); //Set output 2 high to create a pulse for the stepper controller
+    else if (throttle_counter_left_motor == 2)
+      digitalWrite(STEP_1, LOW); //Set output 2 low because the pulse only has to last for 20us
+
+    //right motor pulse calculations
+    throttle_counter_right_motor++; //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
+    if (throttle_counter_right_motor > throttle_right_motor_memory)
+    {                                                     //If the number of loops is larger then the throttle_right_motor_memory variable
+      throttle_counter_right_motor = 0;                   //Reset the throttle_counter_right_motor variable
+      throttle_right_motor_memory = throttle_right_motor; //Load the next throttle_right_motor variable
+      if (throttle_right_motor_memory < 0)
+      {                                    //If the throttle_right_motor_memory is negative
+        digitalWrite(DIR_2, BWD);          //Set output 5 low to reverse the direction of the stepper controller
+        throttle_right_motor_memory *= -1; //Invert the throttle_right_motor_memory variable
+      }
+      else
+        digitalWrite(DIR_2, FWD);
+      ; //Set output 5 high for a forward direction of the stepper motor
+    }
+    else if (throttle_counter_right_motor == 1)
+      digitalWrite(STEP_2, HIGH); //Set output 4 high to create a pulse for the stepper controller
+    else if (throttle_counter_right_motor == 2)
+      digitalWrite(STEP_2, LOW);
+
+    delay(1);
+  }
+}