arduino文件
#include <Servo.h>
Servo myservo1,myservo2,myservo3,myservo4;
const int SERVOS = 4;
int value[SERVOS], idle[SERVOS], currentAngle[SERVOS], MIN[SERVOS], MAX[SERVOS], INITANGLE[SERVOS], previousAngle[SERVOS],ANA[SERVOS];
char command;
#define ServoDelayTime 50
int delta = 5;
int delta_bottom = 2;
void setup() {
Serial.begin(9600);
pinMode( 8, OUTPUT);
pinMode( 9, OUTPUT);
pinMode( 10, OUTPUT);
pinMode( 11, OUTPUT);
myservo1.attach(2);
myservo2.attach(7);
myservo3.attach(12);
myservo4.attach(13);
MIN[0] = 10;
MAX[0] = 50;
INITANGLE[0] = 30;
MIN[1] = 10;
MAX[1] = 140;
INITANGLE[1] = 90;
MIN[2] = 40;
MAX[2] = 170;
INITANGLE[2] = 90;
MIN[3] = 0;
MAX[3] = 170;
INITANGLE[3] = 90;
myservo1.write(INITANGLE[0]);
myservo2.write(INITANGLE[1]);
myservo3.write(INITANGLE[2]);
myservo4.write(INITANGLE[3]);
currentAngle[0]=INITANGLE[0];
currentAngle[1]=INITANGLE[1];
currentAngle[2]=INITANGLE[2];
currentAngle[3]=INITANGLE[3];
}
void loop()
{
if(Serial.available()>0)
{
command = Serial.read();
Serial.println(command);
}
switch (command)
{
case 'F':
forward();
break;
case 'B':
back();
break;
case 'R':
right();
break;
case 'L':
left();
break;
case 'S':
stop();
Serial.println("S");
break;
case '0':
Serial.println("Servo all stop");
delay(ServoDelayTime);
break;
case '1':
Serial.println("MeArm turn Left");
BottomLeft();
delay(ServoDelayTime);
break;
case '2':
Serial.println("MeArm turn Right");
BottomRight();
delay(ServoDelayTime);
break;
case '3':
Serial.println("Arm A Up");
Arm_A_Up();
delay(ServoDelayTime);
break;
case '4':
Serial.println("Arm A Down");
Arm_A_Down();
delay(ServoDelayTime);
break;
case '5':
Serial.println("Arm B Up");
Arm_B_Up();
delay(ServoDelayTime);
break;
case '6':
Serial.println("Arm B Down");
Arm_B_Down();
delay(ServoDelayTime);
break;
case '7':
Serial.println("Clamp Open");
ClampOpen();
break;
case '8':
Serial.println("Clamp Close");
ClampClose();
break;
}
}
void ClampOpen()
{
myservo1.write(MAX[0]);
delay(300);
}
void ClampClose()
{
myservo1.write(MIN[0]);
delay(300);
}
void BottomLeft()
{
if(currentAngle[3] + delta_bottom < MAX[3])
{currentAngle[3] += delta_bottom;}
myservo4.write(currentAngle[3]);
}
void BottomRight()
{
if(currentAngle[3] - delta_bottom > MIN[3])
currentAngle[3] -= delta_bottom;
myservo4.write(currentAngle[3]);
}
void Arm_A_Up()
{
if(currentAngle[1] + delta < MAX[1])
currentAngle[1] += delta;
myservo2.write(currentAngle[1]);
}
void Arm_A_Down()
{
if(currentAngle[1] - delta > MIN[1])
currentAngle[1] -= delta;
myservo2.write(currentAngle[1]);
}
void Arm_B_Up()
{
if(currentAngle[2] - delta > MIN[2])
currentAngle[2] -= delta;
myservo3.write(currentAngle[2]);
}
void Arm_B_Down()
{
if(currentAngle[2] + delta < MAX[2])
currentAngle[2] += delta;
myservo3.write(currentAngle[2]);
}
void Servo_stop()
{
myservo1.write(currentAngle[0]);
myservo2.write(currentAngle[1]);
myservo3.write(currentAngle[2]);
myservo4.write(currentAngle[3]);
}
void forward()
{
digitalWrite(8, LOW);
digitalWrite(9, HIGH);
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
}
void right()
{
digitalWrite(8, LOW);
digitalWrite(9, HIGH);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
}
void back()
{
digitalWrite(8, HIGH);
digitalWrite(9, HIGH);
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
}
void left()
{
digitalWrite(8, LOW);
digitalWrite(9, LOW);
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
}
void stop()
{
digitalWrite(8, LOW);
digitalWrite(9, LOW);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
}