0 Replies Latest reply on Mar 15, 2020 5:57 PM by MoTa_728816

    CY8CKIT-044 5-Axis Robot Arm

    MoTa_728816

      IMG_2794.jpeg

      So it was early 2018, I found a sale on a 5-axis Robot Arm at Saint Smart

      https://www.sainsmart.com/collections/robotics/products/s5-5-axis-desktop-robotic-arm-with-servos

       

      それは 2018年の初頭、Saint Smart のウェブで5軸ロボットアームがセールになっているのを見つけました。

      https://www.sainsmart.com/collections/robotics/products/s5-5-axis-desktop-robotic-arm-with-servos

       

      Since it was less than $70.00 at that time, I decided that even if I will fail the damage will not be big.

       

      まぁ、1万円で結構おつりがくる代物だったので、失敗しても、まっいいか、くらいのノリで注文しました。

       

      As I am a man of "Think after I act", I noticed that I need a driver board between the PSoC Board and the RobotArm.

       

      例によって“論よりRun”で行ってしまった為、サーボを動かすのにドライバ基板が必要なことに後から気が付きました。

       

      At first I ordered a DC/DC up converter from Akizuki

      http://akizukidenshi.com/catalog/g/gK-13065/

       

      先ずは秋月で DC/DCコンバータを注文してみました。

      http://akizukidenshi.com/catalog/g/gK-13065/

       

      I thought it has enough current and voltage, but it did not work (T^T)

      because it can not keep up with the PWM for the Servo.

       

      電流も電圧も充分だったはずなのですが、動きませんでした (;_;)

      周波数応答性がサーボに必要な全然追いついていないようでした。

       

      Finally I decided that it was time to ask help for my hardware colleague.

      After let him laugh a while, he suggested me to get this device (IC BUFF/DVR NON-INVERT).

      https://www.marutsu.co.jp/pc/i/15339510/

       

      ようやく、これはハードウェアさんに助けを求めないとダメだなと思い、

      同僚のハードウェアの匠に話に行きました。

      私の選んだデバイスをみて少し笑ってから、

      彼は下記のデバイスを入手するように指示してくれました。(IC BUFF/DVR NON-INVERT).

      https://www.marutsu.co.jp/pc/i/15339510/

       

      So I asked him to make me a board with following connections.

       

      そこで、以下のような配線で基板を作ってもらえるようにお願いしました。

      003-shield_pin_list.JPG

       

      Now time to rock with the PSoC Creator!

       

      さて、ようやく PSoC Creator でブイブイ言わせるところに到達しました。

       

      Schematic

      Note: Well for the robot arm, circuit is not so complicated.

      I used a 5x5 Matrix Key Pad to control the arm.

      注:ロボットアーム用の回路自体は簡単なものですね。

      制御用に以前買って持っていた 5x5 マトリックキーパッドを使用しました。

      000-circuit.JPG

      Pins

      002-pin-list.JPG

      main.c

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

      /* ========================================

      * 5ch Servo Robot ARM controller

      *

      * According to the servo datasheets

      * usual RC servo PWM is

      * frequency: 50Hz = 20ms/cycle

      * PWM width of center: 1.5ms = 1500 us

      * Dynamic Rage is -500us ~ +500us

      * which makes the PWM width from 1000us to 2000us

      *

      * Servos

      * A: ch0 : Base

      * B: ch1 : Shoulder

      * C: ch2 : Elbow

      * D: ch3 : Wrist

      * E: ch4 : Grip

      *

      * ========================================

      */

      #include "project.h"

      #include "main.h"

      #include "servo_ctrl.h"

      #include "num_key.h"

      #include <stdio.h>

      #include <stdlib.h>

       

      int gVerbose = 0 ;

       

      #if 0 /* general servo */

      #define MIN_PULSE    1000

      #define CENTER_PULSE 1500

      #define MAX_PULSE    2000

      #else /* Servo = SG90 */

      #define MIN_PULSE    500

      #define CENTER_PULSE 1500

      #define MAX_PULSE    2500

      #endif

      /****

      * Key layout

      * 20 21 22 23 24

      * 15 16 17 18 19

      * 10 11 12 12 14

      *  5  6  7  8  9

      *  0  1  2  3  4

      */

       

      char str[STRING_LENGTH+1] ;

       

      int pause[][5] = {

          {1500, 1500, 1500, 1500, 1500},

          {1450,  820,  640, 1460, 1480},

          {1450, 1870, 1550, 1460,  990},

          {1530, 2020, 1440, 1460, 1080},

          {1530, 1380, 2500, 1460, 1720}

      } ;

      int num_pause = sizeof(pause)/(sizeof(int) * 5) ;

       

      void doHelp(void)

      {

          UART_UartPutString("\n===== HELP =====\n") ;

          UART_UartPutString("HELP         SPEED1     SPEED2      SPEED3        SPEED4\n") ;

          UART_UartPutString("REPORT       pause1       Wrist<-   Elbow(Up)      Wrist->\n") ;

          UART_UartPutString("pause0       pause2        (NC)     Elbow(Down)      (NC)\n") ;

          UART_UartPutString("SILENT       pause3        (NC)     Shoulder(Up)     (NC)\n") ;

          UART_UartPutString("Grip(Open) Grip(Close) Base(Right) Shoulder(Down) Base(Left)\n") ;

          UART_UartPutString("==================\n") ;

      }

       

      void doReport(void)

      {

          int i ;

          UART_UartPutString("\n==== REPORT ====\n") ;

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

              print_servo(i) ;

          }

          if (gVerbose) {

              sprintf(str, "Verbose Mode ") ;

          } else {

              sprintf(str, "Silent Mode ") ;

          }

          UART_UartPutString(str) ;

          UART_UartPutString("\n==================\n") ;

      }

       

      void set_pause(void)

      {

          pause_to(pause[0]) ;

      #if 0

          int i ;

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

              servo[i].WriteCompare(servo[i].value) ;

          }

          CyDelay(40) ;

      #endif

      }

       

      void doPause(int num)

      {

          pause_to(pause[num]) ;

      #if 0

          int i, j ;

          int current[NUM_CHANNEL] ;

          int delta[20] ;

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

              current[i] = servo[i].value ;

              delta[i] = (pause[num][i] - current[i]) / 20 ;

          }

          for (j = 0 ; j < 19 ; j++ ) {

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

                  servo[i].value = current[i] + delta[i] * j ;

              }

              set_pause() ;

              CyDelay(40) ;

          }

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

              servo[i].value = pause[num][i] ;

          }

          set_pause() ;

      #endif

      }

       

      void reset_pause(void)

      {

          pause_to(pause[0]) ;

      #if 0

          int i ;

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

              ch_move_to(i, servo[i].center) ;

      //        servo[i].value = servo[i].center ;

          }

          set_pause() ;

      #endif

      }

         

      void list_pause(void)

      {

          int i ;

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

              sprintf(str, "%s[%d] ", servo[i].name, servo[i].value) ;

              UART_UartPutString(str) ;

          }

          UART_UartPutString("\n") ;

      }

       

      void assign_servo(int ch_no, int pulse)

      {

          ch_move_to(ch_no, pulse) ;

      #if 0

          if (pulse > servo[ch_no].max) {

              servo[ch_no].value = servo[ch_no].max ;

          } else if (pulse < servo[ch_no].min) {

              servo[ch_no].value = servo[ch_no].min ;

          } else {

              servo[ch_no].value = pulse ;

          }

          servo[ch_no].WriteCompare(servo[ch_no].value) ;

      #endif

      }

       

       

      void doSpeed(int value)

      {

          int i, delta = 20;

          switch(value) {

          case 1: delta = 50 ; break ;

          case 2: delta = 100 ; break ;

          case 3: delta = 300 ; break ;

          case 4: delta = 600 ; break ;

          }

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

              servo[i].step = delta ;

          }

      }

       

       

      void toggle_verbose(void)

      {

          if (gVerbose) {

              UART_UartPutString("Silent Mode\n") ;

              gVerbose = 0 ;

          } else {

              UART_UartPutString("Verbose Mode\n") ;

              gVerbose = 1 ;

          }

      }

       

      void doCommand(uint32_t key)

      {

          if (key & CH0_DOWN) { ch_down(0) ; }

          if (key & CH0_UP)   { ch_up(0) ; }

          if (key & CH1_DOWN) { ch_down(1) ; }

          if (key & CH1_UP)   { ch_up(1) ; }

          if (key & CH2_DOWN) { ch_down(2) ; }

          if (key & CH2_UP)   { ch_up(2) ; }

          if (key & CH3_DOWN) { ch_down(3) ; }

          if (key & CH3_UP)   { ch_up(3) ; }

          if (key & CH4_DOWN) { ch_down(4) ; }

          if (key & CH4_UP)   { ch_up(4) ; }

          if (key & SPEED1)   { doSpeed(1) ; }

          if (key & SPEED2)   { doSpeed(2) ; }

          if (key & SPEED3)   { doSpeed(3) ; }

          if (key & SPEED4)   { doSpeed(4) ; }

          if (key & SHOW_HELP) { doHelp() ; }

          if (key & SILENT)   { toggle_verbose() ; }

          if (key & REPORT)   { doReport() ; }

          if (key & PAUSE0)   { doPause(0) ; }

          if (key & PAUSE1)   { doPause(1) ; }

          if (key & PAUSE2)   { doPause(2) ; }

          if (key & PAUSE3)   { doPause(3) ; }

          if (key & PAUSE4)   { doPause(4) ; }

      }

       

      void init_hardware(void)

      {

          CyGlobalIntEnable; /* Enable global interrupts. */

          UART_Start() ;

         

          PWMA_Start() ;

          PWMB_Start() ;

          PWMC_Start() ;

          PWMD_Start() ;

          PWME_Start() ;

          reset_pause() ;

      }

       

      int main(void)

      {

          uint32_t key ;

         

          init_hardware() ;

         

          UART_UartPutString("Program Started\n") ;

       

          for (;;) {

              key = num_key_read() ;

              if (key) {

                  doCommand(key) ;

                  if (gVerbose) {

                      sprintf(str, "key: %08X\n", (unsigned int)key) ;

                      UART_UartPutString(str) ;

                  }

              }

              CyDelay(100) ;

          }

      }

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

       

      Well, well, well, this time the story does not end here.

      Although the robot arm is a little one, still it's servo has rather strong power

      and the inertia of the weight of servos and arm is not ignore-able.

       

      さて、さて、さて、いつもはここで終わりなのですが、

      今回のロボットアーム、小型とはいえサーボも強力ですし、

      サーボとアーム自体の重さによる慣性も無視できません。

       

      So I skimmed some motion control webs.

      And decided that I need some control over the movement.

       

      そこでモーションコントロールに関するウェブを少しサーフィンして

      動作を少し制御してやる必要があると感じました。

       

      If I set the servo's target angle directly, the arm acts like a wild horse.

      So we need to increase the velocity gradually and decrease it gradually, too.

      Which is the velocity control, but to realize smooth movement we need to control

      the acceleration and farther more the jerk.

       

      もし、サーボに移動目的の角度を直接設定するとジャジャ馬のような動きになってしまいます。

      その為速度を徐々に上げて、また徐々に下げる必要があります。

      これが速度制御と呼ばれるものなのですが、スムーズな動作制御には

      加速の制御、さらには躍度の制御が必要になるようです。

       

      Unfortunately neither of PSoC nor I have enough CPU power for a real-time differential equation solving.

      I approached it with hand curve fitting with Excel.

      Ideally there should be not "edge" in the jerk curve I could not avoid a couple of them with this picture.

      But still even with this level, the movement of servos were much smoother and sound was more quiet.

       

      残念ながら PSoC も 私もリアルタイムで微分方程式を解くのに充分は CPU パワーを有していないため

      エクセルを使いながら、カーブの手動フィッテングを行いました。

      理想的には躍度の曲線に角が無い方が良いのですが、流石に2点残ってしまいました。

      それでもこのレベルの調整で動作はかなりスムーズになりましたし、音も静かになりました。

       

      004-jerk-control.JPG

       

      Finally, it was difficult or almost impossible to control 5 axis same time,

      I added a feature "pause #", so that I can store some number of pauses

      and recall it by one button.

       

      最後に 5軸を同時に手で制御するのはほぼ無理だったので、

      "pause #" という機能を増やして、予め登録しておいた設定を

      ボタン一つで呼び起せるようにしました。

       

      moto