Нашел хорошую библиотеку в которой есть фильтрация данных с УЗ датчика HC-04SR (CyberMod с сайта cybr-place.ru). В ближайшее время закину картинки графиков с пояснениями и скетч. Скажу лишь что график стал ровнее некуда. В точности есть конечно ньюансы всё таки у датчика есть разница по разного качества поверхностям (глянец, шершавость) но ровная стенка это теперь, на графике который отрисован по координатам Х, У построенному в декартовой системе, ровная прямая!!! Вобщем большего желать пока не смею, начинаю строить платформу ориентирующуюся в пространстве по этому методу.
П.С. Есть недостаток (куда без него) датчик производит несколько измерений, в моем случае 5, а это время! ну например если поставить ограничение на 12000 микросекунд ожидания (это 206см) то только измерения на каждом из углов займут 12*5=60мс. Если тележка медленная как у меня то проблем особых нет, а если быстрая, нужно думать где оставить приоритет по замеру , какую задать скорость и так далее.
продолжаем.
Вложение:
Пространство+симиляр.jpg [ 229.72 КиБ | Просмотров: 3509 ]
Текст тесовой программы. срабатывает с нажатия кнопки повешенной на пин8 и измеряет все вокруг с 15 градусов серво до 165, делает это начиная с 90 градусов и поэтому получается 2 прохода.
Код:
#include <CyberLib.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#define size_buff 5 //размер массива sensor
uint16_t sensor[size_buff]; //массив для хранения замеров дальномера
//**********Переменные*****************************
byte ugol=15;
byte ugol_servo=90, count_range=1, ugol_count=5, but1; /* кнопка запуска измерений but1 1 bit new state 2-bit old stste*/
int shag1=1, shag2=1, range[11], x, y, schet1=1;
// градусы 165 150 135 120 105 90 75 60 45 30 15 //Для расчета координат по Х
int sinus[11]={-124,-111,-91,-64,-33, 0 , 33, 64, 91, 111, 124}; //Значения синуса и косинуса умноженные на 128 чтобы без запятых и можно потом Х и У
// градусы 165 150 135 120 105 90 75 60 45 30 15
byte cosinus[11]={33, 64, 91, 111, 124,128, 124, 111, 91, 64, 33}; //для расчета координат по У
//****Функция рачета расстояния с сортировкой неверных значений из CyberLib**********
uint16_t GetDistance()
{
uint16_t dist;
for (uint8_t i = 0; i < size_buff; ++i) //производим несколько замеров
{
D2_High; delay_us(10); D2_Low; //запустить измерение
dist = pulseIn(3, HIGH, 12000); //считываем длительность времени прохождения эха, ограничить время ожидания
if(dist==0) dist=12000; // 12000 микросекунд = 206 сантиметрам
sensor[i]=dist; //сохранить в массиве
delay_ms(40); //задержка между посылками
}
dist=(find_similar(sensor, size_buff, 58))/58; //фильтруем показания датчика и переводим в см
return dist;
}
Servo motor; // инициализируем серво мотор (добавляем его в программу)
void setup() {
Serial.begin(9600); //скорость серийного порта, для сброса значений на экран компьютера
motor.attach(12); //серво вешаем на 12 ногу
delay(200);
motor.write(ugol_servo); //задано в переменных изначальный угол = 90 градусов
delay (200);
D2_Out; D2_Low; //пин trig ультразвукового сонара
D3_In; //пин echo ультразвукового сонара
pinMode(8,INPUT); // прикрепляем кнопку запуска измерений
delay (2000);
}
void XY_range () {
x=10000+range[ugol_count]*sinus[ugol_count]; // здесь координата хранится в раз 128 больше
// 10000 добавил чтоб координата была положительной... можно не ставить..... эта задача только в зародыше
Serial.print(x);
Serial.print(" ");
y=range[ugol_count]*cosinus[ugol_count]; // здесь координата хранится в раз 128 больше
Serial.print(y);
Serial.print(" ");
Serial.println();
delay (1);
}
void servo_scan_all () {
for (int i=0; i<20; i++) servo_scan();
} // запускаем серву на измерение 90 105 120 и так до 165 потом назад до 15 градусов и снова до 90
void servo_scan () {
if (ugol_servo==165) {ugol=-15; } // если серво дошла до конца то разворачиваем назад
if (ugol_servo==15) {ugol=+15; } // если серво дошла до 15 то разворачиваем назад
if (ugol_count==10) count_range=-1; //здесь храним порядковый номер угла 0-15 1-30 2-45......10-165
if (ugol_count==0) count_range=+1;
/******************проводим 5 измерений подряд чтобы увидеть погрешности измерений***********/
uint16_t dist=GetDistance(); //производим замер дистанции
range[ugol_count]=dist;
delay (3);
Serial.print(ugol_servo);
Serial.print(" ");
Serial.print(dist);
Serial.print(" ");
XY_range();
motor.write(ugol_servo);
delay (50);
ugol_servo+=ugol;
ugol_count+=count_range;
}
void loop() {
but1=bitWrite(but1, 1, digitalRead(8)); //записвываем в первый бит состояние кнопки
if (bitRead(but1,1) && bitRead(but1, 2)) { servo_scan_all();} //проверяем второй бит и сравниваем его с первым
but1=bitWrite(but1, 2, bitRead(but1, 1)); //записываем во второй бит состояние первого, таким образом если кнопка уже нажата, то второй раз измерение не запустится. (хотя читал это здесь и анализировал, где то есть косяк.. но работает!
}
Чуть код покоментировал.
Вход
Регистрация
FAQ
Поиск
, но что-то в этом есть
спасибо =DeaD=
, смотря от чего мерять, учитывать ли внутренности головок и т.д.) В общем и целом выяснилась и так понятная "в догадках" теория. Для ультразвукового датчика камнем преткновения являются углы. при движущейся платформе, с первого раза, без мониторинга значений можно не понять ибо датчик успеет сделать несколько измерений прежде чем возникнет ситуация когда он (робот) сможет столкнуться и одно из значений точно будет верным. Что касается самого эксперимента. Таблицы здесь почему то всегда сбиваются, вставлю картинкой (УЗ-Ультразвуковой цифровой датчик ОПТ-оптический датчик шарп, аналоговый. Дистанция в сантиметрах, измерение "туда" это движение серво от 165 градусов (справа) до 15 градусов (слева) против часовой стрелки, так как конечные углы по 2 раза не сканируются то в этих местах нет значений.) 