1 Reply Latest reply on May 15, 2019 10:26 PM by MoTa_728816

    TSoC CY8C4146LQI-S433 基板 サーボ SG-90 サンプル

    MoTa_728816

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

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

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

       

      良い機会と 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. Re: TSoC CY8C4146LQI-S433 基板 サーボ SG-90 サンプル
          MoTa_728816

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

          ということで、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

          1 of 1 people found this helpful