四自由度机械臂

福州布衣

四自由度机械臂

代码:

//四自由度机械臂程序(三个9克舵机,一个步进电机.  Arduino程序控制.)

#include <Stepper.h>

#include <Servo.h>

//抓手,主臂,次臂三舵机

Servo myservo1;   //抓手

Servo myservo2;   //主臂

Servo myservo3;   //次臂

int pos =0;  

//底盘步进电机

const int stepsPerRevolution=200;

Stepper myStepper(stepsPerRevolution, 8, 10, 9, 11);    //步进电机 Pin

int step=0;

void setup() {

//三舵机Pin

  myservo1.attach(2);          //Pin 2

  myservo2.attach(3);          //Pin 3

  myservo3.attach(4);          //Pin 4

  //三舵机复位

  myservo1.write(0);  

  delay(150);

  myservo2.write(0);

  delay(150);

  myservo3.write(0);

  delay(150);

  myStepper.setSpeed(30);

}

void loop() {

//第一次顺环动作

s1open(20);            //抓手开启

delay(1000);

s2Forward(45);       //主臂向前 45 度

delay(1000);

s1close(20);             //抓手关闭

delay(1000);

s2Backward(45);     //主臂向后 45 度

delay(1000);

s3Forward(45);        //次臂抬升 45 度

Stepper(200);           //步进电机正转 200 步

delay(500);

delay(1000);

//第二次顺环动作

s2Forward(45);      //主臂向前 45 度

delay(1000);

s3Backward(45);     //次臂下降 45 度

delay(1000);

s1open(20);              //抓手开启

delay(1000);

s1close(20);               //抓手关闭

delay(1000);

s2Backward(45);     //主臂向后 45 度

//s3Forward(45);      //次臂抬升 45 度

Stepper(-200);        //步进电机反转 200 步

delay(500);

delay(1000);

}

//抓手开启

void s1open(int anlge) {

for (pos = 0; pos <= anlge; pos += 1) {

myservo1.write(pos);   

delay(30);                       

}

}

//抓手关闭

void s1close(int anlge) {

for (pos = anlge; pos >= 0; pos -= 1) {

myservo1.write(pos);   

delay(30);                       

}

}

//主臂向前

void s2Forward(int anlge) {

for (pos = 0; pos <= anlge; pos += 1) {

myservo2.write(pos);   

delay(30);                       

}

}

//主臂向后

void s2Backward(int anlge) {

for (pos = anlge; pos >= 0; pos -= 1) {

myservo2.write(pos);   

delay(30);                       

}

}

//次臂向前

void s3Forward(int anlge) {

for (pos = 0; pos <= anlge; pos += 1) {

myservo3.write(pos);   

delay(30);                       

}

}

//次臂向后

void s3Backward(int anlge) {

for (pos = anlge; pos >= 0; pos -= 1) {

myservo3.write(pos);   

delay(30);                       

}

}

//步进电机

void Stepper(int stepsPerRevolution) {

myStepper.step(stepsPerRevolution);

delay(500);

}