WhyCan Forum(哇酷开发者社区)

servo_set(GPIOA_BASE_ADDR,GPIO_PIN0,0);

#ifndef MC3172_SERVO_H_
#define MC3172_SERVO_H_

#include "./MC3172.h"
#include "./thread_config.h"

#if ROTHD_COLCK_SOURCE_SEL==0
#define SYS_CORE_CLK_MHZ 48   // X3
#endif
#if ROTHD_COLCK_SOURCE_SEL==1
#define SYS_CORE_CLK_MHZ 200  // X5
#endif
#if ROTHD_COLCK_SOURCE_SEL==2
#define SYS_CORE_CLK_MHZ 200  // RC
#endif
#if ROTHD_COLCK_SOURCE_SEL==3
#define SYS_CORE_CLK_MHZ 8  // RC with slow speed
#endif

void servo_delay_us(u32 nus);

u16 servo_map(u8 Angle,u8 MinAngle,u8 MaxAngle,u16 MinTime,u16 MaxTime);

void servo_set(u32 GPIOGroupName,u32 GPIONum,u16 Angle);

#endif /* MC3172_SERVO_H_ */

#include "MC3172.h"
#include "servo.h"

void servo_delay_us(u32 nus)
{
    u32 start,stop,ticks;
    start = CORE_CNT;
    ticks = nus * SYS_CORE_CLK_MHZ/4;
    if(0xFFFFFFFF-start>=ticks){
        stop=start+ticks;
        while(CORE_CNT<stop && CORE_CNT>start){}
    }
    else{
        stop=ticks-(0xFFFFFFFF-start);
        while(CORE_CNT>start){}
        while(CORE_CNT<stop){}
    }
}

u16 servo_map(u8 Angle,u8 MinAngle,u8 MaxAngle,u16 MinTime,u16 MaxTime)
{
    return (Angle - MinAngle)*(MaxTime-MinTime)/(MaxAngle-MinAngle) + MinTime;
}

void servo_set(u32 GPIOGroupName,u32 GPIONum,u16 Angle)
{
    // 配置IO组
//    INTDEV_SET_CLK_RST(GPIOA_BASE_ADDR,(INTDEV_RUN|INTDEV_IS_GROUP0|INTDEV_CLK_IS_CORECLK_DIV2));
    INTDEV_SET_CLK_RST(GPIOGroupName,(INTDEV_RUN|INTDEV_IS_GROUP0|INTDEV_CLK_IS_CORECLK_DIV2));

    // 设置具体IO
    GPIO_SET_OUTPUT_EN_VALUE(GPIOGroupName,(GPIONum),GPIO_SET_ENABLE);

    // 50Hz 每个周期20ms
    u8 i = 50;

    while(i--)
    {
        // 设置IO 高电平
        GPIO_SET_OUTPUT_PIN_TO_1(GPIOGroupName,(GPIONum));

        u32 NeedDelayUS = servo_map(Angle, 0, 180, 500, 2500);

        // 延迟
        servo_delay_us(NeedDelayUS);

        // 设置IO 低电平
        GPIO_SET_OUTPUT_PIN_TO_0(GPIOGroupName,(GPIONum));

        // 延迟
        servo_delay_us(20*1000 - NeedDelayUS);
    }
}