#define servoPin 2 // PA6

#define EXPECTED_PERIOD 20000 //microseconds
#define NINETY_DEG 1500 // microseconds
#define ZERO_DEG 700 //microseconds
#define HUNDREDEIGHTY_DEG 2300 //microseconds

void setup() {
  // put your setup code here, to run once:
  pinMode(servoPin, OUTPUT);
}

// 0 - 90 - 180 - 90, repeats
void loop() {
  // servo expects 20ms pulses
  int waitTime = 0;
  waitTime = EXPECTED_PERIOD - ZERO_DEG;
  for(int i = 0; i < 50; i++) {
    // servo keeps at 0 degrees for 1 second
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(ZERO_DEG);
    digitalWrite(servoPin, LOW);
    delayMicroseconds(waitTime); 
  }

  waitTime = EXPECTED_PERIOD - NINETY_DEG;
  for(int i = 0; i < 50; i++) {
    // servo keeps at 0 degrees for 1 second
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(NINETY_DEG);
    digitalWrite(servoPin, LOW);
    delayMicroseconds(waitTime); 
  }

  waitTime = EXPECTED_PERIOD - HUNDREDEIGHTY_DEG;
  for(int i = 0; i < 50; i++) {
    // servo keeps at 0 degrees for 1 second
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(HUNDREDEIGHTY_DEG);
    digitalWrite(servoPin, LOW);
    delayMicroseconds(waitTime); 
  }

  waitTime = EXPECTED_PERIOD - NINETY_DEG;
  for(int i = 0; i < 50; i++) {
    // servo keeps at 0 degrees for 1 second
    digitalWrite(servoPin, HIGH);
    delayMicroseconds(NINETY_DEG);
    digitalWrite(servoPin, LOW);
    delayMicroseconds(waitTime); 
  }
}