Управление серво моторами

как управлять серво моторами по коду,
#include <Servo.h>
#include <SoftwareSerial.h>

// контакты модуля блютуз
#define RXD_PIN 7
#define TXD_PIN 8

// контакты сервоприводов
#define SERVO1_PIN 3
#define SERVO2_PIN 5
#define SERVO3_PIN 6
#define SERVO4_PIN 9
#define SERVO5_PIN 10
#define SERVO6_PIN 11

#define SERVOS_NUM 6 // количество сервоприводов

Servo servo[SERVOS_NUM];
uint8_t servoPin[SERVOS_NUM] = {SERVO1_PIN, SERVO2_PIN, SERVO3_PIN, SERVO4_PIN, SERVO5_PIN, SERVO6_PIN};
SoftwareSerial mySerial(TXD_PIN, RXD_PIN); // RX, TX

void setup() {
Serial.begin(9600);

mySerial.begin(9600);
mySerial.setTimeout(200);

// настройка сервоприводов
for (int i = 0; i < SERVOS_NUM; i++) {
servo[i].attach(servoPin[i]);
delay(10);
servo[i].write(0);
delay(250);
}
}

void loop() {
// парсер
if (mySerial.available()) {
String s = mySerial.readString();
s.trim();
s.toUpperCase();
int i = s.indexOf('=');
if (i != -1) {
String name = s.substring(0, i);
int number = s.substring(i + 1).toInt();
if (name == "S1") {
Serial.println("servo #1 move to " + String(number) + " deg");
servo[0].write(number);
}
if (name == "S2") {
Serial.println("servo #2 move to " + String(number) + " deg");
servo[1].write(number);
}
if (name == "S3") {
Serial.println("servo #3 move to " + String(number) + " deg");
servo[2].write(number);
}
if (name == "S4") {
Serial.println("servo #4 move to " + String(number) + " deg");
servo[3].write(number);
}
if (name == "S5") {
Serial.println("servo #5 move to " + String(number) + " deg");
servo[4].write(number);
}
if (name == "S6") {
Serial.println("servo #6 move to " + String(number) + " deg");
servo[5].write(number);
}
}
}

delay(1);
}