舵機(3.3V~6V)信號線接 6,7,8,9 (不需要PWM)
搖桿(5V)接A0~A3
搖桿的測試標準可能要試一下
#include <Servo.h>
const int SERVOS = 4;
int PIN[SERVOS],value[SERVOS],idle[SERVOS],currentAngle[SERVOS],MIN[SERVOS],MAX[SERVOS],INITANGLE[SERVOS];
Servo myservo[SERVOS];
int anaPins[]={A0,A1,A2,A3};
int DELAYTIME = 200;
void setup() {
Serial.begin(9600);
//夾子
PIN[0] = 6;
MIN[0] = 10;
MAX[0] = 40;
INITANGLE[0] = 30;
//左臂
PIN[1] = 7;
MIN[1] = 20;
MAX[1] = 160;
INITANGLE[1] = 90;
//又臂
PIN[2] = 8;
MIN[2] = 20;
MAX[2] = 160;
INITANGLE[2] = 90;
//底座
PIN[3] = 9;
MIN[3] = 10;
MAX[3] = 170;
INITANGLE[3] = 90;
for(int i = 0; i < SERVOS; i++){
pinMode(anaPins[i], INPUT);
myservo[i].attach(PIN[i]);
myservo[i].write(INITANGLE[i]);
delay(DELAYTIME);
myservo[i].detach();
value[i] = 0;
idle[i] = 0;
}
}
void loop(){
for(int i = 0; i < SERVOS; i++){
value[i] = analogRead(anaPins[i]);
currentAngle[i] = myservo[i].read();
if(value[i] < 400) {
idle[i] = 0;
if(currentAngle[i] > MIN[i]){
--currentAngle[i];
}
if(!myservo[i].attached()){
myservo[i].attach(PIN[i]);
}
myservo[i].write(currentAngle[i]);
}else if (value[i] > 600) {
idle[i] = 0;
if(currentAngle[i] < MAX[i])
++currentAngle[i];
if(!myservo[i].attached()){
myservo[i].attach(PIN[i]);
}
myservo[i].write(currentAngle[i]);
}else{
++idle[i];
}
if(idle[i] > 100){
myservo[i].detach();
idle[i] = 0;
}
}
}