#include <Servo.h>
Servo servo1;
Servo servo2;
int i = 0;
void setup() {
Serial.begin(9600);
servo1.attach(8);
servo2.attach(9);
}
int sweep_time = 1000; // 70 * 50ms = 3500
int head_degrees = 75; // sweep degrees for the head to move
int body_degrees = 65; // sweep degrees for the body to move
int millis_per_head_move = sweep_time / head_degrees;
int millis_per_body_move = sweep_time / body_degrees;
void loop() {
int i = 0; int j = 0;
unsigned long now_millis = 0;
unsigned long start_sweep = millis(); // mark the time when we start the first sweep
while ( millis() - start_sweep < sweep_time ) { // keep moving first sweep until sweep time has elapsed
now_millis = millis() - start_sweep;
if (now_millis % millis_per_head_move == 0 ) servo1.write(i++);
if (now_millis % millis_per_body_move == 0 ) servo2.write(j++);
delay(1);
}
Serial.print(i); Serial.print(" "); Serial.println(j);
start_sweep = millis(); // mark the time when we start the second/back sweep
while ( millis() - start_sweep < sweep_time ) { // keep moving second sweep until sweep time has elapsed
now_millis = millis() - start_sweep;
if (now_millis % millis_per_head_move == 0) servo1.write(i--);
if (now_millis % millis_per_body_move == 0) servo2.write(j--);
delay(1);
}
Serial.print(i); Serial.print(" "); Serial.println(j);
}