Тестирование системы.
В ходе разработки алгоритмов стабилизации скорости были реализованы два различных метода стабилизации, которые затем были протестированы. Результаты тестирования позволяют сравнить теоретические данные с реальными, а также выявить эффективность каждого метода.
Так же были получены новые знания и навыки для работы с мобильной платформой Rover 5, разработки алгоритмов стабилизации и программировании Arduino.
Приложение
Код программы пропорциональной стабилизации скорости
#include <SimpleTimer.h>dirPin1 = 9; //пин направления 1-го двигателяpvmPin1 = 10; //пин шим-а 1-го двигателяdirPin2 = 7; //пин направления 2-го двигателяpvmPin2 = 6; //пин шим-а 2-го двигателяencoderPinA = 2; //пин считывающий сигналы энкодера 1 (они вызывают прерывание)encoderPinB = 3; //пин считывающий сигналы энкодера 2 (они вызывают прерывание)time_1; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 1-го энкодераspe_1; //итоговое число импульсов за период с 1-го энкодераtime_2; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 2-го энкодераspe_2; //итоговое число импульсов за период со 2-го энкодера
SimpleTimer timer; //конструктор (см библиотеку SimpleTimer)
String S = ""; //строка считанного числа скоростиU = 0; //заданная скорость в процентах (U = от 0% до 100%)MaxEnk = 10; //значение энкодера при U = 100%a = 2; //число изменения ШИМP = 200; //максимально допустимое число ШИМT = 50; //период подсчитывания числа импульсов с энкодеровshim1 = 0; //уровень ШИМ 1-го двигателяshim2 = 0; //уровень ШИМ 2-го двигателяenk = 0; //число импульсов с энкодера для установления нужной скоростиsetup()
{ .begin(9600); //скорость обмена данными с COM портом(pvmPin1, OUTPUT); //настройка портов на прием или передачу данных
pinMode(dirPin1, OUTPUT);(pvmPin2, OUTPUT);(dirPin2, OUTPUT);(encoderPinA,INPUT);(encoderPinB,INPUT);(0, encoder_1, RISING); //настройка прерывания 0-номер прерывания, которому соответствует порт 2 encoder_1 процедура вызываемая прерыванием.
attachInterrupt(1, encoder_2, RISING); //RISING - режим прерывания означающий что прервание возникнет при переходе от низкого уровня сигнала к высокому.setInterval(T, timer_1); //настройка таймера: после T мс запускать процедуру timer_1(dirPin1,LOW); //направление 1-го двигателя(dirPin2,LOW); //направление 2-го двигателя
}loop()
{.run(); //запуск таймера(Serial.available() > 0) //если в буфере есть символы
{inChar = Serial.read(); //считать символ
if (isDigit(inChar)) //если символ является цифрой
{+= (char)inChar; //прибавить символ к строке
}(inChar == '!') //символ окончания строки
{= S.toInt(); //перевод string в integer
S = ""; //очистка строки
}
//ограничения в заданной скорости(U>100) U = 100;(U<0) U = 0;
//алгоритм стабилизации скорости= round((MaxEnk/100)*U); //необходимое число импульсов на энкодере для установления нужной скорости(enk == 0) //если число импульсов равно 0
{ //ШИМ равен 0= 0;= 0;
}
{
//определение ШИМ для 1-го двигателя(spe_1<enk) shim1 = shim1+a;
if (spe_1>enk) shim1 = shim1-a;
if (shim1>P) {shim1 = P;} //ограничение ШИМ(shim1<0) {shim1 = 0;}
//определение ШИМ для 2-го двигателя
if (spe_2<enk) shim2 = shim2+a;(spe_2>enk) shim2 = shim2-a;
if (shim2>P) {shim2 = P;} //ограничение ШИМ(shim2<0) {shim2 = 0;}
}
//установление уровней ШИМ для двигателей
analogWrite(pvmPin1, shim1);(pvmPin2, shim2);
}
void encoder_1()
{_1+=1; //счетчик прерываний (числа импульсов с 1-го энкодера)
}encoder_2()
{_2+=1; //счетчик прерываний (числа импульсов с 2-го энкодера)
}timer_1() //подпрограмма, запускаемая каждые T мс
{_1 = time_1; //число импульсов с 1-го энкодера _1 = 0; //обнуление счетчика прерываний_2 = time_2; //число импульсов с 2-го энкодера_2 = 0; //обнуление счетчика прерываний
//вывод данных.print(U); //вывод заданной скорости %.print(" ");.print(enk); //вывод расчётного числа импульсов с энкодера для установления нужной скорости.print(" ");.print(spe_1); //вывод числа импульсов с 1-го энкодера.print(" ");.print(spe_2); //вывод числа импульсов с 2-го энкодера.print(" ");.print(shim1); //вывод уровня ШИМ для 1-го двигателя.print(" ");.println(shim2); //вывод уровня ШИМ для 2-го двигателя
}
Код программы стабилизации скорости по отклонению
#include <SimpleTimer.h>dirPin1 = 9; //пин направления 1-го двигателяpvmPin1 = 10; //пин шим-а 1-го двигателяdirPin2 = 7; //пин направления 2-го двигателяpvmPin2 = 6; //пин шим-а 2-го двигателяencoderPinA = 2; //пин считывающий сигналы энкодера 1 (они вызывают прерывание)encoderPinB = 3; //пин считывающий сигналы энкодера 2 (они вызывают прерывание)time_1; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 1-го энкодераspe_1; //итоговое число импульсов за период с 1-го энкодераtime_2; //счетчик прерываний(каждый импульс с энкодера вызывает прерывание) 2-го энкодераspe_2; //итоговое число импульсов за период со 2-го энкодера
SimpleTimer timer; //конструктор (см библиотеку SimpleTimer)
String S = ""; //строка считанного числа скоростиU = 0; //заданная скорость в процентах (U = от 0% до 100%)MaxEnk = 8; //значение энкодера при U = 100%a = 50; //коэф. изменения ШИМP = 200; //максимально допустимое число ШИМT = 50; //период подсчитывания числа импульсов с энкодеровK; //вычисляющийся коэф. (К = от 0 до 1)shim1 = 0; //уровень ШИМ 1-го двигателяshim2 = 0; //уровень ШИМ 2-го двигателяenk = 0; //число импульсов с энкодера для установления нужной скоростиsetup()
{ .begin(9600); //скорость обмена данными с COM портом(pvmPin1, OUTPUT); //настройка портов на прием или передачу данных
pinMode(dirPin1, OUTPUT);(pvmPin2, OUTPUT);(dirPin2, OUTPUT);(encoderPinA,INPUT);(encoderPinB,INPUT);(0, encoder_1, RISING); //настройка прерывания 0-номер прерывания, которому соответствует порт 2 encoder_1 процедура вызываемая прерыванием.
attachInterrupt(1, encoder_2, RISING); //RISING - режим прерывания означающий что прервание возникнет при переходе от низкого уровня сигнала к высокому.setInterval(T, timer_1); //настройка таймера: после T мс запускать процедуру timer_1(dirPin1,LOW); //направление 1-го двигателя(dirPin2,LOW); //направление 2-го двигателя
}loop()
{.run(); //запуск таймера(Serial.available() > 0) //если в буфере есть символы
{inChar = Serial.read(); //считать символ
if (isDigit(inChar)) //если символ является цифрой
{+= (char)inChar; //прибавить символ к строке
}(inChar == '!') //символ окончания строки
{= S.toInt(); //перевод string в integer
S = ""; //очистка строки
}
}
//ограничения в заданной скорости
if (U>100) U = 100;(U<0) U = 0;
}encoder_1()
{_1+=1; //счетчик прерываний (числа импульсов с 1-го энкодера)
}encoder_2()
{_2+=1; //счетчик прерываний (числа импульсов с 2-го энкодера)
}timer_1() //подпрограмма, запускаемая каждые T мс
{_1 = time_1; //число импульсов с 1-го энкодера _1 = 0; //обнуление счетчика прерываний_2 = time_2; //число импульсов с 2-го энкодера_2 = 0; //обнуление счетчика прерываний
//алгоритм стабилизации скорости= round((MaxEnk/100)*U); //необходимое число импульсов на энкодере для установления нужной скорости(enk == 0) //если число импульсов равно 0
{ //ШИМ равен 0= 0;= 0;
}
{
//определение ШИМ для 1-го двигателя(spe_1<enk) //если число импульсов с 1-го энкодера меньше заданного
{= enk-spe_1;= K/enk*a;
shim1 = round(shim1+K); //увеличение ШИМ для 1-го двигателя
}(spe_1>enk) //если число импульсов с 1-го энкодера больше заданного
{= spe_1-enk;= K/spe_1*a;
shim1 = round(shim1-K); //уменьшение ШИМ для 1-го двигателя
}(shim1>P) shim1 = P; //ограничение ШИМ(shim1<0) shim1 = 0;
//определение ШИМ для 2-го двигателя(spe_2<enk) //если число импульсов с 2-го энкодера меньше заданного
{= enk-spe_2;= K/enk*a;
shim2 = round(shim2+K); //увеличение ШИМ для 2-го двигателя
}(spe_2>enk) //если число импульсов с 2-го энкодера больше заданного
{= spe_2-enk;= K/spe_2*a;
shim2 = round(shim2-K); //уменьшение ШИМ для 2-го двигателя
}(shim2>P) shim2 = P; //ограничение ШИМ(shim2<0) shim2 = 0;
}
//установление уровней ШИМ для двигателей
analogWrite(pvmPin1, shim1);(pvmPin2, shim2);
}