2016年12月22日 星期四

六足機器人 – 做出一個最簡單的動作




這是給Pizg ChenBridan Wang(http://4rdp.blogspot.tw/2015/12/rosa-18-p-6-x-2-spider.html)六足機器人做出一個動作的簡單程式,六隻腳12個馬達,分別接到servoPin[]指定的12pin
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]);
    }
}

沒有留言:

張貼留言