廢材四足機器人（三）一點點的程式設計說明


Stick Hexapod

相較於組裝廢材機器人與足態的決定，程式設計方面是比較簡單的，稍微有點程式設計底子，應該都可以看得懂 Github 上的程式，這邊主要就來解釋一下，程式上我的基本想法是什麼。

伺服馬達的狀態更新

因為只使用了一片 Arduino Uno，沒有使用其他的驅動板，因此，可想而知的，Arduino Uno 本身會很忙，必須逐一更新馬達的角度，我採用單純的做法，無論馬達角度是否有變動，每次都跑 12 個馬達更新：

Servo servos[4][3];

int degOfServo[4][3] = {
  {90, 180, 180}, // LEG3 - Servo 1, 2, 3
  {90, 180, 180}, // LEG4 - Servo 1, 2, 3
  {90, 0,   0},   // LEG2 - Servo 1, 2, 3
  {90, 0,   0}    // LEG1 - Servo 1, 2, 3
};

void initServos() {
  for(int i = 0; i < 4; i++) {
    for(int j = 0; j < 3; j++) {
      servos[i][j].attach(8 + i * 3 + j);
    }
  }
  updateServos();
}

void updateServos() {
  for(int i = 0; i < 4; i++) {
    for(int j = 0; j < 3; j++) {
      servos[i][j].write(degOfServo[i][j]); 
      delayMicroseconds(MICROSECONDS);
    }
  }
}

各個馬達的角度是記錄在 degOfServo 陣列中，編號在〈廢材四足機器人（一）修修補補成廢材〉的圖中有標示，接的 Arduino Uno 是從 D8 到 D19，這麼安排的好處，就如上頭程式中看到的，只要跑迴圈，就可以更新所有馬達狀態。

在示範影片中，四足的動作是特意放慢的，這樣比較容易看清楚動作，如果想要讓它動得快一些，只要修改一下 MICROSECONDS 的值。

在 Commit history 中可看到，之後要讓四足做出動作，經常使用 for 迴圈，後來我就重構出一個 forLoop 函式來做這件事：

void forLoop(int to,  void (*fn)(int), int arg) {
  for(int i = 0; i < to; i++) {
    fn(arg);
  }
}

封裝伺服馬達的角度設定

由於伺服馬達在組裝是，並不是同一個方向，每次都得想著，這顆伺服馬達要轉正幾度，那顆伺服馬達又得轉負幾度，這實在很麻煩，我就寫了些函式，讓垂直方向運動的馬達只有上下之別，而水平方向的馬達只有順時針或逆時針之別：

void joint1_1ClkDataStep(int deg) {
  degOfServo[3][0] += deg;
}

void joint2_1ClkDataStep(int deg) {
  degOfServo[2][0] += deg;
}

void joint3_1ClkDataStep(int deg) {
  degOfServo[0][0] += deg;
}

void joint4_1ClkDataStep(int deg) {
  degOfServo[1][0] += deg;
}

void joint1_2DownDataStep(int deg) {
  degOfServo[3][1] += deg;
}

void joint2_2DownDataStep(int deg) {
  degOfServo[2][1] += deg;
}

void joint3_2DownDataStep(int deg) {
  degOfServo[0][1] -= deg;
}

void joint4_2DownDataStep(int deg) {
  degOfServo[1][1] -= deg;
}

void joint1_3DownDataStep(int deg) {
  degOfServo[3][2] += deg;
}

void joint2_3DownDataStep(int deg) {
  degOfServo[2][2] += deg;
}

void joint3_3DownDataStep(int deg) {
  degOfServo[0][2] -= deg;
}

void joint4_3DownDataStep(int deg) {
  degOfServo[1][2] -= deg;
}

函式的名稱是 joint（關節）加上馬達編號，Data 表示這只是設定馬達角度陣列，還沒實際 write 到馬達，Step 表示以目前既有角度步進，也就是指定的 deg 會與目前角度相加或相減，來做到上下或者順逆時針之運動。

基本的連續技

四足機器人每個動作，實際上都由數個馬達連續做動來組成，例如說單獨只有一隻腿的上昇好了，其實是由兩個關節的馬達轉動組成：

void leg1UpStep(int deg) {
  joint1_2DownDataStep(deg); joint1_3DownDataStep(deg); 
  updateServos();
}

void leg2UpStep(int deg) {
  joint2_2DownDataStep(deg); joint2_3DownDataStep(deg); 
  updateServos();
}

void leg3UpStep(int deg) {
  joint3_2DownDataStep(deg); joint3_3DownDataStep(deg); 
  updateServos();
}

void leg4UpStep(int deg) {
  joint4_2DownDataStep(deg); joint4_3DownDataStep(deg); 
  updateServos();
}

而將整個四足撐起來的動作，就要八個馬達合作：

void hexapodUpStep(int deg) {
  joint1_2DownDataStep(deg); joint2_2DownDataStep(deg); joint3_2DownDataStep(deg); joint4_2DownDataStep(deg);
  joint1_3DownDataStep(deg); joint2_3DownDataStep(deg); joint3_3DownDataStep(deg); joint4_3DownDataStep(deg);
  updateServos();
}

因為我想要機器人的動作看來平順一些，在不透過其他驅動板的情況下，比較好的方式是增減一點角度就更新一次馬達，這會讓機器人的各馬達動作，看來比較像是在「同時」進行，就我目前的作法是，每次 deg 只是指定 1，增加這個值的話，機器人的動作看起來就會比較「粗魯」，無論如何，這邊都還看得到 deg 參數的指定，這是為了要讓機器人動作平順些，還是快速些或粗魯些而做的保留。

其他的函式大概也是這樣的的設計，像是整個機器人身體的轉動，或者是單一腳的轉動：

void hexapodClockwiseStep(int deg) {
  joint1_1ClkDataStep(-deg);
  joint2_1ClkDataStep(-deg);
  joint3_1ClkDataStep(-deg);
  joint4_1ClkDataStep(-deg);
  updateServos();
}

void joint1_1ClkStep(int deg) {
  joint1_1ClkDataStep(deg);
  updateServos();
}

void joint2_1ClkStep(int deg) {
  joint2_1ClkDataStep(deg);
  updateServos();
}

void joint3_1ClkStep(int deg) {
  joint3_1ClkDataStep(deg);
  updateServos();
}

void joint4_1ClkStep(int deg) {
  joint4_1ClkDataStep(deg);
  updateServos();
}

足態的實作程式 V2

如果你仔細看看 Github 中的程式，會發現 Hexapod.ino 內容有點不同，這是因為後來我想到了個可以不用重心轉移就能抬腿的足態 V3 版本，不過，V2 版本還是值得瞭解一下，因為它足態與程式撰寫上都比較簡單，因此前一篇文章與這邊還是先說明一下 V2 的版本，你還是可以在 Commit history 中找到 V2 相關的程式碼：

首先，想將整個身體撐起來，要連續跑 90 次的 hexapodUpStep：

void hexapod(int dir) {
  forLoop(90, hexapodUpStep, dir);
}

如果想讓身體向上，就是 hexapod(UP)，反之則是 hexapod(DOWN)，UP、DOWN 的值分別是 1 與 -1， 這麼呼叫函式，會比 hexapod(1)、hexapod(-1) 清楚些。

真正要讓機器人能抬腿，如〈廢材四足機器人（二）電路連接、靜態平行與足態設計〉中談過的，是對角線的腿與腿間的合作：

void leg1UpDown() {
  forLoop(75, leg4UpStep, -1);
  forLoop(60, leg1UpStep, -1);  
  forLoop(60, leg1UpStep, 1);
  forLoop(75, leg4UpStep, 1);
}

void leg2UpDown() {
  forLoop(75, leg3UpStep, -1);
  forLoop(60, leg2UpStep, -1);  
  forLoop(60, leg2UpStep, 1);
  forLoop(75, leg3UpStep, 1);
}

void leg3UpDown() {
  forLoop(75, leg2UpStep, -1);
  forLoop(60, leg3UpStep, -1);  
  forLoop(60, leg3UpStep, 1);
  forLoop(75, leg2UpStep, 1);    
}

void leg4UpDown() {
  forLoop(75, leg1UpStep, -1);
  forLoop(60, leg4UpStep, -1);  
  forLoop(60, leg4UpStep, 1);
  forLoop(75, leg1UpStep, 1);    
}

轉動的部份就需要四隻腿的合作了：

void hexapodTurn(int dir) {
  forLoop(45, hexapodClockwiseStep, dir);

  leg1Turn(dir);

  if(dir == CLK) {
    leg2Turn(dir);
  } else {
    leg3Turn(dir);
 }    

  leg4Turn(dir);   

  if(dir == CLK) {  
    leg3Turn(dir);
  } else {
    leg2Turn(dir);
  }
}

想要順時針轉動的話，就是 hexapodTurn(CLK)，逆時針轉動就是 hexapodTurn(CT_CLK)，CLK 的值是 1，CT_CLK 的值是 -1，這麼設計也是為了程式的可讀性，上面的函式中有 leg1Turn、leg2Turn、leg3Turn、leg4Turn，都是由一度一度步進來實作出來的：

void leg1Turn(int dir) {
  forLoop(75, leg4UpStep, -1);
  forLoop(60, leg1UpStep, -1);  
  forLoop(45, joint1_1ClkStep, dir);
  forLoop(60, leg1UpStep, 1);
  forLoop(75, leg4UpStep, 1);  
}

void leg2Turn(int dir) {
  forLoop(75, leg3UpStep, -1);
  forLoop(60, leg2UpStep, -1);  
  forLoop(45, joint2_1ClkStep, dir);  
  forLoop(60, leg2UpStep, 1);
  forLoop(75, leg3UpStep, 1);
}

void leg3Turn(int dir) {
  forLoop(75, leg2UpStep, -1);
  forLoop(60, leg3UpStep, -1);  
  forLoop(45, joint3_1ClkStep, dir);  
  forLoop(60, leg3UpStep, 1);
  forLoop(75, leg2UpStep, 1); 
}

void leg4Turn(int dir) {
  forLoop(75, leg1UpStep, -1);
  forLoop(60, leg4UpStep, -1);  
  forLoop(45, joint4_1ClkStep, dir);  
  forLoop(60, leg4UpStep, 1);
  forLoop(75, leg1UpStep, 1);    
}

至於機器人的前進設計上也是類似：

void leg12Turn(int dir) {
  joint1_1ClkDataStep(dir);
  joint2_1ClkDataStep(dir);    
  updateServos();  
}

void leg34Turn(int dir) {
  joint3_1ClkDataStep(dir);
  joint4_1ClkDataStep(dir);    
  updateServos();
}

void moveForward() {
  leg2Turn(CT_CLK);  
  forLoop(45, leg12Turn, CLK);
  leg1Turn(CT_CLK);

  leg4Turn(CLK);   
  forLoop(45, leg34Turn, CT_CLK);
  leg3Turn(CLK);
}

這樣的演算其實都是基於靜態平衡，感覺是有些蠻幹，應該有更好的演算法，之前我在玩 樂高 EV3 時就知道了，做機器人不是我擅長，總是會遇上各種問題，有時一點點設計方向的錯誤，也會讓一切重新來過（就樂高來說就是拆掉重組啦！），這是很令人懊惱的事，感覺就像之前的時間都白費了呢！

不過，樂趣也在於解決問題，就算砍掉重練很令人不甘，不過有時重練出東西來，反而有更大的樂趣，在做這隻四足的過程中，有好幾次我也就差一點想劈了它當柴燒，還好我忍住了… XD

下一個玩具該做什麼呢？…

PS1. 足態 V3 版本與相關程式，就自行研究囉！
PS2. 目前超音波感應器只是裝飾器。
PS3. 你可以試著為它裝上紅外線或藍牙來遙控它。