這是給Pizg Chen與Bridan Wang(http://4rdp.blogspot.tw/2015/12/rosa-18-p-6-x-2-spider.html)六足機器人做出一個動作的簡單程式，六隻腳12個馬達，分別接到servoPin[]指定的12的pin，spiderMove[]共有8個動作，每個動作有13個參數，第一個是該動作的執行時間，後面接著12個馬達的角度。loop()不斷地送出每個馬達的角度，然後休息指定的動作時間，程式非常簡單。

#include <Servo.h>

 

Servo servo[12];

int servoPin[12]={2,3,4,5,6,7,8,9,A0,A1,A2,A3};

 

#define Steps 8

int spiderMove[]  = {

         200, 150,  90,    60,  90,    90,  90,   150,  90,    60,  90,    90,  90, //put leg set 2

         200, 150,  90,    60,  45,    90,  90,   150, 135,    60,  90,    90, 135, //lift leg set 1

         200,  90,  90,    60,  45,    30,  90,   150, 130,   120,  90,    90, 135, //backward leg set 2

         200,  90,  90,   120,  45,    30,  90,    90, 130,   120,  90,    30, 135, //forward leg set 1

         200,  90,  90,   120,  90,    30,  90,    90,  90,   120,  90,    30,  90, //21 put leg set 1

         200,  90,  45,   120,  90,    30,  45,    90,  90,   120, 135,    30,  90, //lift leg set 2

         200,  90,  45,    60,  90,    30,  45,   150,  90,   120, 135,    90,  90, //backward leg set 1

         200, 150,  45,    60,  90,    90,  45,   150,  90,    60, 135,    90,  90, //forward leg set 2 

    //repeat put leg set 2        

};

   

void setup() {

    Serial.begin(9600);

    servoInit();

}

 

void loop() {

    for (int cmd=0; cmd<Steps; cmd++) {

        Serial.print("Command");

        Serial.print(cmd);

        Serial.print(":");

        for (int i=0; i<12; i++) {

            servo[i].write(spiderMove[cmd*13+i+1]);

            Serial.print(spiderMove[cmd*13+i+1]);

            Serial.print(" ");

        }

        delay(spiderMove[cmd*13]);   

        Serial.println(spiderMove[cmd*13]);               

    }

}

 

void servoInit()

{

    // 指定各個馬達使用腳位

    for (int i=0; i<12; i++) {

        servo[i].attach(servoPin[i]);

    }

}

 

arrow
arrow
    全站熱搜

    ghostyguo 發表在 痞客邦 留言(0) 人氣()

    E-mail轉寄