From 534739ac2f4b0a4120eb1680bb0e711b52c8cc83 Mon Sep 17 00:00:00 2001 From: SEEK-THE-VOID-LIGHT Date: Mon, 12 Jul 2021 17:03:24 +0200 Subject: [PATCH 1/3] added servo example --- pwm/servo/CMakeLists.txt | 5 ++++ pwm/servo/main.c | 15 +++++++++++ pwm/servo/servo.c | 56 ++++++++++++++++++++++++++++++++++++++++ pwm/servo/servo.h | 24 +++++++++++++++++ 4 files changed, 100 insertions(+) create mode 100755 pwm/servo/CMakeLists.txt create mode 100755 pwm/servo/main.c create mode 100755 pwm/servo/servo.c create mode 100755 pwm/servo/servo.h diff --git a/pwm/servo/CMakeLists.txt b/pwm/servo/CMakeLists.txt new file mode 100755 index 000000000..d425002a8 --- /dev/null +++ b/pwm/servo/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(servo main.c servo.c) + +target_link_libraries(servo pico_stdlib hardware_pwm pico_bootrom hardware_clocks) + +pico_add_extra_outputs(servo) \ No newline at end of file diff --git a/pwm/servo/main.c b/pwm/servo/main.c new file mode 100755 index 000000000..913e06e4c --- /dev/null +++ b/pwm/servo/main.c @@ -0,0 +1,15 @@ +#include "pico/bootrom.h" +#include "pico/stdlib.h" +#include "servo.h" +#include + +#define ANGLE 90 + + +int main() +{ + servo_init(1, 50); + servo_put(1, ANGLE, true); + + reset_usb_boot(0,0); // reset to BOOTSEL for another flash +} \ No newline at end of file diff --git a/pwm/servo/servo.c b/pwm/servo/servo.c new file mode 100755 index 000000000..63c455c89 --- /dev/null +++ b/pwm/servo/servo.c @@ -0,0 +1,56 @@ +#include "servo.h" +#include "pico/stdlib.h" +#include "hardware/pwm.h" +#include "hardware/clocks.h" +#include + +#define DUTY_MIN 2400 +#define DUTY_MAX 8000 + +long map(long x, long in_min, long in_max, long out_min, long out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +void servo_put(int gpio, int angle, bool wait_write) +{ + uint slice = pwm_gpio_to_slice_num(gpio); + uint channel = pwm_gpio_to_channel(gpio); + uint32_t top = pwm_hw->slice[slice].top; + int duty_u16 = map(angle, 0, 180, DUTY_MIN, DUTY_MAX); + uint32_t cc = duty_u16 * (top + 1) / 65535; + pwm_set_chan_level(slice, channel, cc); + pwm_set_enabled(slice, true); + + if(wait_write) + sleep_ms(1000); +} + +void servo_init(int gpio, int base_frequency) +{ + gpio_set_function(gpio, GPIO_FUNC_PWM); + uint slice = pwm_gpio_to_slice_num(gpio); + uint channel = pwm_gpio_to_channel(gpio); + + + uint32_t source_hz = clock_get_hz(clk_sys); + #define TOP_MAX 65534 + uint freq = 50; + uint32_t div16_top = 16 * source_hz / freq; + uint32_t top = 1; + for (;;) { + if (div16_top >= 16 * 5 && div16_top % 5 == 0 && top * 5 <= TOP_MAX) { + div16_top /= 5; + top *= 5; + } else if (div16_top >= 16 * 3 && div16_top % 3 == 0 && top * 3 <= TOP_MAX) { + div16_top /= 3; + top *= 3; + } else if (div16_top >= 16 * 2 && top * 2 <= TOP_MAX) { + div16_top /= 2; + top *= 2; + } else { + break; + } + } + pwm_hw->slice[slice].div = div16_top; + pwm_hw->slice[slice].top = top; +} \ No newline at end of file diff --git a/pwm/servo/servo.h b/pwm/servo/servo.h new file mode 100755 index 000000000..85cb83ad8 --- /dev/null +++ b/pwm/servo/servo.h @@ -0,0 +1,24 @@ +#include +/** + * @brief initialize PWM pins and frequency clock + * + * @param gpio PWM Pin + * @param base_frequency set Servo Frequency - 50Hz is common + */ +void servo_init(int gpio, int base_frequency); + + +/** + * @brief Map a range of values to another range + * @return long + */ +long map(long x, long in_min, long in_max, long out_min, long out_max); + +/** + * @brief Write an angle to the Servo + * + * @param gpio PWM pin attached to the servo + * @param angle Angle from 1-180 + * @param wait_write Wait for write to finish + */ +void servo_put(int gpio, int angle, bool wait_write); \ No newline at end of file From b9bcdeba3b4694d6ba505529564a32d6500ed44d Mon Sep 17 00:00:00 2001 From: SEEK-THE-VOID-LIGHT Date: Mon, 12 Jul 2021 18:07:02 +0200 Subject: [PATCH 2/3] changes according to PR review --- pwm/servo/CMakeLists.txt | 2 +- pwm/servo/main.c | 18 +++++++++++------- pwm/servo/servo.c | 2 +- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/pwm/servo/CMakeLists.txt b/pwm/servo/CMakeLists.txt index d425002a8..a8bb7951c 100755 --- a/pwm/servo/CMakeLists.txt +++ b/pwm/servo/CMakeLists.txt @@ -1,5 +1,5 @@ add_executable(servo main.c servo.c) -target_link_libraries(servo pico_stdlib hardware_pwm pico_bootrom hardware_clocks) +target_link_libraries(servo pico_stdlib hardware_pwm hardware_clocks) pico_add_extra_outputs(servo) \ No newline at end of file diff --git a/pwm/servo/main.c b/pwm/servo/main.c index 913e06e4c..0b7c2d07a 100755 --- a/pwm/servo/main.c +++ b/pwm/servo/main.c @@ -1,15 +1,19 @@ -#include "pico/bootrom.h" #include "pico/stdlib.h" #include "servo.h" #include -#define ANGLE 90 - - int main() { - servo_init(1, 50); - servo_put(1, ANGLE, true); + for(;;) + { + servo_init(1, 50); + servo_put(1, 0, true); + + sleep_ms(1000); + + servo_init(1, 50); + servo_put(1, 90, true); - reset_usb_boot(0,0); // reset to BOOTSEL for another flash + sleep_ms(1000); + } } \ No newline at end of file diff --git a/pwm/servo/servo.c b/pwm/servo/servo.c index 63c455c89..2b1b516a0 100755 --- a/pwm/servo/servo.c +++ b/pwm/servo/servo.c @@ -6,6 +6,7 @@ #define DUTY_MIN 2400 #define DUTY_MAX 8000 +#define TOP_MAX 65534 long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; @@ -33,7 +34,6 @@ void servo_init(int gpio, int base_frequency) uint32_t source_hz = clock_get_hz(clk_sys); - #define TOP_MAX 65534 uint freq = 50; uint32_t div16_top = 16 * source_hz / freq; uint32_t top = 1; From 0a76daad2852f94992cda685055de62385050b54 Mon Sep 17 00:00:00 2001 From: SEEK-THE-VOID-LIGHT Date: Mon, 12 Jul 2021 18:51:25 +0200 Subject: [PATCH 3/3] typo --- pwm/servo/main.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/pwm/servo/main.c b/pwm/servo/main.c index 0b7c2d07a..f2bd7441d 100755 --- a/pwm/servo/main.c +++ b/pwm/servo/main.c @@ -4,16 +4,13 @@ int main() { + servo_init(1, 50); for(;;) { - servo_init(1, 50); servo_put(1, 0, true); - sleep_ms(1000); - servo_init(1, 50); servo_put(1, 90, true); - sleep_ms(1000); } } \ No newline at end of file