PSOC 5LP를 이용한 RC car의 Servo Motor Control 예제

우선 RC car용 Servo모터 입니다. Traxxas의 2080으로 자세한 Spec이 없습니다.;;;

그래서 googling 해보니 나름 Standard가 있더군요.

우선 cable은 아래와 같이 Red Positive에 5V 공급하고, Negative에 Ground 연결하고, PSOC PWM output을 Signal Line에 연결해 주었습니다.

PWM Component를 이용하여 Servo Motor에 PWM signal을 전달 할 예정이며, Control은 아래처럼 하면 됩니다.

PWM Period는 20 msec를 유지하고, Pulse Duty Cycle을 이용하여, Servo Motor의 각도를 조정 하게 됩니다.

PWM Clock은 480KHz를 연결 해 줍니다.

PWM Configuration 입니다. PWM Mode를 Center Aligh으로 설정합니다.
Period를 20 msec를 설정 하기 위해 아래와 같이 계산 합니다.
20 msec / ( 1 / 480 KHz * 2)
(2: Center Align 설정)

위 식에 의해 20msec은 Period는 4800으로 설정 됩니다.
다음 Duty Cycle은 CMP Type이 Greater로 설정 되어 있기에, 설정 원하는(ex, 1.5 msec) Duty Cycle을 Count값로 계산한 후 Period count에 뺀 값을 입력 합니다.
1.5 msec / ( 1 / 480 KHz * 2)

Duty Cycle은 240 count값이고 4800 – 240 = 4560을 입력 하면 됩니다.

위의 Control 하는 move_surve() function 입니다.

void move_survo(float duration, float pulsewidth)
{
    double clock = 480.0e3;

    double periodCount = duration / (1.0 / clock * 2.0);
    double pulsewidthCount = pulsewidth / (1.0 / clock * 2.0);

    uint16 period = periodCount;
    uint16 compare = periodCount - pulsewidthCount;

    char buffer[USBUART_BUFFER_SIZE];
    PWM_SERVO_WritePeriod(period);
    PWM_SERVO_WriteCompare(compare);
    sprintf(buffer, "duration:%f, pulsewidth: %f, period: %d, compare: %d\r\n", duration, pulsewidth, period, compare);
    DebugLog(buffer);
}

예제로 1.4 msec ~ 2.0 msec을 반복 하여 움직이도록 control 해 보았습니다.

double startPeriod = 1.4e-3;
double endPeriod = 2.0e-3;
for(;;)
{
    for(double i=startPeriod; i<endPeriod; i+=0.1e-3) 
    { 
        move_survo(20.0e-3, i); 
        CyDelay(50); 
    }
 
    for(double i=endPeriod; i>=startPeriod; i-=0.1e-3)
    {
        move_survo(20.0e-3, i);
        CyDelay(50);
    }
}

실제 작동 화면 입니다.