pimoroni-pico/examples/breakout_ioexpander/servo/servo.cpp

61 wiersze
1.6 KiB
C++

#include "pico/stdlib.h"
#include <stdio.h>
#include <math.h>
#include "breakout_ioexpander.hpp"
using namespace pimoroni;
static const uint8_t IOE_SERVO_PIN = 1;
// Settings to produce a 50Hz output from the 24MHz clock.
// 24,000,000 Hz / 8 = 3,000,000 Hz
// 3,000,000 Hz / 60,000 Period = 50 Hz
static const uint8_t DIVIDER = 8;
static const uint16_t PERIOD = 60000;
static constexpr float CYCLE_TIME = 5.0f;
static constexpr float SERVO_RANGE = 1000.0f; // Between 1000 and 2000us (1-2ms)
BreakoutIOExpander ioe(0x18);
bool toggle = false;
int main() {
gpio_init(PICO_DEFAULT_LED_PIN);
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
stdio_init_all();
if(ioe.init()) {
printf("IOExpander found...\n");
ioe.set_pwm_period(PERIOD);
ioe.set_pwm_control(DIVIDER);
ioe.set_mode(IOE_SERVO_PIN, IOExpander::PIN_PWM);
while(true) {
gpio_put(PICO_DEFAULT_LED_PIN, toggle);
toggle = !toggle;
absolute_time_t at = get_absolute_time();
float t = to_us_since_boot(at) / 1000000.0f;
float s = sinf((t * M_PI * 2.0f) / CYCLE_TIME) / 2.0f;
float servo_us = 1500.0f + (s * SERVO_RANGE);
float duty_per_microsecond = (float)PERIOD / (float)(20 * 1000); // Default is 3 LSB per microsecond
uint16_t duty_cycle = (uint16_t)(roundf(servo_us * duty_per_microsecond));
printf("Cycle Time: %.2f, Pulse: %.1fus, Duty Cycle: %d\n", fmodf(t, CYCLE_TIME), servo_us, duty_cycle);
ioe.output(IOE_SERVO_PIN, duty_cycle);
sleep_ms(20);
}
}
else {
printf("IOExpander not found :'(\n");
gpio_put(PICO_DEFAULT_LED_PIN, true);
}
return 0;
}