六足機器人 – 接收馬達控制指令

這是給Pizg Chen與Bridan Wang(http://4rdp.blogspot.tw/2015/12/rosa-18-p-6-x-2-spider.html)六足機器人的程式，可接收馬達控制指令，控制馬達的角度。指令的格式為 $MmmAaaa#，mm是馬達編號 (00~11)，aaa是馬達角度 (000~180)，例如 $M00A120# 會令馬達0轉動到120度的位置。

#include <Servo.h> #define   CmdBufferLength        12 #define   NumberOfServo          12 Servo servo[NumberOfServo]; int servoPin[NumberOfServo]={2,3,4,5,6,7,8,9,A0,A1,A2,A3}; int servoAngle[NumberOfServo]={90,90,90,90,90,90,90,90,90,90,90,90}; char cmdBuffer[CmdBufferLength]; int  cmdLength=0;   void setup() {     Serial.begin(9600);     servoInit(); } void loop() {     if (getCommand()) {            for (int i=0; i<NumberOfServo; i++) {             servo[i].write(servoAngle[i]);         }     }  } void servoInit() {     // 指定各個馬達使用腳位     for (int i=0; i<12; i++) {         servo[i].attach(servoPin[i]);     } } int getCommand() {     while (Serial.available()) {         byte inByte = Serial.read();         if (inByte=='$') { // Start Of Command             cmdLength=0;         }         cmdBuffer[cmdLength++] = inByte;         if (inByte=='#') {  // End Of Command             if (parseCommand()>0) {                 //Serial.println("parse ok");                 return 1;  // success             } else {                 //Serial.println("parse fail");                 return 0;//fail             }         }     }     return 0; } int parseCommand() {     int retCode=0;         if ((cmdBuffer[1]=='M') && (cmdBuffer[4]=='A') && (cmdBuffer[8]=='#'))     {         // Command : Motor Angle         int motorID = (cmdBuffer[2]-'0')*10 + (cmdBuffer[3]-'0');         servoAngle[motorID] = (cmdBuffer[5]-'0')*100 + (cmdBuffer[6]-'0')*10 + (cmdBuffer[7]-'0');         retCode=1;     }     cmdLength=0; //reset     for (int i=0; i<CmdBufferLength; i++)         cmdBuffer[i]=0;     return retCode; }