From 82ebb956784c69dbbfa2906753ab3040ad22886a Mon Sep 17 00:00:00 2001 From: martin chaparro Date: Sat, 29 Aug 2020 18:41:28 -0500 Subject: [PATCH] control on off --- src/main.cpp | 218 ++++++++++++++++++++++++++++++++++----------------- 1 file changed, 147 insertions(+), 71 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index d158d9b..11f0ba4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,24 +1,45 @@ #include #include #include -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; - +//TaskHandle_t TareaProcesador1; +//void loop1(void *pvParameters); +/* +========================================= +definiciones de los motores paso a paso +========================================= +*/ #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,92 +47,147 @@ 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() { - mpu6050.update(); - if (millis() - timer > 10) + + if (millis() - timer > 4) { - - // ax = mpu6050.getAngleX(); + mpu6050.update(); ay = mpu6050.getAngleY(); - //az = mpu6050.getAngleZ(); - /* Serial.print(ax), Serial.print(","); - Serial.print(ay), Serial.print(","); - Serial.println(az);*/ - if (ay > 4) + if (ay > 30 || ay < -30) { - dir = -1; - } - else if (ay < -4) - { - dir = 1; + throttle_left_motor = 0; + throttle_right_motor = 0; } 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); +} - 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 +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 + 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 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); + else + digitalWrite(DIR_1, FWD); //Set output 3 high for a forward direction of the stepper motor } -} \ No newline at end of file + 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); +} + +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()); + } +}*/ \ No newline at end of file