Arduino.ru

Всем привет. Я студент, работаю над проектом по управлению манипулятором, а именно, работе 4-х сервоприводов, что в совокупности представляет собой "клешню".

Задача состоит в том, чтобы можно было управлять манипулятором через Веб-интерфейс, т.е. клиент выставляет в интерфейсе нужную градусную меру, а прошивка считывала их корректно и поворачивала сервоприводы на указанную градуную меру, и во время выполнения постоянно показывала свое текущее местоположение.

Для осуществления корректного считывания положения, данные углов поворота преобразовывались в формат String, в строку из 20 значений, и добавлялись нули перед градусной мерой, где каждые 5 значений - это угол поворота для одного сервопривода.

Это выглядит так: 00025001170000900180, где 00025 - угол поворота 1 сервопривода, 00117 - угол 2 сервопривода, 00009 - третьего и 00180 - четвертого. Прошивка преобразует эти данные в число Int, убирая лишние 0, и поворачивает манипулятор на нужную градусную меру. 

В приведенном ниже коде, все работает, прошивка принимает данные и поворачивает на нужную градусную меру, однако я не могу осуществить, "Поградусное" считывание положения. Другими словами, мне нужно чтобы скетч, постоянно опрашивал сервоприводы о том, где они находится в момент движения и выводить эти данные в монитор порта, скажем каждые 100 или 10 миллисекунд. 

Пример:

Допустим сервопривод стоит на положении 00025001170000900180 (тот же чото я и указал выше) и на него пришли новые данные, допустим 00021001200001500175, после того как сервоприводы достигнут новых значений - остановить опрос, пока не придут новые. 

Это должно выглядеть так: (я разделю пробелами каждые 5 знаков чтобы вас не запутать) 

00025 00117 00009 00180 (текущее)

00024 00118 00010 00179

00023 00119 00011 00178

00022 00120 00012 00177

00021 00120 00013 00176

00021 00120 00014 00175

00021 00120 00015 00175 (новое положение)

В моем случае в мониторе порта отображается так:

00025 00117 00009 00180 (текущее)

00021 00120 00015 00175 (новое положение)

Вопрос как мне добиться нужного результата???

Вот собственно и код:

#include <Servo.h>

//переменные сервоприводов
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;

// Управления и обратной связи(присвоение пинов)
int servoPin1 = 9;
int servoPin2 = 8;
int servoPin3 = 7;
int servoPin4 = 6;

//переменные угла поворота
int number1;
int number2;
int number3;
int number4;

//Преобразование String в число Int
String inpStrNumber1;
String inpStrNumber2;
String inpStrNumber3;
String inpStrNumber4;

String inpStrNumbers;

//преобразование значение угла поворота в String
String strNumber1;
String strNumber2;
String strNumber3;
String strNumber4;

String strNumbers;

unsigned long previousMillis = 0;
unsigned long period = 100; // 100 миллисекунд


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

myservo1.attach(servoPin1);
myservo2.attach(servoPin2);
myservo3.attach(servoPin3);
myservo4.attach(servoPin4);
}

void loop()
{

if (Serial.available() > 0) {
inpStrNumbers = Serial.readString(); //считывание Стринговского 20-ти символьного числа

inpStrNumber1 = inpStrNumbers.substring(1,5); // разбивка Стринговского на части с 1 по 5
inpStrNumber2 = inpStrNumbers.substring(5,10); // разбивка Стринговского на части с 5 по 10
inpStrNumber3 = inpStrNumbers.substring(10,15); // разбивка Стринговского на части с 10 по 15
inpStrNumber4 = inpStrNumbers.substring(15,20); // разбивка Стринговского на части с 15 по 20

// Преобразование стринговского числа в INT для правильного считывания угла поворота
number1=inpStrNumber1.toInt();
number2=inpStrNumber2.toInt();
number3=inpStrNumber3.toInt();
number4=inpStrNumber4.toInt();

//движение сервоприводов на заданный угол

myservo1.write(number1);
myservo2.write(number2);
myservo3.write(number3);
myservo4.write(number4);

//Преобразование числа в INT в String
//Задаем условия String для первого сервопривода
strNumber1 = String(number1);
if(strNumber1.length() == 1) {
strNumber1 = "0000" + strNumber1;
}
if(strNumber1.length() == 2) {
strNumber1 = "000" + strNumber1;
}
if(strNumber1.length() == 3) {
strNumber1 = "00" + strNumber1;
}
//Задаем условия String для второго сервопривода
strNumber2 = String(number2);
if(strNumber2.length() == 1) {
strNumber2 = "0000" + strNumber2;
}
if(strNumber2.length() == 2) {
strNumber2 = "000" + strNumber2;
}
if(strNumber2.length() == 3) {
strNumber2 = "00" + strNumber2;
}
//Задаем условия String для третьего сервопривода
strNumber3 = String(number3);
if(strNumber3.length() == 1) {
strNumber3 = "0000" + strNumber3;
}
if(strNumber3.length() == 2) {
strNumber3 = "000" + strNumber3;
}
if(strNumber3.length() == 3) {
strNumber3 = "00" + strNumber3;
}
//Задаем условия String для четвертого сервопривода
strNumber4 = String(number4);
if(strNumber4.length() == 1) {
strNumber4 = "0000" + strNumber4;
}
if(strNumber4.length() == 2) {
strNumber4 = "000" + strNumber4;
}
if(strNumber4.length() == 3) {
strNumber4 = "00" + strNumber4;
}

//опрос положенияя кадые 100 милисекунд, я думал что данный опрос решит мою проблему
//но я ошибся
 if(millis()-previousMillis >= period)
  {
   previousMillis = millis(); //запоминаем текущий момент
   strNumbers = strNumber1 + strNumber2 + strNumber3 + strNumber4;
   Serial.println(strNumbers);
   delay(100);
  }
 }
}