日記

フリーページ

2019年02月21日
XML
カテゴリ:Linux


ここ最近の検討したものを全部突っ込んで、ロボットのトリム調整プログラムを作成しました。
ちゃんと、23個のサーボモーターに5msで指示が出せています。(^-^)V

コンパイルしたプログラムは、LAN経由でLinuxボード(のSDカード内のフォルダ)に保存してあるので、
ターミナル接続するだけで実行できます。ただし、環境設定してないので、実行するためにはフルパスが必要です。
/home/root/robo って感じです。

しかし、アルミ板金でバックパックを早く作らねば・・・。

プログラムはこんな感じ。

#include <stdio.h> // printf
#include <stdlib.h> // exit,strtol
#include <time.h> // clock
#include <pthread.h> // pthread
#include <termios.h> // termios
#include <unistd.h> // read & write
#include <sys/types.h> // open
#include <sys/stat.h> // open
#include <sys/ioctl.h> // ioctl
#include <fcntl.h> // open
#include <string.h> // memcpy
#define hz 200 // 5msec
#define N_servo 25
#define console_view_time 20 // 0.1*hz
#define approach_time 600 // 3*hz
#define POS_MIN 3500
#define POS_MAX 11500
/********************************/
/* サーボスレッド用マクロ記述部 */
/********************************/
 /* 機能SW */
#define CH1_1_S 1
#define CH1_2_S 1
#define CH1_3_S 1
#define CH1_4_S 0
#define CH2_1_S 1
#define CH2_2_S 1
#define CH2_3_S 1
#define CH2_4_S 0
#define CH3_1_S 1
#define CH3_2_S 1
#define CH3_3_S 1
#define CH3_4_S 0
#define CH4_1_S 1
#define CH4_2_S 1
#define CH4_3_S 1
#define CH4_4_S 0
#define CH5_1_S 1
#define CH5_2_S 1
#define CH5_3_S 1
#define CH5_4_S 1
#define CH6_1_S 1
#define CH6_2_S 1
#define CH6_3_S 1
#define CH6_4_S 0
#define CH7_1_S 1
#define CH7_2_S 1
#define CH7_3_S 1
#define CH7_4_S 1
 /* リンクID */
#define CH1_1_L 4
#define CH1_2_L 3
#define CH1_3_L 2
#define CH2_1_L 10
#define CH2_2_L 9
#define CH2_3_L 8
#define CH3_1_L 11
#define CH3_2_L 12
#define CH3_3_L 13
#define CH4_1_L 14
#define CH4_2_L 19
#define CH4_3_L 24
#define CH5_1_L 20
#define CH5_2_L 21
#define CH5_3_L 22
#define CH5_4_L 23
#define CH6_1_L 5
#define CH6_2_L 6
#define CH6_3_L 7
#define CH7_1_L 15
#define CH7_2_L 16
#define CH7_3_L 17
#define CH7_4_L 18
 /* サーボID */
#define CH1_1_ID 28
#define CH1_2_ID 27
#define CH1_3_ID 26
#define CH2_1_ID 22
#define CH2_2_ID 21
#define CH2_3_ID 20
#define CH3_1_ID 23
#define CH3_2_ID 24
#define CH3_3_ID 25
#define CH4_1_ID 1
#define CH4_2_ID 9
#define CH4_3_ID 0
#define CH5_1_ID 10
#define CH5_2_ID 11
#define CH5_3_ID 12
#define CH5_4_ID 13
#define CH6_1_ID 29
#define CH6_2_ID 30
#define CH6_3_ID 31
#define CH7_1_ID 2
#define CH7_2_ID 3
#define CH7_3_ID 4
#define CH7_4_ID 5
#define PP_W_0(c,i) \
 do { \
 } \
 while (0);
#define PP_W_1(c,i) \
 do { \
  a[0] = 0x80 + CH ## c ## _ ## i ## _ID; \
  a[1] = 0x7f & (sPOS[CH ## c ## _ ## i ## _L] >> 7); \
  a[2] = 0x7f & sPOS[CH ## c ## _ ## i ## _L]; \
  write(fd ## c ,a,3); \
 } \
 while (0);
#define PP_W_SS(c,i,s) PP_W_ ## s (c,i)
#define PP_W_S(c,i,s) PP_W_SS(c,i,s)
#define PP_W(c,i) PP_W_S(c,i,CH ## c ## _ ## i ## _S)
#define PP_R_0(c,i) \
 do { \
 } \
 while (0);
#define PP_R_1(c,i) \
 do { \
  if (read(fd ## c ,a,64) == 6) { \
   if ((int)(0x7f & a[3]) == CH ## c ## _ ## i ## _ID) { \
    sTCH[CH ## c ## _ ## i ## _L] = (((int)(a[4])) << 7) + (int)(a[5]); \
   } else { \
    sTCH[CH ## c ## _ ## i ## _L] = 0; \
   } \
  } else { \
   sTCH[CH ## c ## _ ## i ## _L] = 0; \
  } \
 } \
 while (0);
#define PP_R_SS(c,i,s) PP_R_ ## s (c,i)
#define PP_R_S(c,i,s) PP_R_SS(c,i,s)
#define PP_R(c,i) PP_R_S(c,i,CH ## c ## _ ## i ## _S)
#define PP_O(c) \
 do { \
  fd ## c = open("/dev/ttySC1" # c ,O_RDWR); \
  if (fd ## c < 0) { \
   printf("open error\n"); \
   exit(1); \
  } \
  tcsetattr(fd ## c ,TCSANOW,&tio); \
  ioctl(fd ##c ,TCSETS,&tio); \
 } \
 while (0);
/************************/
/* グローバル変数記述部 */
/************************/
pthread_mutex_t mutex_servo;
pthread_cond_t  cond_servo;
struct timespec tsb;
int POS[N_servo];
int TCH[N_servo];
int timer_servo;
int stop_request;
/**********************/
/*   サーボスレッド   */
/**********************/
void *servo_thread(void *arg)
{
 struct timespec tsrq;
 int fd1,fd2,fd3,fd4,fd5,fd6,fd7;
 struct termios tio;
 int baudRate = B115200;
 int sPOS[N_servo];
 int sTCH[N_servo];
 unsigned char a[64];
 /* シリアル通信起動 */
 bzero(&tio,sizeof(tio));
 cfsetispeed(&tio,baudRate);
 cfsetospeed(&tio,baudRate);
 cfmakeraw(&tio);
 tio.c_cc[VMIN] = 0;
 tio.c_cc[VTIME] = 0;
 tio.c_cflag += CREAD;
 tio.c_cflag += CLOCAL;
 tio.c_cflag += PARENB;
 PP_O(1)
 PP_O(2)
 PP_O(3)
 PP_O(4)
 PP_O(5)
 PP_O(6)
 PP_O(7)
 /* タイマ初期化 */
 clock_gettime(CLOCK_REALTIME,&tsb);
 tsb.tv_sec++;
 tsb.tv_sec++;
 tsb.tv_nsec = 0;
 clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&tsb,NULL);
 timer_servo = 0;
pthread_mutex_lock(&mutex_servo);
 /* 繰り返し処理 */
while(!stop_request)
{
 /* POS読み込み */
 memcpy(sPOS,POS,sizeof(int)*N_servo);
 
pthread_mutex_unlock(&mutex_servo);
 /* 1回目送信 */
 PP_W(1,1)
 PP_W(2,1)
 PP_W(3,1)
 PP_W(4,1)
 PP_W(5,1)
 PP_W(6,1)
 PP_W(7,1)
 /* 1msスリープ */
 tsrq.tv_sec = 0;
 tsrq.tv_nsec = 1000000;
 nanosleep(&tsrq,NULL);
 /* 1回目受信 */
 PP_R(1,1)
 PP_R(2,1)
 PP_R(3,1)
 PP_R(4,1)
 PP_R(5,1)
 PP_R(6,1)
 PP_R(7,1)
 /* 2回目送信 */
 PP_W(1,2)
 PP_W(2,2)
 PP_W(3,2)
 PP_W(4,2)
 PP_W(5,2)
 PP_W(6,2)
 PP_W(7,2)
 /* 1msスリープ */
 tsrq.tv_sec = 0;
 tsrq.tv_nsec = 1000000;
 nanosleep(&tsrq,NULL);
 /* 2回目受信 */
 PP_R(1,2)
 PP_R(2,2)
 PP_R(3,2)
 PP_R(4,2)
 PP_R(5,2)
 PP_R(6,2)
 PP_R(7,2)
 /* 3回目送信 */
 PP_W(1,3)
 PP_W(2,3)
 PP_W(3,3)
 PP_W(4,3)
 PP_W(5,3)
 PP_W(6,3)
 PP_W(7,3)
 /* 1msスリープ */
 tsrq.tv_sec = 0;
 tsrq.tv_nsec = 1000000;
 nanosleep(&tsrq,NULL);
 /* 3回目受信 */
 PP_R(1,3)
 PP_R(2,3)
 PP_R(3,3)
 PP_R(4,3)
 PP_R(5,3)
 PP_R(6,3)
 PP_R(7,3)
 /* 4回目送信 */
 PP_W(1,4)
 PP_W(2,4)
 PP_W(3,4)
 PP_W(4,4)
 PP_W(5,4)
 PP_W(6,4)
 PP_W(7,4)
 /* 1msスリープ */
 tsrq.tv_sec = 0;
 tsrq.tv_nsec = 1000000;
 nanosleep(&tsrq,NULL);
 /* 4回目受信 */
 PP_R(1,4)
 PP_R(2,4)
 PP_R(3,4)
 PP_R(4,4)
 PP_R(5,4)
 PP_R(6,4)
 PP_R(7,4)
 /* 時刻指定スリープ */
 tsrq.tv_sec = tsb.tv_sec + (timer_servo + 1) / hz;
 tsrq.tv_nsec = ((timer_servo + 1) % hz) * (1000000000 / hz);
 if(clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&tsrq,NULL) != 0)
 {
  printf("Error: on clock_nanosleep.\n");
  exit(1);
 }
pthread_mutex_lock(&mutex_servo);
 /* タイマ処理 */
 timer_servo++;
 /* TCH書き込み */
 memcpy(TCH,sTCH,sizeof(int)*N_servo);
 /* メインスレッドに通知 */
 pthread_cond_signal(&cond_servo);
}
pthread_mutex_unlock(&mutex_servo);
 /* ストップ処理 */
 close(fd1);
 close(fd2);
 close(fd3);
 close(fd4);
 close(fd5);
 close(fd6);
 close(fd7);
 return NULL;
}
/**********************/
/*   メインスレッド   */
/**********************/
int main(int argc, char **argv)
{
 pthread_t thread_servo;
 FILE *file;
 int mPOS[N_servo];
 int iPOS[N_servo];
 int aPOS[N_servo];
 int mTCH[N_servo];
 int timer_main;
 int step_console;
 int timer_console;
 int console_select_no;
 int i;
 char c;
 struct termios term_attr,term_backup;
 struct timespec tsm;
 /* 標準入力設定(入力待ちなし) */
 tcgetattr(0,&term_attr);
 term_backup = term_attr;
 term_attr.c_lflag &= ~(ICANON|ECHO);
 term_attr.c_cc[VMIN] = 0;
 term_attr.c_cc[VTIME] = 0;
 tcsetattr(0,TCSANOW,&term_attr);
 fcntl(0,F_SETFL,O_NONBLOCK);
 /* 初期化 */
 stop_request = 0;
 step_console = 0;
 timer_console = 0;
 console_select_no = 2;
 /* 起動画面 */
 printf("\n**********************************\n");
 printf(  "*  RoBo System Powered by Linux  *\n");
 printf(  "**********************************\n");
 /* iPOS読み込み */
 file = fopen("iPOS.txt","r");
 if(file == NULL) {
  printf("Can't open file:%s\n","iPOS.txt");
  exit(1);
 }
 for(i = 2; i < N_servo; i++ ) {
  fscanf(file,"%d\n",&iPOS[i]);
 }
 fclose(file);
 /* iPOS表示 */
 printf("\n*** Load iPOS ***\n\n");
 for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
  printf("No.%2d ",i);
 } printf("\n");
 for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
  printf("%5d ",iPOS[i]);
 } printf("\n");
 for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
  printf("No.%2d ",i);
 } printf("\n");
 for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
  printf("%5d ",iPOS[i]);
 } printf("\n");
 printf("\nCapture Start\n\n\n\n\n\n\n");
 /* POS初期化 */
 for(i = 2; i < N_servo; i++) {
  POS[i] = 0;
 }
 /* サーボスレッド作成 */
 pthread_mutex_init(&mutex_servo,NULL);
 pthread_cond_init(&cond_servo,NULL);
 if(pthread_create(&thread_servo,NULL,servo_thread,NULL) != 0)
 {
  printf("Error: Failed to create servo thread.\n");
  exit(1);
 }
pthread_mutex_lock(&mutex_servo);
 /* 繰り返し処理 */
while(!stop_request)
{
 /* サーボスレッドの通知待ち */
 if(pthread_cond_wait(&cond_servo,&mutex_servo) != 0)
 {
  printf("Error: on pthread_cond_wait.\n");
  exit(1);
 }
 /* タイマ処理 */
 timer_main = timer_servo;
 /* TCH読み込み */
 memcpy(mTCH,TCH,sizeof(int)*N_servo);
 
pthread_mutex_unlock(&mutex_servo);
 /* コンソール処理 */
 switch( step_console ) {
  case 0:
   timer_console++;
   if(timer_console > console_view_time-1) {
    timer_console = 0;
    /* mTCH表示 */
    printf("\033[5A"); // カーソルを上に
    for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
     printf("No.%2d ",i);
    } printf("\n");
    for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
     printf("%5d ",mTCH[i]);
    } printf("\n");
    for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
     printf("No.%2d ",i);
    } printf("\n");
    for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
     printf("%5d ",mTCH[i]);
    } printf("\n");
    printf("[q:Quit  a:Approaching iPOS  h:Hold(iPOS < TCH)]\n");
    if ( read(0, &c, 1) == 1 ) {
     if ( c == 'q' ) stop_request = 1;
     if ( c == 'a' ) {
      step_console = 1;
      for(i = 2; i < N_servo; i++) {
       aPOS[i] = mTCH[i];
      }
      timer_console = 0;
     }
     if ( c == 'h' ) {
      step_console = 2;
      for(i = 2; i < N_servo; i++) {
       iPOS[i] = mTCH[i];
      }
      timer_console = 0;
      printf("\n*** Hold(iPOS < TCH) ***\n\n\n\n\n\n\n");
     }
    }
   }
   for(i = 2; i < N_servo; i++) {
    mPOS[i] = 0;
   }
   break;
  case 1:
   timer_console++;
   for(i = 2; i < N_servo; i++) {
    mPOS[i] = aPOS[i] + ((iPOS[i]-aPOS[i])*timer_console)/approach_time;
   }
   if(timer_console > approach_time - 1) {
    step_console = 2;
    printf("\nIn Operation\n\n\n\n\n\n\n");
   }
   if ( read(0, &c, 1) == 1 ) step_console = 0;
   break;
  case 2:
   timer_console++;
   if(timer_console > console_view_time-1) {
    timer_console = 0;
    /* iPOS表示 */
    printf("\033[5A"); // カーソルを上に
    for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
     printf("No.%2d ",i);
    } printf("\n");
    for ( i = 2; i < (N_servo+1)/2+1; i++ ) {
     printf("%5d ",iPOS[i]);
    } printf("\n");
    for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
     printf("No.%2d ",i);
    } printf("\n");
    for ( i = (N_servo+1)/2+1; i < N_servo; i++ ) {
     printf("%5d ",iPOS[i]);
    } printf("\n");
    printf("[Trim No.%2d 1-8:Trim  f:Free  s:Save iPOS]\n",console_select_no);
    if ( read(0, &c, 1) == 1 ) {
     if ( c == 'f' ) {
      step_console = 0;
      printf("\n*** Servo Free ***\n");
      printf("\nCapture Start\n\n\n\n\n\n\n");
     }
     if ( c == 's' ) {
      file = fopen("iPOS.txt","w");
      if (file == NULL) {
       printf("Can't open file:%s\n","iPOS.txt");
      }
      for ( i = 2; i < N_servo; i++ ) {
       fprintf(file,"%d\n",iPOS[i]);
      }
      fclose(file);
      printf("\n*** Save iPOS ***\n\n\n\n\n\n\n");
     }
     if ( c == '1' ) {
      iPOS[console_select_no] = iPOS[console_select_no] - 1;
      if (iPOS[console_select_no] < POS_MIN) {
       iPOS[console_select_no] = POS_MIN;
      }
     }
     if ( c == '2' ) {
      iPOS[console_select_no] = iPOS[console_select_no] - 10;
      if (iPOS[console_select_no] < POS_MIN) {
       iPOS[console_select_no] = POS_MIN;
      }
     }
     if ( c == '3' ) {
      iPOS[console_select_no] = iPOS[console_select_no] - 100;
      if (iPOS[console_select_no] < POS_MIN) {
       iPOS[console_select_no] = POS_MIN;
      }
     }
     if ( c == '4' ) {
      iPOS[console_select_no] = iPOS[console_select_no] + 1;
      if (iPOS[console_select_no] > POS_MAX) {
       iPOS[console_select_no] = POS_MAX;
      }
     }
     if ( c == '5' ) {
      iPOS[console_select_no] = iPOS[console_select_no] + 10;
      if (iPOS[console_select_no] > POS_MAX) {
       iPOS[console_select_no] = POS_MAX;
      }
     }
     if ( c == '6' ) {
      iPOS[console_select_no] = iPOS[console_select_no] + 100;
      if (iPOS[console_select_no] > POS_MAX) {
       iPOS[console_select_no] = POS_MAX;
      }
     }
     if ( c == '7' ) {
      console_select_no = console_select_no - 1;
      if (console_select_no < 2) {
       console_select_no = N_servo - 1;
      }
     }
     if ( c == '8' ) {
      console_select_no = console_select_no + 1;
      if (console_select_no > N_servo - 1) {
       console_select_no = 2;
      }
     }
    }
   }
   for(i = 2; i < N_servo; i++) {
    mPOS[i] = iPOS[i];
   }
   break;
 }
pthread_mutex_lock(&mutex_servo);
 /* POS書き込み */
 memcpy(POS,mPOS,sizeof(int)*N_servo);
}
pthread_mutex_unlock(&mutex_servo);
 /* ストップ処理 */
 clock_gettime(CLOCK_REALTIME,&tsm);
 printf("\nservo %dsec %dmsec\n",timer_main / hz,(timer_main % hz)*1000/hz);
 printf(  "main  %dsec %ldmsec\n\n",(int)(tsm.tv_sec - tsb.tv_sec),(tsm.tv_nsec - tsb.tv_nsec)/1000000);
 if(pthread_join(thread_servo,NULL) != 0)
 {
  printf("Error: Failed to wait for servo thread termination.\n");
  exit(1);
 }
 pthread_mutex_destroy(&mutex_servo);
 pthread_cond_destroy(&cond_servo);
 /* 標準入力復帰 */
 tcsetattr(0,TCSANOW,&term_backup);
 return 0;
}









最終更新日  2019年02月21日 22時30分54秒

PR

キーワードサーチ

▼キーワード検索

プロフィール


nisiken2002

お気に入りブログ

まだ登録されていません

Copyright (c) 1997-2019 Rakuten, Inc. All Rights Reserved.