84 lines
2.4 KiB
C
84 lines
2.4 KiB
C
|
// #include <rtthread.h>
|
||
|
|
// #include <rtdevice.h>
|
||
|
|
// #include <board.h>
|
||
|
|
//
|
||
|
|
// #define PWM_DEV_NAME "pwm1"
|
||
|
|
// #define PWM_DEV_CHANNEL 1
|
||
|
|
//
|
||
|
|
// /* 常见舵机刷新周期 20ms (50Hz) */
|
||
|
|
// #define SERVO_PERIOD_NS 20000000UL
|
||
|
|
//
|
||
|
|
// /* 你的规格：脉宽限幅 */
|
||
|
|
// #define SERVO_MIN_US 800
|
||
|
|
// #define SERVO_MAX_US 2200
|
||
|
|
//
|
||
|
|
// /* 你的标定点：-50/0/+50 -> 1000/1500/2000 */
|
||
|
|
// #define SERVO_CENTER_US 1500
|
||
|
|
// #define SERVO_US_PER_DEG 10 /* 1° 对应 10us */
|
||
|
|
//
|
||
|
|
// static struct rt_device_pwm *pwm_dev = RT_NULL;
|
||
|
|
//
|
||
|
|
// static int clamp_int(int v, int lo, int hi)
|
||
|
|
// {
|
||
|
|
// if (v < lo) return lo;
|
||
|
|
// if (v > hi) return hi;
|
||
|
|
// return v;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /* 角度(°) -> 脉宽(us)，按你的三点标定线性换算，并做 800~2200us 限幅 */
|
||
|
|
// static int angle_to_pulse_us(int angle_deg)
|
||
|
|
// {
|
||
|
|
// int pulse = SERVO_CENTER_US + angle_deg * SERVO_US_PER_DEG;
|
||
|
|
// return clamp_int(pulse, SERVO_MIN_US, SERVO_MAX_US);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /* 设置角度（单位：度），例如 -50 / 0 / +50 */
|
||
|
|
// static void servo_set_angle(int angle_deg)
|
||
|
|
// {
|
||
|
|
// int pulse_us = angle_to_pulse_us(angle_deg);
|
||
|
|
// rt_uint32_t pulse_ns = (rt_uint32_t)pulse_us * 1000UL; /* us -> ns */
|
||
|
|
// rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL, SERVO_PERIOD_NS, pulse_ns);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// static void servo_thread_entry(void *parameter)
|
||
|
|
// {
|
||
|
|
// while (1)
|
||
|
|
// {
|
||
|
|
// servo_set_angle(50);
|
||
|
|
// rt_thread_mdelay(1500);
|
||
|
|
// servo_set_angle(-50);
|
||
|
|
// rt_thread_mdelay(1500);
|
||
|
|
//
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// static int servo_app_init(void)
|
||
|
|
// {
|
||
|
|
// pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
|
||
|
|
// if (pwm_dev == RT_NULL)
|
||
|
|
// {
|
||
|
|
// rt_kprintf("can't find %s\n", PWM_DEV_NAME);
|
||
|
|
// return -RT_ERROR;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// /* 中位 0° -> 1500us */
|
||
|
|
// rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL, SERVO_PERIOD_NS, (rt_uint32_t)SERVO_CENTER_US * 1000UL);
|
||
|
|
// rt_pwm_enable(pwm_dev, PWM_DEV_CHANNEL);
|
||
|
|
//
|
||
|
|
// rt_thread_t tid = rt_thread_create("servo",
|
||
|
|
// servo_thread_entry,
|
||
|
|
// RT_NULL,
|
||
|
|
// 1024,
|
||
|
|
// 20,
|
||
|
|
// 10);
|
||
|
|
// if (tid == RT_NULL)
|
||
|
|
// {
|
||
|
|
// rt_kprintf("create servo thread failed\n");
|
||
|
|
// return -RT_ERROR;
|
||
|
|
// }
|
||
|
|
// rt_thread_startup(tid);
|
||
|
|
//
|
||
|
|
// return RT_EOK;
|
||
|
|
// }
|
||
|
|
// INIT_APP_EXPORT(servo_app_init);
|