66 lines
1.5 KiB
C
66 lines
1.5 KiB
C
#include <hardware/structs/clocks.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include "pico/stdlib.h"
|
|
#include "hardware/pwm.h"
|
|
#include "hardware/clocks.h"
|
|
|
|
#define ROTATE_180 2500
|
|
#define ROTATE_0 500
|
|
|
|
|
|
void setup_servo(uint pin) {
|
|
// Initialize the GPIO pin for PWM output
|
|
gpio_set_function(pin, GPIO_FUNC_PWM);
|
|
|
|
// Get the PWM slice for the given pin
|
|
uint slice_num = pwm_gpio_to_slice_num(pin);
|
|
|
|
// Set the frequency and wrap (period)
|
|
uint32_t clk = clock_get_hz(clk_sys);
|
|
uint32_t div = clk / 20000/ 50;
|
|
pwm_set_clkdiv_int_frac4(slice_num,div,1); // Set the clock divider
|
|
pwm_set_wrap(slice_num, 20000);
|
|
|
|
// Enable the PWM
|
|
pwm_set_enabled(slice_num, true);
|
|
}
|
|
|
|
void set_degree(uint pin, float deg) {
|
|
|
|
int duty = (((float)(ROTATE_180 - ROTATE_0) / 180.0) * deg) + ROTATE_0;
|
|
pwm_set_gpio_level(pin, duty);
|
|
}
|
|
|
|
int main()
|
|
{
|
|
stdio_init_all();
|
|
|
|
setup_servo(2);
|
|
setup_servo(3);
|
|
setup_servo(4);
|
|
setup_servo(5);
|
|
setup_servo(6);
|
|
setup_servo(7);
|
|
setup_servo(8);
|
|
setup_servo(9);
|
|
setup_servo(10);
|
|
setup_servo(11);
|
|
setup_servo(12);
|
|
while (true) {
|
|
printf("Hello, world!\n");
|
|
sleep_ms(100);
|
|
set_degree(2, 0);
|
|
set_degree(3, 20);
|
|
set_degree(4, 30);
|
|
set_degree(5, 40);
|
|
set_degree(6, 50);
|
|
set_degree(7, 60);
|
|
set_degree(8, 70);
|
|
set_degree(9, 80);
|
|
set_degree(10, 90);
|
|
set_degree(11, 100);
|
|
set_degree(12, 180);
|
|
}
|
|
}
|