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 Esp32ServoController::setServo(uint8_t num, int pos){ if (num < LEDC_N_SERVOS) { ledcWrite(num, map(pos, 0, 180, servoMin, servoMax)); } else { switch (num) { case 15: mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, map(pos, 0, 180, MCPWM_DUTY_MIN, MCPWM_DUTY_MAX)); break; case 16: mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_B, map(pos, 0, 180, MCPWM_DUTY_MIN, MCPWM_DUTY_MAX)); break; case 17: mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_OPR_A, map(pos, 0, 180, MCPWM_DUTY_MIN, MCPWM_DUTY_MAX)); break; } } }
/** * @brief directly set servo motor to a particular angle */ static void servo_set_angle(uint32_t angle_of_rotation) { uint32_t angle_t; angle_t = servo_per_degree_init(angle_of_rotation); mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, angle_t); }