control on off

src/main.cpp

@@ -1,24 +1,45 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include <MPU6050_tockn.h>
-TaskHandle_t TareaProcesador1;
+//TaskHandle_t TareaProcesador1;
-void loop1(void *pvParameters);
+//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;
+definiciones  de los motores paso a paso
-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);
+#define max_speed 100
+void IRAM_ATTR onTimer(); //declaración callback interrupciones tick Motores
+void motores_calc();
+void control_on_off(float angulo);
+hw_timer_t *timer_motores = NULL; //timer para controlar tick de los motores paso a paso
+portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
+volatile int tick_motores;
 
-float ax, ay, az;
+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 float setPoint = 0.0;
+volatile int dir = 0;
+bool FWD = HIGH;
+bool BWD = LOW;
+//==========================================
+/*
+============================================
+Definiciones MPU6050
+============================================
+*/
+MPU6050 mpu6050(Wire);
+float ay;
 long timer = 0;
+//==========================================
+
 void setup()
 {
+  //Configuracion motores
   pinMode(23, OUTPUT);
   pinMode(STEP_1, OUTPUT);
   pinMode(STEP_2, OUTPUT);
@@ -26,54 +47,86 @@ void setup()
   pinMode(DIR_2, OUTPUT);
   digitalWrite(DIR_1, LOW);
   digitalWrite(DIR_2, LOW);
+  timer_motores = timerBegin(0, 80, true);
+  timerAttachInterrupt(timer_motores, &onTimer, true);
+  timerAlarmWrite(timer_motores, 50, true);
+
+  //I2C
   Wire.begin();
   Serial.begin(115200);
   Wire.begin();
-  xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);
+  // xTaskCreatePinnedToCore(loop1, "Tarea1", 10000, NULL, 1, &TareaProcesador1, 0);
 
-  delay(2000); // Pause for 2 seconds
+  delay(1000); // Pause for 1 seconds
 
+  //Configuracion MPU6050
   mpu6050.begin();
   mpu6050.calcGyroOffsets(false);
+  delay(2000);
+
+  //Calculamos el setPoint inicial
+  Serial.print("\n[");
+  for (int i = 0; i < 100; i++)
+  {
+    mpu6050.update();
+    setPoint += mpu6050.getAngleY();
+    //Serial.print(setPoint),Serial.print(",");
+    delay(100);
+  }
+  Serial.print("]");
+  setPoint = setPoint / 100;
+  Serial.print("\nSetpoint:"), Serial.println(setPoint);
+  Serial.print("Core: "), Serial.println(xPortGetCoreID());
+
+  timerAlarmEnable(timer_motores);
 }
 
 void loop()
 {
+
+  if (millis() - timer > 4)
+  {
     mpu6050.update();
-  if (millis() - timer > 10)
-  {
-
-    // ax = mpu6050.getAngleX();
     ay = mpu6050.getAngleY();
-    //az = mpu6050.getAngleZ();
 
-    /*   Serial.print(ax), Serial.print(",");
+    if (ay > 30 || ay < -30)
-    Serial.print(ay), Serial.print(",");
-    Serial.println(az);*/
-    if (ay > 4)
     {
-      dir = -1;
+      throttle_left_motor = 0;
-    }
+      throttle_right_motor = 0;
-    else if (ay < -4)
-    {
-      dir = 1;
     }
     else
     {
-      dir = 0;
+      control_on_off(ay);
     }
-    throttle_left_motor = 6 * dir;
+
-    throttle_right_motor = 6 * dir;
+    //Serial.println(ay);*/
 
     timer = millis();
   }
+  if (tick_motores > 0)
+  {
+    motores_calc();
+  }
 }
 
-void loop1(void *pvParameters)
+/*
+=============================
+Funciones Motores paso a paso
+==============================
+*/
+//Interrupcion Tick Motores paso a paso
+void IRAM_ATTR onTimer()
 {
-  for (;;)
+  portENTER_CRITICAL_ISR(&timerMux);
-  {
+  tick_motores++;
+  portEXIT_CRITICAL_ISR(&timerMux);
+}
 
+void motores_calc()
+{
+  portENTER_CRITICAL(&timerMux);
+  tick_motores--;
+  portEXIT_CRITICAL(&timerMux);
   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
@@ -111,7 +164,30 @@ void loop1(void *pvParameters)
     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);
-  }
 }
+
+void control_on_off(float angulo)
+{
+
+  float error = angulo - setPoint;
+  if (error > 4)
+  {
+    dir = -1;
+  }
+  else if (error < -4)
+  {
+    dir = 1;
+  }
+  else
+  {
+    dir = 0;
+  }
+  throttle_left_motor = max_speed * dir;
+  throttle_right_motor = max_speed * dir;
+}
+
+/*void loop1(void *pvParameters){
+  for(;;){
+  Serial.print("Core: "),Serial.println(xPortGetCoreID());   
+  }
+}*/