ServoTesterUltimate 1.0

#include <Servo.h>
Servo s;
void setup() {
  Serial.begin(115200);
  Serial.println(F("//servotest//CR|LF//[a <deg>|s <mspdeg>]//"));
  s.attach(9);
}
char buf[64];
char tmp;
uint8_t cur = 0;
char state = 0;
char tmpm;
int opt = 0;
int tmpopt;
long dela = 0;
long diff;
int swstate = 0;
int wrapval;
void loop() {
  while(Serial.available()) {
    buf[cur] = Serial.read();
    Serial.write(buf[cur]);
    if (buf[cur] == 'X' || buf[cur] == '\n'  || buf[cur] == '\r') {
      buf[cur] = '\0';
      sscanf(buf, " %c %i ", &tmpm, &tmpopt);
      switch (tmpm) {
        case 'a':
        state = 0;
        opt = tmpopt;
        Serial.println(F("\nANGLE OK"));
        break;
        case 's':
        state = 1;
        opt = tmpopt;
        dela = millis();
        wrapval = 360*opt;
        Serial.println(F("\nSWEEP OK"));
        break;
        default:
        Serial.println(F("\nOPCOD UNKN"));
      }
      cur = 0;
    } else {
      cur++;
    }
  }
  if (state == 0) {
    s.write(opt);
  } else if (state == 1) {
    diff = millis() - dela;
    if (diff > wrapval) {
      diff -= wrapval;
      dela += wrapval;
    }
    swstate = diff / opt;
    if (swstate > 180) {
      swstate = 360-swstate;
    }
    s.write(swstate);
  }
}