void Esp32ServoController::init() {
// Setup ledc servos
for (byte i = 0; i < LEDC_N_SERVOS; i++) {
byte channel = i;
byte servoPin = ledcServoPins[i];
ledcSetup(channel, 50, 16); // channel X, 50 Hz, 16-bit depth
ledcAttachPin(servoPin, channel); // GPIO servoPin on channel X
ledcWrite(channel, servoMid); // Set initial (midpoint) position
}
// Setup MCPWM servos
// Setup which MCPWM units are linked to which pins
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, mcpwmServoPins[0]);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, mcpwmServoPins[1]);
mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, mcpwmServoPins[2]);
// Configure MCPWM parameters
mcpwm_config_t pwm_config;
pwm_config.frequency = 50; //frequency = 50Hz, i.e. for every servo motor time period should be 20ms
pwm_config.cmpr_a = MCPWM_DUTY_MID; //duty cycle of PWMxA
pwm_config.cmpr_b = MCPWM_DUTY_MID; //duty cycle of PWMxb
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with same settings
// Set some initial positions
// TODO: surely this shouldn't be required?
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, MCPWM_DUTY_MID);
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, MCPWM_DUTY_MID);
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, MCPWM_DUTY_MID);
}
/**
* @brief Configure MCPWM module
*/
void mcpwm_example_servo_control(void *arg)
{
uint32_t angle, count;
//1. mcpwm gpio initialization
mcpwm_example_gpio_initialize();
//2. initial mcpwm configuration
printf("Configuring Initial Parameters of mcpwm......\n");
mcpwm_config_t pwm_config;
pwm_config.frequency = 50; //frequency = 50Hz, i.e. for every servo motor time period should be 20ms
pwm_config.cmpr_a = 0; //duty cycle of PWMxA = 0
pwm_config.cmpr_b = 0; //duty cycle of PWMxb = 0
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
while (1) {
for (count = 0; count < SERVO_MAX_DEGREE; count++) {
printf("Angle of rotation: %d\n", count);
angle = servo_per_degree_init(count);
printf("pulse width: %dus\n", angle);
mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, angle);
vTaskDelay(10); //Add delay, since it takes time for servo to rotate, generally 100ms/60degree rotation at 5V
}
}
}
void mc_interface_set_configuration(mc_configuration *configuration) {
if (m_conf.motor_type == MOTOR_TYPE_FOC
&& configuration->motor_type != MOTOR_TYPE_FOC) {
mcpwm_foc_deinit();
m_conf = *configuration;
mcpwm_init(&m_conf);
} else if (m_conf.motor_type != MOTOR_TYPE_FOC
&& configuration->motor_type == MOTOR_TYPE_FOC) {
mcpwm_deinit();
m_conf = *configuration;
mcpwm_foc_init(&m_conf);
} else {
m_conf = *configuration;
}
update_override_limits(&m_conf);
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_configuration(&m_conf);
break;
case MOTOR_TYPE_FOC:
mcpwm_foc_set_configuration(&m_conf);
break;
default:
break;
}
}
int main(void) {
halInit();
chSysInit();
hw_init_gpio();
ledpwm_init();
mcpwm_init();
comm_init();
app_init();
// Threads
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO, sample_send_thread, NULL);
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
for(;;) {
chThdSleepMilliseconds(100);
}
}
void mc_interface_init(mc_configuration *configuration) {
m_conf = *configuration;
m_fault_now = FAULT_CODE_NONE;
m_ignore_iterations = 0;
m_cycles_running = 0;
m_lock_enabled = false;
m_lock_override_once = false;
m_motor_current_sum = 0.0;
m_input_current_sum = 0.0;
m_motor_current_iterations = 0.0;
m_input_current_iterations = 0.0;
m_amp_seconds = 0.0;
m_amp_seconds_charged = 0.0;
m_watt_seconds = 0.0;
m_watt_seconds_charged = 0.0;
m_position_set = 0.0;
m_last_adc_duration_sample = 0.0;
m_sample_len = 1000;
m_sample_int = 1;
m_sample_ready = 1;
m_sample_now = 0;
m_sample_at_start = 0;
m_start_comm = 0;
// Start threads
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
// Initialize selected implementation
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_init(&m_conf);
break;
case MOTOR_TYPE_FOC:
mcpwm_foc_init(&m_conf);
break;
default:
break;
}
}
int main(void) {
halInit();
chSysInit();
ledpwm_init();
mcpwm_init();
comm_init();
servo_init();
#if USE_SERVO_INPUT
servodec_init();
#endif
// Threads
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO, sample_send_thread, NULL);
for(;;) {
chThdSleepMilliseconds(100);
}
}
/**
* @brief Configure whole MCPWM module for bldc motor control
*/
static void mcpwm_example_bldc_control(void *arg)
{
//1. mcpwm gpio initialization
mcpwm_example_gpio_initialize();
//2. initial mcpwm configuration
printf("Configuring Initial Parameters of mcpwm bldc control...\n");
mcpwm_config_t pwm_config;
pwm_config.frequency = 1000; //frequency = 1000Hz
pwm_config.cmpr_a = 50.0; //duty cycle of PWMxA = 50.0%
pwm_config.cmpr_b = 50.0; //duty cycle of PWMxb = 50.0%
pwm_config.counter_mode = MCPWM_UP_COUNTER;
pwm_config.duty_mode = MCPWM_DUTY_MODE_1;
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings
mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings
//3. Capture configuration
//configure CAP0, CAP1 and CAP2 signal to start capture counter on rising edge
//we generate a gpio_test_signal of 20ms on GPIO 12 and connect it to one of the capture signal, the disp_captured_function displays the time between rising edge
//In general practice you can connect Capture to external signal, measure time between rising edge or falling edge and take action accordingly
mcpwm_capture_enable(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, MCPWM_POS_EDGE, 0); //capture signal on rising edge, pulse num = 0 i.e. 800,000,000 counts is equal to one second
mcpwm_capture_enable(MCPWM_UNIT_0, MCPWM_SELECT_CAP1, MCPWM_POS_EDGE, 0); //capture signal on rising edge, pulse num = 0 i.e. 800,000,000 counts is equal to one second
mcpwm_capture_enable(MCPWM_UNIT_0, MCPWM_SELECT_CAP2, MCPWM_POS_EDGE, 0); //capture signal on rising edge, pulse num = 0 i.e. 800,000,000 counts is equal to one second
//enable interrupt, so each this a rising edge occurs interrupt is triggered
MCPWM[MCPWM_UNIT_0]->int_ena.val = (CAP0_INT_EN | CAP1_INT_EN | CAP2_INT_EN); //Enable interrupt on CAP0, CAP1 and CAP2 signal
mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
//According to the hall sensor input value take action on PWM0A/0B/1A/1B/2A/2B
while (1) {
hall_sensor_value = (gpio_get_level(GPIO_NUM_27) * 1) + (gpio_get_level(GPIO_NUM_26) * 2) + (gpio_get_level(GPIO_NUM_25) * 4);
if (hall_sensor_value != hall_sensor_previous) {
//printf("hall_sen val: %d\n", hall_sensor_value);
if (hall_sensor_value == 2) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM0A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM0B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10us
}
if (hall_sensor_value == 6) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B);
mcpwm_deadtime_disable(MCPWM_UNIT_0, MCPWM_TIMER_0);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM2A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM2B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10us
}
if (hall_sensor_value == 4) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM2A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM2B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10us
}
if (hall_sensor_value == 5) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B);
mcpwm_deadtime_disable(MCPWM_UNIT_0, MCPWM_TIMER_2);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM1A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM1B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10uss
}
if (hall_sensor_value == 1) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM1A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM1B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10uss
}
if (hall_sensor_value == 3) {
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_B);
mcpwm_deadtime_disable(MCPWM_UNIT_0, MCPWM_TIMER_1);
mcpwm_set_signal_low(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_A);
mcpwm_set_signal_high(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_OPR_B);
//MCPWMXA to duty mode 1 and MCPWMXB to duty mode 0 or vice versa will generate MCPWM compliment signal of each other, there are also other ways to do it
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, MCPWM_DUTY_MODE_1); //Set PWM0A to duty mode one
mcpwm_set_duty_type(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, MCPWM_DUTY_MODE_0); //Set PWM0B back to duty mode zero
mcpwm_deadtime_enable(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_BYPASS_FED, 100, 100); //Deadtime of 10us
}
hall_sensor_previous = hall_sensor_value;
}
}
}
