ESP32 Forum

Code: Select all

// Test code to identify the configuration and control of motors using MCPWM on an ESP32-WROOM-32
// This code uses the LATEST mcpwm_prelude.h driver code as the old mcpwm.h has been depreciated.
//   Definitely hurt my brain to understand this so apologies if this is not the right way.
//   The code is to drive FOUR motors via TWO DBH-12V H-Bridges
/*
   Andy Wharton - November 2024

*/
#include "driver/mcpwm_prelude.h"
#include <soc/gpio_struct.h>
#include <hal/gpio_ll.h>

#define VERSION 1.20

/*-----( Declare Constants and Pin Numbers )-----*/
#define DEBUG_SETUP          true
#define DEBUG_LOOP           true

// Motor GPIO pin assignments
#define GPIO_PWM0A_OUT 16   // Define GPIO 16 as PWM0A - Motor 0  H-Bridge 1  
#define GPIO_PWM0B_OUT 17   // Define GPIO 17 as PWM0B - Motor 1  H-Bridge 2 
#define GPIO_PWM1A_OUT 18   // Define GPIO 18 as PWM1A - Motor 2  H-Bridge 2
#define GPIO_PWM1B_OUT 19   // Define GPIO 19 as PWM1B - Motor 3  H-Bridge 1

#define ZERO 0
#define MAINDELAY 5000

// We need the folowing for the DBH-12V H-Bridge...
//   PWM frequency of 16kHz
//   Duty Cycle in steps of 100 - MAX 98% though so limit at 95% for extra safety
//   PWM outputs from each Operator are symmetric but independent
//   Use the same timer for both Operators - MCPWM_UNIT_0
#define PWM_FREQUENCY0 16384 * 100   // 1.6MHz resolution_hz value
#define PWM_DUTY_STEPS0 100          // and period_ticks of 100 gives a Frequency of 100/1.6MHz = 16kHz

//============= DEBUG stuff
//=============
extern gpio_dev_t GPIO;

bool pd,      //PULLUP
pu,           //PULLDOWN
ie,           //INPUT
oe,           //OUTPUT
od,           //OPEN_DRAIN
slp_sel;      //sleep mode magic

uint32_t
 drv,          // drive capability
fun_sel,       // iomux & gpio matrix magic 
sig_out;       // iomux & gpio matrix magic

// Define all the instances for MCPWM
// Timers, one for the main motors and one for the side motors
mcpwm_timer_handle_t timer0;

// Define an instance of the timer config structure and set values for the Main Motors
mcpwm_timer_config_t timer_config0 = {
   .group_id = 0,
   .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
   .resolution_hz = PWM_FREQUENCY0,
   .count_mode = MCPWM_TIMER_COUNT_MODE_UP,                       // Count Up
   .period_ticks = PWM_DUTY_STEPS0,
   .flags = {
       .update_period_on_empty = false,
       .update_period_on_sync = true,
   },
};

// Operators - one each for the Main motor pairs
mcpwm_oper_handle_t operator0, operator1;

// Define instances of the operator config structures.  Sets them to the same group as the timer
mcpwm_operator_config_t operator_config0 = {
	.group_id = 0,   // operator must be in the same group to the timer.  Everything else defaults
};
mcpwm_operator_config_t operator_config1 = {
	.group_id = 0,   // operator must be in the same group to the timer.  Everything else defaults
};

// Comparators for the main motors
mcpwm_cmpr_handle_t comparator1, comparator2, comparator3, comparator4;

// Create Comparator configurations - only 1 needed as they are all the same
mcpwm_comparator_config_t comparator_config1 = {
     .flags = {
        .update_cmp_on_tep = 1,  // Update compare threshold when timer counts to peak
    },
};
mcpwm_comparator_config_t comparator_config2 = {
     .flags = {
       .update_cmp_on_tep = 1,  // Update compare threshold when timer counts to peak
    },
};
mcpwm_comparator_config_t comparator_config3 = {
     .flags = {
       .update_cmp_on_tep = 1,  // Update compare threshold when timer counts to peak
    },
};
mcpwm_comparator_config_t comparator_config4 = {
     .flags = {
       .update_cmp_on_tep = 1,  // Update compare threshold when timer counts to peak
    },
};

// FOUR generators for the main motors
mcpwm_gen_handle_t generator1, generator2, generator3, generator4;

// Generators - One generator per PWM
// define instances of their configurations - only the GPIO assignments change
mcpwm_generator_config_t generator_config1 = {
	.gen_gpio_num = GPIO_PWM0A_OUT,
};

mcpwm_generator_config_t generator_config2 = {
	.gen_gpio_num = GPIO_PWM0B_OUT,
};

mcpwm_generator_config_t generator_config3 = {
	.gen_gpio_num = GPIO_PWM1A_OUT,
};

mcpwm_generator_config_t generator_config4 = {
	.gen_gpio_num = GPIO_PWM1B_OUT,
};

/*-----( Declare User-written Functions )-----*/
//<<<<<<<<<<<<<<<<<<<<>>>>>>>>>>>>>>>>>>>

void setup_Main_Motors()
{
  if (DEBUG_SETUP) {
    Serial.print(F("Create Timers.")); 
    Serial.println("     ");
  }  

  // MCPWM Initialise the timer
  ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config0, &timer0));  // Main Motors

  if (DEBUG_SETUP) {
    Serial.print(F("Create Operators.")); 
    Serial.println("     ");
  } 
  // Initialise the Operators
  ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config0, &operator0)); // Main Motors Same config
  ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config1, &operator1)); // Main motors Same config
  
  if (DEBUG_SETUP) {
    Serial.print(F("Connect Timers and Operators.")); 
    Serial.println("     ");
  } 

  // Connect the Operators to the single Timer
  ESP_ERROR_CHECK(mcpwm_operator_connect_timer(operator0, timer0));  // Main  Motors
  ESP_ERROR_CHECK(mcpwm_operator_connect_timer(operator1, timer0));  // Main Motors

  if (DEBUG_SETUP) {
    Serial.print(F("Create Comparators.")); 
    Serial.println("     ");
  } 

  // Create the comparators which define the duty cycle.
  // Set the duty cycle using mcpwm_comparator_set_compare_value()
  // One comparator per Operator
  // Main Motors
  ESP_ERROR_CHECK(mcpwm_new_comparator(operator0, &comparator_config1, &comparator1));
  ESP_ERROR_CHECK(mcpwm_new_comparator(operator0, &comparator_config2, &comparator2));
  ESP_ERROR_CHECK(mcpwm_new_comparator(operator1, &comparator_config3, &comparator3));
  ESP_ERROR_CHECK(mcpwm_new_comparator(operator1, &comparator_config4, &comparator4));

  if (DEBUG_SETUP) {
    Serial.print(F("Create Generators.")); 
    Serial.println("     ");
  } 

  // Create the generators 
  // Main Motors
  ESP_ERROR_CHECK(mcpwm_new_generator(operator0, &generator_config1, &generator1));
  ESP_ERROR_CHECK(mcpwm_new_generator(operator0, &generator_config2, &generator2));
  ESP_ERROR_CHECK(mcpwm_new_generator(operator1, &generator_config3, &generator3));
  ESP_ERROR_CHECK(mcpwm_new_generator(operator1, &generator_config4, &generator4));

  if (DEBUG_SETUP) {
    Serial.print(F("Set the generator event actions.")); 
    Serial.println("     ");
  } 

  // Now set the generators to carry out the correct actions based on their direction and the timer event
  // Each generator will get an event when it's operator/comparator hits the duty_cycle
  // >> Main Motors <<
  // Generator 1
  // First go high when the counter is empty
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator1, 
         MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));

  // Then go LOW when the counter hits the duty cycle threshold.
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator1, 
         MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator1, MCPWM_GEN_ACTION_LOW)));

  // Generator 2
  // First go high when the counter is empty
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator2, 
         MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));

  // Then go LOW when the counter hits the duty cycle threshold.
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator2, 
         MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator2, MCPWM_GEN_ACTION_LOW)));

  // Generator 3
  // First go high when the counter is empty
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator3, 
         MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));

  // Then go LOW when the counter hits the duty cycle threshold.
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator3, 
         MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator3, MCPWM_GEN_ACTION_LOW)));

  // Generator 4
  // First go high when the counter is empty
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(generator4, 
         MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));

  // Then go LOW when the counter hits the duty cycle threshold.
  ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(generator4, 
         MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator4, MCPWM_GEN_ACTION_LOW)));

  if (DEBUG_SETUP) {
    Serial.print(F("Enable and Start Timer.")); 
    Serial.println("     ");
  } 

  // Enable the timers and start them
  // >> Main Motors <<
  ESP_ERROR_CHECK(mcpwm_timer_enable(timer0));
  ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer0, MCPWM_TIMER_START_NO_STOP));

  if (DEBUG_SETUP) {
    Serial.print(F("Set the duty cycle to 0 (Stop).")); 
    Serial.println("     ");
  } 

  // Set the initial duty cycle to ZERO for all motors
  // >> Main Motors <<
  ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator1, ZERO));
  ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator2, ZERO));
  ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator3, ZERO));
  ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator4, ZERO));
}

void setup()
{
    // Start the built-in serial port, probably to Serial Monitor
  if (DEBUG_SETUP) {
    Serial.begin(9600);
    delay(200);
    Serial.print(F("ESP32 MCPWM Debug Output - V"));
    Serial.println(VERSION);
  }

  // Now set up PWM on the 4 main motors
  setup_Main_Motors();

}

void gpio_Print(int GPIO_PIN)
{    //=======
  gpio_ll_get_io_config(&GPIO, GPIO_PIN ,&pu, &pd, &ie, &oe, &od,&drv, &fun_sel, &sig_out, &slp_sel);
  
  Serial.print("GPIO ");
  Serial.print(GPIO_PIN);
  Serial.println(" Settings:  PU, PD, IN, OUT, OD, SMM, drv, FS, SO ");
  Serial.print("                   ");
  Serial.print(pu);
  Serial.print(", ");
  Serial.print(pd);
  Serial.print(", ");
  Serial.print(ie);
  Serial.print(", ");
  Serial.print(oe);
  Serial.print(", ");
  Serial.print(od);
  Serial.print(", ");
  Serial.print(slp_sel);
  Serial.print(", ");
  Serial.print(drv);
  Serial.print(", ");
  Serial.print(fun_sel);
  Serial.print(", ");
  Serial.print(sig_out);
  Serial.println(" ");
}

void cycle_Motor_UP(int MotorID, int Cycle)
{
  // Loop and ramp the motors up then down

  if (DEBUG_LOOP) {
    Serial.print(F("Cycle UP - Motor  Steps"));
    Serial.println(F("             "));
    Serial.print(MotorID); 
    Serial.print("    ");
    Serial.print(Cycle);
    Serial.println("     ");
  }

  // Motor Ramp UP
  for (int duty_Cycle = 0; duty_Cycle <= Cycle; duty_Cycle++ ) 
  {
    if (DEBUG_LOOP) {
      Serial.print(F("Motor - ")); 
      Serial.print(MotorID);
      Serial.print(F("   Duty Cycle = ")); 
      Serial.print(duty_Cycle);
      Serial.println("     ");
    }

    // Set the initial duty cycle to the required duty cycle
    switch(MotorID) {
      case 0:
        gpio_Print(GPIO_PWM0A_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator1, duty_Cycle));
        break;
      case 1:
        gpio_Print(GPIO_PWM0B_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator2, duty_Cycle));
        break;
      case 2:
        gpio_Print(GPIO_PWM1A_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator3, duty_Cycle));
        break;
      case 3:
        gpio_Print(GPIO_PWM1B_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator4, duty_Cycle));
        break;
      default:
        break; // Do nothing
    }

    delay(200);  // Delay 200ms
  }

}


void cycle_Motor_DOWN(int MotorID, int Cycle)
{
  // Loop and ramp the motors up then down

  if (DEBUG_LOOP) {
    Serial.println(F("Cycle DOWN - Motor   Steps"));
    Serial.print(F("             "));
    Serial.print(MotorID); 
    Serial.print("    ");
    Serial.print(Cycle);
    Serial.println("     ");
  }

  // Motor Ramp DOWN
  for (int duty_Cycle = Cycle; duty_Cycle >= 0; duty_Cycle-- ) 
  {
    if (DEBUG_LOOP) {
      Serial.print(F("Motor - ")); 
      Serial.print(MotorID);
      Serial.print(F("   Duty Cycle = ")); 
      Serial.print(duty_Cycle);
      Serial.println("     ");
    }

    // Set the initial duty cycle to the required duty cycle
    switch(MotorID) {
      case 0:
        gpio_Print(GPIO_PWM0A_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator1, duty_Cycle));
        break;
      case 1:
        gpio_Print(GPIO_PWM0B_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator2, duty_Cycle));
        break;
      case 2:
        gpio_Print(GPIO_PWM1A_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator3, duty_Cycle));
        break;
      case 3:
        gpio_Print(GPIO_PWM1B_OUT);
        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator4, duty_Cycle));
        break;
      default:
        break; // Do nothing
    }
    delay(100);  // Delay 100ms
  }

}

void loop()
{
  delay(5000);  // Delay 5s
 
  // Call Loops to ramp the motors up then down one at a time
  // Max Duty Cycle of 95% of the available steps

  if (DEBUG_LOOP) {
    Serial.print(F("Motor 0, GP16")); 
    Serial.println("     ");
  }
  cycle_Motor_UP(0, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 5s 

  cycle_Motor_DOWN(0, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 10s  - allows me to move my scope probes

  if (DEBUG_LOOP) {
    Serial.print(F("Motor 1, GP17")); 
    Serial.println("     ");
  }
  cycle_Motor_UP(1, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 5s

  cycle_Motor_DOWN(1, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 10s  - allows me to move my scope probes

  if (DEBUG_LOOP) {
    Serial.print(F("Motor 2, GP18")); 
    Serial.println("     ");
  }
  cycle_Motor_UP(2, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 5s

  cycle_Motor_DOWN(2, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 10s - allows me to move my scope probes

  if (DEBUG_LOOP) {
    Serial.print(F("Motor 3, GP19")); 
    Serial.println("     ");
  }
  cycle_Motor_UP(3, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 5s

  cycle_Motor_DOWN(3, PWM_DUTY_STEPS0 * 0.95);

  delay(MAINDELAY);  // Delay 10s - allows me to move my scope probes
}