void Servo::writeMicroseconds(int value)
{
uint8_t channel, instance;
uint8_t pin = servos[this->servoIndex].Pin.nbr;
//instance of pwm module is MSB - look at VWariant.h
instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16;
//index of pwm channel is LSB - look at VWariant.h
channel=g_APinDescription[pin].ulPWMChannel & 0x0F;
group_pins[instance][channel]=g_APinDescription[pin].ulPin;
NRF_PWM_Type * PWMInstance = instance == 0 ? NRF_PWM0 : (instance == 1 ? NRF_PWM1 : NRF_PWM2);
//configure pwm instance and enable it
seq_values[instance][channel]= value | 0x8000;
nrf_pwm_sequence_t const seq={
seq_values[instance],
NRF_PWM_VALUES_LENGTH(seq_values),
0,
0
};
nrf_pwm_pins_set(PWMInstance, group_pins[instance]);
nrf_pwm_enable(PWMInstance);
nrf_pwm_configure(PWMInstance, NRF_PWM_CLK_125kHz, NRF_PWM_MODE_UP, 2500); // 20ms - 50Hz
nrf_pwm_decoder_set(PWMInstance, NRF_PWM_LOAD_INDIVIDUAL, NRF_PWM_STEP_AUTO);
nrf_pwm_sequence_set(PWMInstance, 0, &seq);
nrf_pwm_loop_set(PWMInstance, 0UL);
nrf_pwm_task_trigger(PWMInstance, NRF_PWM_TASK_SEQSTART0);
}
static void configure_pins(nrf_drv_pwm_t const * const p_instance,
nrf_drv_pwm_config_t const * p_config)
{
uint32_t out_pins[NRF_PWM_CHANNEL_COUNT];
uint8_t i;
for (i = 0; i < NRF_PWM_CHANNEL_COUNT; ++i)
{
uint8_t output_pin = p_config->output_pins[i];
if (output_pin != NRF_DRV_PWM_PIN_NOT_USED)
{
bool inverted = output_pin & NRF_DRV_PWM_PIN_INVERTED;
out_pins[i] = output_pin & ~NRF_DRV_PWM_PIN_INVERTED;
if (inverted)
{
nrf_gpio_pin_set(out_pins[i]);
}
else
{
nrf_gpio_pin_clear(out_pins[i]);
}
nrf_gpio_cfg_output(out_pins[i]);
}
else
{
out_pins[i] = NRF_PWM_PIN_NOT_CONNECTED;
}
}
nrf_pwm_pins_set(p_instance->p_registers, out_pins);
}
