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);
}
