cancel
Showing results for 
Search instead for 
Did you mean: 

Code Examples

MotooTanaka
Esteemed Contributor

平成最後のゴールデンウィークということで、

いつものようにチマチマとプログラムを書いていたら

別のスレでサーボの動かし方について質問をいただきました。

良い機会と TSoC で秋月で買った SG-90 を動かしてみました。

猫・右向き (compare = 500)

IMG_3579.JPG

猫・正面 (compare = 1500)

IMG_3578.JPG

猫・左向き (compare = 2400)

IMG_3577.JPG

サーボのページ

マイクロサーボ9g SG−90: パーツ一般 秋月電子通商-電子部品・ネット通販

猫は秋月から秋葉原駅に移動する途中にあったガチャポンでゲットしましたが、

季節物なので、その時に入手できるものを使用してください。

使用では 20ms 周期の PWM で、デューティーが 0.5ms から 2.4ms ということですので、

1MHz のクロックで動かす PWM で、period を 20000 (19999) とすると

compare が 500 ~ 2400 で良さそうとして、だいたい 1500 位で真ん中なので

初期値は 1500 にしておきました。

回路図

schematic.JPG

TeraTerm のログ

000-TeraTerm-log.JPG

main.c

===========================

#include "project.h"

#include "stdio.h"

#include "string.h"

#define STR_LEN 32

#define RX_BUF_LEN 128

#define COMPARE_MAX 2400

#define COMPARE_MIN 500

#define SPACE ' '

#define TAB '\t'

#define CR  '\r'

#define LF  '\n'

inline int is_delimiter(uint8_t c)

{

    int result = 0 ;

    switch(c) {

    case CR:

    case LF:

    case TAB:

    case SPACE:

        result = c ;

        break ;

    }

    return( result ) ;

}

volatile char rx_buf[RX_BUF_LEN] ;

volatile int  rx_read_index = 0 ;

volatile int  rx_write_index = 0 ;

char          str[STR_LEN+1] ; /* print buffer */

int           str_index = 0 ;

CY_ISR(usr_isr)

{

    uint8_t int_flag ;

    UART_ClearRxInterruptSource(UART_INTR_RX_NOT_EMPTY) ;

    if (UART_SpiUartGetRxBufferSize()) {

        int_flag = CyEnterCriticalSection() ;

        rx_buf[rx_write_index] = UART_UartGetByte() ;

        rx_write_index = (rx_write_index + 1) % RX_BUF_LEN ;

        CyExitCriticalSection(int_flag) ;      

    }

}

void print(char *str)

{

    UART_UartPutString(str) ;

}

void help(void)

{

    print("============== usage ===============\n") ;

    print("led {on | off}    : turn LED on or off\n") ;

    sprintf(str, "servo [ numbers ] : set servo pulse in us (%d~%d)\n",

        COMPARE_MIN, COMPARE_MAX) ;

    print(str) ;

    print("flip              : flip the servo\n") ;

    print("status            : show current status\n") ;

    print("help              : print this\n") ;

    print("=====================================\n") ;

}

void init_hardware(void)

{

    CyGlobalIntEnable; /* Enable global interrupts. */

          

    UART_SpiUartClearRxBuffer() ;

    UART_SetCustomInterruptHandler(usr_isr) ;

    UART_Start() ;

  

    PWM_Start() ;

}

void splash(void)

{

    char str[64] ;

    sprintf(str, "TSoC Board UART Command Test (%s %s)\n", __DATE__, __TIME__) ;

    print(str) ;

}

void prompt(void)

{

    print("> ") ;

}

int get_str(void)

{

    int result = 0 ;

    if (rx_read_index != rx_write_index) {

        result = is_delimiter(rx_buf[rx_read_index]) ;

        if (result) { /* a string delimter was detected */

            str[str_index] = 0 ;

            str_index = 0 ;

        } else { /* still in the middle of a string */

            str[str_index++] = rx_buf[rx_read_index] ;

            if (str_index >= STR_LEN) { /* string is too long */

                str[STR_LEN] = 0 ;

                str_index = 0 ;

                result = -1 ;

            }

        }

        rx_read_index = (rx_read_index + 1) % RX_BUF_LEN ;      

    }

    return( result ) ;

}

void str2upper(char *str)

{

    while(str && *str) {

        if (('a' <= *str) && (*str <= 'z')) {

            *str -= ('a' - 'A') ;

        }

        str++ ;

    }

}

void do_led(void)

{

    while(get_str() == 0) ;

    str2upper(str) ;

    if (strcmp(str, "ON") == 0) {

        LED_Write(1) ;

        print("LED is ON\n") ;

    } else {

        LED_Write(0) ;

        print("LED is OFF\n") ;

    }

}

void do_servo(void)

{

    uint16_t compare = 0 ;

    while(get_str() == 0) ;

    str2upper(str) ;

    sscanf(str, "%hd", &compare) ;

    if ((COMPARE_MIN <= compare) && (compare <= COMPARE_MAX)) {

        PWM_WriteCompare(compare) ;

    } else {

        sprintf(str, "Compare value %hd is out of range, ignored\n", compare) ;

        print(str) ;

        sprintf(str, "Acceptable range is %d ~ %d\n", COMPARE_MIN, COMPARE_MAX) ;

        print(str) ;

    }

}

void do_flip(void)

{

    uint16_t compare = 0 ;

    int i ;

  

    compare = PWM_ReadCompare() ;

    for (i = (compare - 500) / 10; i > 0 ; i--) {

        compare = 500 + i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(1) ;

        CyDelay(10) ;

    }

    for (i = 0 ; i < 180 ; i++  ) {

        compare = 500 +  i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(0) ;

        CyDelay(10) ;

     }

    for (i = 180 ; i >= 0 ; i-- ) {

        compare = 500 + i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(1) ;

        CyDelay(10) ;

    }

    for (i = 0 ; i < 90 ; i++  ) {

        compare = 500 +  i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(0) ;

        CyDelay(10) ;

     }

    LED_Write(0) ;

}

void do_status(void)

{

    uint16_t period, compare ;

    int led_value ;

  

    period = PWM_ReadPeriod() ;

    compare = PWM_ReadCompare() ;

    led_value = LED_Read() ;

    print("=== Status ===\n") ;

    sprintf(str, "LED = %d\n", led_value) ;

    print(str) ;

    sprintf(str, "SERVO : %hd / %hd\n", compare, period) ;

    print(str) ;

    print("==============\n") ;

}

void do_command(char *str)

{

    str2upper(str) ;

    if (strcmp(str, "LED") == 0) {

        do_led() ;

    } else if (strcmp(str, "SERVO") == 0) {

        do_servo() ;

    } else if (strcmp(str, "FLIP") == 0) {

        do_flip() ;

    } else if (strcmp(str, "STATUS") == 0) {

        do_status() ;

    } else {

        help() ;

    }

}

int main(void)

{

    init_hardware() ;

  

    splash() ;

    prompt() ;

    for(;;)

    {

        if (get_str()) { /* a string was recived in str[] */

            do_command(str) ;

            prompt() ;

        }

    }

}

===========================

moto

1 Reply
MotooTanaka
Esteemed Contributor

その後、やっぱりいちいちキーボードから数字入れるの面倒くさいよね~

ということで、POTを付けてみました。

IMG_3598.JPG

schematic

schematic.JPG

main.c

==================

#include "project.h"

#include "stdio.h"

#include "string.h"

#define STR_LEN 32

#define RX_BUF_LEN 128

#define COMPARE_MAX 2400

#define COMPARE_MIN 500

#define SPACE ' '

#define TAB '\t'

#define CR  '\r'

#define LF  '\n'

inline int is_delimiter(uint8_t c)

{

    int result = 0 ;

    switch(c) {

    case CR:

    case LF:

    case TAB:

    case SPACE:

        result = c ;

        break ;

    }

    return( result ) ;

}

volatile char rx_buf[RX_BUF_LEN] ;

volatile int  rx_read_index = 0 ;

volatile int  rx_write_index = 0 ;

char          str[STR_LEN+1] ; /* print buffer */

int          str_index = 0 ;

CY_ISR(usr_isr)

{

    uint8_t int_flag ;

    UART_ClearRxInterruptSource(UART_INTR_RX_NOT_EMPTY) ;

    if (UART_SpiUartGetRxBufferSize()) {

        int_flag = CyEnterCriticalSection() ;

        rx_buf[rx_write_index] = UART_UartGetByte() ;

        rx_write_index = (rx_write_index + 1) % RX_BUF_LEN ;

        CyExitCriticalSection(int_flag) ;      

    }

}

void print(char *str)

{

    UART_UartPutString(str) ;

}

void help(void)

{

    print("============== usage ===============\n") ;

    print("led {on | off}    : turn LED on or off\n") ;

    sprintf(str, "servo [ numbers ] : set servo pulse in us (%d~%d)\n",

        COMPARE_MIN, COMPARE_MAX) ;

    print(str) ;

    print("pot              : control servo using pot\n") ;

    print("flip              : flip the servo\n") ;

    print("status            : show current status\n") ;

    print("help              : print this\n") ;

    print("=====================================\n") ;

}

void init_hardware(void)

{

    CyGlobalIntEnable; /* Enable global interrupts. */

          

    UART_SpiUartClearRxBuffer() ;

    UART_SetCustomInterruptHandler(usr_isr) ;

    UART_Start() ;

  

    ADC_Start() ;

  

    PWM_Start() ;

}

void splash(void)

{

    char str[64] ;

    sprintf(str, "TSoC Board UART Command Test (%s %s)\n", __DATE__, __TIME__) ;

    print(str) ;

}

void prompt(void)

{

    print("> ") ;

}

int get_str(void)

{

    int result = 0 ;

    if (rx_read_index != rx_write_index) {

        result = is_delimiter(rx_buf[rx_read_index]) ;

        if (result) { /* a string delimter was detected */

            str[str_index] = 0 ;

            str_index = 0 ;

        } else { /* still in the middle of a string */

            str[str_index++] = rx_buf[rx_read_index] ;

            if (str_index >= STR_LEN) { /* string is too long */

                str[STR_LEN] = 0 ;

                str_index = 0 ;

                result = -1 ;

            }

        }

        rx_read_index = (rx_read_index + 1) % RX_BUF_LEN ;      

    }

    return( result ) ;

}

void str2upper(char *str)

{

    while(str && *str) {

        if (('a' <= *str) && (*str <= 'z')) {

            *str -= ('a' - 'A') ;

        }

        str++ ;

    }

}

void do_led(void)

{

    while(get_str() == 0) ;

    str2upper(str) ;

    if (strcmp(str, "ON") == 0) {

        LED_Write(1) ;

        print("LED is ON\n") ;

    } else {

        LED_Write(0) ;

        print("LED is OFF\n") ;

    }

}

void do_servo(void)

{

    uint16_t compare = 0 ;

    while(get_str() == 0) ;

    str2upper(str) ;

    sscanf(str, "%hd", &compare) ;

    if ((COMPARE_MIN <= compare) && (compare <= COMPARE_MAX)) {

        PWM_WriteCompare(compare) ;

    } else {

        sprintf(str, "Compare value %hd is out of range, ignored\n", compare) ;

        print(str) ;

        sprintf(str, "Acceptable range is %d ~ %d\n", COMPARE_MIN, COMPARE_MAX) ;

        print(str) ;

    }

}

void do_flip(void)

{

    uint16_t compare = 0 ;

    int i ;

  

    compare = PWM_ReadCompare() ;

    for (i = (compare - 500) / 10; i > 0 ; i--) {

        compare = 500 + i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(1) ;

        CyDelay(10) ;

    }

    for (i = 0 ; i < 180 ; i++  ) {

        compare = 500 +  i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(0) ;

        CyDelay(10) ;

    }

    for (i = 180 ; i >= 0 ; i-- ) {

        compare = 500 + i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(1) ;

        CyDelay(10) ;

    }

    for (i = 0 ; i < 90 ; i++  ) {

        compare = 500 +  i * 10 ;

        PWM_WriteCompare(compare) ;

        LED_Write(0) ;

        CyDelay(10) ;

    }

    LED_Write(0) ;

}

void do_status(void)

{

    uint16_t period, compare ;

    int led_value ;

  

    period = PWM_ReadPeriod() ;

    compare = PWM_ReadCompare() ;

    led_value = LED_Read() ;

    print("=== Status ===\n") ;

    sprintf(str, "LED = %d\n", led_value) ;

    print(str) ;

    sprintf(str, "SERVO : %hd / %hd\n", compare, period) ;

    print(str) ;

    print("==============\n") ;

}

int16_t measure(void)

{

    ADC_StartConvert() ;

    ADC_IsEndConversion(ADC_WAIT_FOR_RESULT) ;

    return( ADC_GetResult16(0) ) ;

}

void do_pot_servo(void)

{

    int16_t adc_value ;

    uint16_t compare = 0 ;

    uint16_t prev_compare = 0 ;

    print("Use pot to control servo\n") ;

    print("Push Reset to exit\n") ;

    while(1) {

        adc_value = measure() ;

        compare = COMPARE_MIN + (COMPARE_MAX - COMPARE_MIN) * (adc_value & 0x7FF) / 0x7FF ;

        if (compare < COMPARE_MIN) {

            compare = COMPARE_MIN ;

        }

        if (compare > COMPARE_MAX) {

            compare = COMPARE_MAX ;

        }

        if (prev_compare != compare) {

            PWM_WriteCompare(compare) ;

            prev_compare = compare ;

        }

        CyDelay(50) ;

    }

}

      

void do_command(char *str)

{

    str2upper(str) ;

    if (strcmp(str, "LED") == 0) {

        do_led() ;

    } else if (strcmp(str, "SERVO") == 0) {

        do_servo() ;

    } else if (strcmp(str, "POT") == 0) {

        do_pot_servo() ;

    } else if (strcmp(str, "FLIP") == 0) {

        do_flip() ;

    } else if (strcmp(str, "STATUS") == 0) {

        do_status() ;

    } else {

        help() ;

    }

}

int main(void)

{

    init_hardware() ;

  

    splash() ;

    prompt() ;

    for(;;)

    {

        if (get_str()) { /* a string was recived in str[] */

            do_command(str) ;

            prompt() ;

        }

    }

}

==================

moto

0 Likes