http://4rdp.blogspot.com/2015/11/rosa-13-servo.html?m=0
前文談過單一伺服馬達驅動,本期續談多顆伺服馬達多工同步控制方法,
// (C) 2015, Bridan Wang, CC BY-NC-SA 3.0 TW
// http://4rdp.blogspot.tw/search/label/ROSA%20(Arduino)
byte servo_set[] = { 0, 90, 90, 90, 90};
byte servo_speed[] = { 0, 5, 1, 5, 5};
byte servo_hold[] = { 0, 0, 0, 0, 0};
void PROCESS_SERVO(void) {
static byte servo_no;
byte i;
if (servo_no == 0 || servo_no > 4) {
servo_no = 4;
}
if (servo_set[servo_no] > servo_angle[servo_no]){
servo_angle[servo_no] += servo_speed[servo_no];
if (servo_set[servo_no] < servo_angle[servo_no])
servo_angle[servo_no] = servo_set[servo_no];
SERVO_SET(servo_no);
}
else if (servo_set[servo_no] < servo_angle[servo_no]){
servo_angle[servo_no] -= servo_speed[servo_no];
if (servo_angle[servo_no] > 180)
servo_angle[servo_no] = servo_set[servo_no];
SERVO_SET(servo_no);
}
else if (servo_hold[servo_no] != 0){
if (servo_hold[servo_no]!=255)
servo_hold[servo_no]--;
SERVO_SET(servo_no);
}
servo_no--;
}
unsigned long now;
void SYS_TIMER() {
static byte tm_10ms = 0;
static byte tm_100ms = 0;
static byte tm_1sec = 0;
while (millis() - now >= 5) {
now += 5;
tm_10ms++;
tm_100ms++;
tm_1sec++;
PROCESS_SERVO();
}
if (tm_1sec >= 200) {
tm_1sec -= 200;
PROCESS_1sec();
}
if (tm_100ms >= 20) {
tm_100ms -= 20;
PROCESS_100ms();
}
if (tm_10ms >= 2) {
tm_10ms -= 2;
PROCESS_10ms();
}
}
void PROCESS_1sec(void){ // print analog value for debugging
Serial.print("A0 - ");
Serial.println(A_IN(A0));
Serial.print("A1 - ");
Serial.println(A_IN(A1));
Serial.print("A2 - ");
Serial.println(D_IN(A2));
Serial.print("A3 - ");
Serial.println(A_IN(A3));
Serial.print("A4 - ");
Serial.println(A_IN(A4));
Serial.print("A5 - ");
Serial.println(D_IN(A5));
}
void PROCESS_100ms(void){
}
void PROCESS_10ms(void){
if (A_IN(A4) < 200){ // right X
if (servo_set[2] > 120) servo_set[2]--;
}
else if (A_IN(A4) >= 800){
if (servo_set[2] < 160) servo_set[2]++;
else servo_hold[2] = 255;
} else
servo_hold[2] = 0;
if (A_IN(A0) < 200){ // left Y
if (servo_set[4] > 60) servo_set[4]--;
}
else if (A_IN(A0) >= 800) {
if (servo_set[4] < 150) servo_set[4]++;
}
if (A_IN(A1) < 200){ // left X
if (servo_set[1] != 0) servo_set[1]--;
}
else if (A_IN(A1) >= 800){
if (servo_set[1] < 180) servo_set[1]++;
}
if (A_IN(A3) < 200){ // right Y
if (servo_set[3] > 60) servo_set[3]--;
}
else if (A_IN(A3) >= 800) {
if (servo_set[3] < 150) servo_set[3]++;
}
}