# 伺服馬達的狀態更新

``````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);
}
}
}

for(int i = 0; i < 4; i++) {
for(int j = 0; j < 3; j++) {
servos[i][j].write(degOfServo[i][j]);
delayMicroseconds(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;
}
``````

# 基本的連續技

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

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

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

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

``````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);
}
``````

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

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

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

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

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

# 足態的實作程式 V2

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

``````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);
}
}
``````

``````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);
}

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

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

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

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