#include<Servo.h>
Servo FS90;
Servo FS90R;

byte FS90Pin = 9;
byte FS90RPin = 10;
String incoming;
String sub_incoming;
bool sens=1;
byte angle = 90;
byte vitesse = 0;

void setup() {
  FS90.attach(FS90Pin, 500, 2500);
  FS90R.attach(FS90RPin, 700, 2300);
  Serial.begin(9600);
  Serial.setTimeout(10);
}

void loop() {
  if (Serial.available() > 0) {
    incoming = Serial.readStringUntil('\n');
    Serial.print("Code ascii envoye: ");Serial.println(incoming);delay(100);
    sub_incoming = incoming.substring(1, 2);
    sens=sub_incoming.toInt();
    Serial.print("sens: ");Serial.println(sens);delay(100);
    sub_incoming =incoming.substring(3, 6);
    vitesse=sub_incoming.toInt();
    Serial.print("vitesse: ");Serial.println(vitesse);delay(100);
    sub_incoming =incoming.substring(7, 10);
    angle=sub_incoming.toInt();
    Serial.print("angle: ");Serial.println(angle);delay(100);
    FS90.write(angle);
    write360(sens, vitesse);
  }
}

void write360(bool sens, byte vitesse) {
  //sens=0: antihoraire, sens=1 horaire
  delay(1); //une pause est nécessaire en simulation avec Tinkercad!!!
  if (sens == 0) FS90R.write(90 + vitesse * 0.9);
  else FS90R.write(90 - vitesse * 0.9);
}