PIC24FJで４足ロボットの製作（その５）

　今回はpicle言語に組み込むC言語側の処理等について書いてみます。

1. 組込変数
   　 PIC24FJで４足ロボットの製作（その２）で書いたようにサーボ制御用の変数をpicle言語に組み込みました。
   　setval にサーボ制御の PWM のパルス幅を設定すると割込み処理内で生成している PWM パルス幅を変更することができます。
   　この変数は主にサーボ回転角度の調整のために使います。
   
   

   typedef struct { int16_t angle; // servo angle int16_t width; // servo PWM width } TYPE_SERVO_CTRL; // servo control typedef struct { TYPE_SERVO_CTRL min; TYPE_SERVO_CTRL center; TYPE_SERVO_CTRL max; int16_t setval; } TYPE_SERVO; TYPE_SERVO Servo[12];

   
   
   　調整結果は下記のように picle 側の初期化処理で設定しています。

   proc Init() { var s; s = Servo_; s[ 0]=-90;s[ 1]=$04c0;s[ 2]= 0;s[ 3]=$0b40;s[ 4]= 90;s[ 5]=$1260;s[ 6]=s[ 3]; s[ 7]=-90;s[ 8]=$0560;s[ 9]= 0;s[10]=$0c00;s[11]= 90;s[12]=$1340;s[13]=s[10]; s[14]=-45;s[15]=$10e0;s[16]=45;s[17]=$0a80;s[18]=135;s[19]=$0440;s[20]=s[17]; Wait( 20 ); s[21]=-90;s[22]=$0520;s[23]= 0;s[24]=$0bc0;s[25]= 90;s[26]=$1300;s[27]=s[24]; s[28]=-90;s[29]=$1280;s[30]= 0;s[31]=$0bc0;s[32]= 90;s[33]=$0540;s[34]=s[31]; s[35]=-45;s[36]=$0560;s[37]=45;s[38]=$0b40;s[39]=135;s[40]=$1200;s[41]=s[38]; Wait( 20 ); s[42]=-90;s[43]=$0530;s[44]= 0;s[45]=$0b80;s[46]= 90;s[47]=$12c0;s[48]=s[45]; s[49]=-90;s[50]=$0480;s[51]= 0;s[52]=$0ae0;s[53]= 90;s[54]=$1200;s[55]=s[52]; s[56]=-45;s[57]=$1360;s[58]=45;s[59]=$0c00;s[60]=135;s[61]=$0580;s[62]=s[59]; Wait( 20 ); s[63]=-90;s[64]=$05c0;s[65]= 0;s[66]=$0c00;s[67]= 90;s[68]=$1340;s[69]=s[66]; s[70]=-90;s[71]=$12c0;s[72]= 0;s[73]=$0bc0;s[74]= 90;s[75]=$0520;s[76]=s[73]; s[77]=-45;s[78]=$0420;s[79]=45;s[80]=$0a40;s[81]=135;s[82]=$10c0;s[83]=s[80]; Wait( 20 ); }

   
   
   
2. 組込処理
   
   1. サーボ角設定
      　指定した足の各サーボの角度を設定します。この処理は主にサーボの調整時に使用して上記のキャリブレーションデータを作成するために使います。
      
      void SetLeg( int16_t leg, int16_t angl0, int16_t angl1, int16_t angl2 )
      
      • leg 　<-　設定する足の番号（０～３）
      • angl0 　<-　足の根元のサーボの角度
      • angl1 　<-　足の中華のサーボの角度
      • angl2 　<-　足の先端のサーボの角度
        　　※角度の単位は度
      
      
   2. サーボ角度設定２
      　逆運動学により足の先端の座標から各サーボの角度を設定します。座標の原点はそれぞれの足の根元のサーボの回転軸です。
      
      int16_t AnglCalc( int16_t leg, int16_t x, int16_t y, int16_t z )
      
      • leg 　<- 設定対処の足の番号（０～３）
      • x, y, z　<- つま先の座標
      • return -> 0:正常　else:エラー発生
   
   
   3. 足の移動
      　指定した座標に足の先端を移動させます。内部的に始点座標と終点座標を管理していて移動時間を指定した場合は直前に移動した座標を始点にして引数の座標（終点）まで指定した時間で移動します。
      　割込み処理により20ms毎に始点と終点の内分点の座標を計算しサーボを制御するのでスムーズに動きます。移動が完了すると *tim をゼロにし終点の座標を始点座標にコピーします。
      　この処理を呼出した場合、*timがゼロになるまで他の処理を行うことができます。
      　移動時間指定無し（移動持間＝０）の場合は引数の座標を始点座標にコピーしてサーボを直ぐに引数の座標に動かします。
      
      void MovLeg( int16_t *tim, TYPE_POS *leg0, TYPE_POS *leg1, TYPE_POS *leg2, TYPE_POS *leg3 )
      
      • *tim 　<-> 移動時間指定[ms]
      • leg0 ～ leg3 <- 各足の移動先座標
        因みに TYPE_POS は下記の構造体です。

      typedef struct {
      	int16_t x;
      	int16_t y;
      	int16_t z;
      } TYPE_POS;
      

      
      
   4. 回転変換
      　picle では浮動小数点や三角関数が使えないので組込処理として回転変換処理を用意しました。
      　ロボットの中心を回転軸として引数の座標変数を回転後の値に書き換えます。
      
      void RotCalc( int16_t angle, int16_t *x, int16_t *y )
      
      • angle 　<- 回転角度（度）
      • *x, *y　<-> 回転元座標(本処理で回転後の座標に書き換えられる）
   
   
3. 制御例
   　上記のような組込処理があれば picle言語で大抵の制御はできるはずです。
   　まだ電源が外部供給だったりサーボの配線が乱れた状態なのですが、下の動画のような制御が簡単にできます。
   
   https://www.youtube.com/watch?v=Y1mvErXJsBA&feature=youtu.be

[TOP] [ 前へ ] 連載記事 [ 次へ ]

タグ：サーボ PIC24FJ PICLE ３Dプリンタ ４足ロボット