第七步:配置代码
控制刷牙的机械臂工作过程非常简单,归结起来不过是左右运动而已,以下是该系统工作时所需要的代码。
#include
Servo servo[3];
//定义舵机接口
static const int servoPin[3] = {7,8,6};
void setup()
{
Serial.begin(9600);
for(int i=0; i<3; i++){
servo[i].attach(servoPin[i]);
}
//让机器臂在头盔顶部折叠
servo[0].write(90); // base
servo[1].write(30); // upper arm
servo[2].write(150); // lower arm
}
void loop()
{
// 开始刷牙前等待5秒
delay(5000);
// 刷牙准备,机械臂就位
servo[1].write(180);
servo[0].write(90);
servo[2].write(70);
delay(1000);
// 来回刷
for(int i = 0; i<10; i++){
servo[2].write(60);
delay(200);
servo[2].write(120);
delay(200);
}
//完成后再次折叠机械臂
servo[0].write(90);
servo[1].write(30);
servo[2].write(150);
}