這是給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;
}
|
全站熱搜
留言列表
雜項記錄 (3)
