Здравствуйте. Возникла необходимость определения угла поворота на подвижной платформе. Приобрел вот такой гироскоп http://amperka.ru/product/triple-axis-gyro . По моим представлениям гироскоп выдает угловую скорость по трем осям. В моем случае нужна только одна ось - Z. Расчет угла поворота произвожу численным интегрированием угловой скорости, т.е. замеряю отсчеты времени и на каждой итерации добавляю к имеющемуся углу произведение угловой скорости помноженное на время прошедшее с момента последнего вычисления. Угловую скорость считаю как среднее показание датчика за две итерации (текущая и предыдущая). В качестве эксперимента ввел искусственную задержку от 1 до 30 миллисекунд на каждой итерации. Запускаю программу и поворачиваю гироскоп на 90 градусов. В результате угол, который рассчитывает моя программа зависит от задержки, которую я ввожу, причем зависимость почти что пропорциональная (не совсем конечно). При задержке 1 миллисекунда показывает порядка 1000, при задержке 20 миллисекунд показывает 50. И самое интересное, если после поворота на какой-то угол в одном направлении повернуть датчик обратно, угол становится снова ноль, что намекает о точности датчика. Может быть кто-то сталкивался с подобной проблемой. Да, еще в магазине амперка ссылка на неактуальную библиотеку для гироскопа, она не взаимодействует с текущей версией Wire.h, откуда скачал уже не помню, файлик (.rar) почему-то не прикладывается к сообщению, если кому-то надо могу выслать по почте. Код (Text): #include <Wire.h> #include <ITG3200.h> float x, y, z1, z2, z3; //Угловая скорость и углы ITG3200 gyro = ITG3200(); unsigned long r, q, i; //Моменты времени boolean boo; void setup(void) { //Инициация гироскопа, как работает не понимаю, взял из примера, //который шел вместе с библиотекой Serial.begin(9600); Wire.begin(); delay(1000); gyro.init(ITG3200_ADDR_AD0_LOW); Serial.print("zeroCalibrating..."); gyro.zeroCalibrate(2500, 2); Serial.println("done."); //Заполняю начальные значения переменных boo=true; z2=0.0; z3=0.0; q=0; i=0; } void loop(void) { while (gyro.isRawDataReady()) { //Здесь я определяю самую первую итерацию для первоначального //заполнения переменной r (момент времени предыдущей итерации) if (boo=true){ r=micros(); boo = false; } gyro.readGyro(&x,&y,&z2);//Считываю показания датчика q = micros(); //Определяю момент времени считывания датчика //Так я борюсь с шумом (лучше, пока ничего не придумал) if (abs(z2) <= 0.4) { z2=0.0; } //Рассчитываю угол z3 = z3 + (z2+z1)*float(q-r)/200000.0; //По идее надо делить на 2000000, но тогда угол вообще мимо ++i; //Один раз за 100 итераций (раз в секунду) вывожу в монитор порта if (i==100) { Serial.println(z3); i=0; } r = q; //Запоминаем отсчет времени на предыдущей итерации z1 = z2;//Запоминаем показания датчика на предыдущей итерации delay(20);//Искуственная задержка } }
Мне гироскоп нужен только для определения угла поворота телеги в момент разворота это приблизительно 1-2 сек., надеюсь далеко не уплывет. Вот ребенок (6 класс) делает проект https://yadi.sk/i/LiklkQAkeutxG . Сейчас телега поворачивается по задержке времени, но угол поворота изменчив, как минимум зависит от подстилающей поверхности и заряда батареек. Вообще-то ребенок все делает и программирует сам (даже ящик для земли), я подключаюсь только обучить если уперся, теперь уперся сам, нужна помощь еще более взрослого )))
Переформулирую. Может быть у кого-то есть работающий код для определения угла под эту или другую модель гироскопа.
Нашел ошибку Код (Text): if (boo=true){ r=micros(); boo = false; } А должно было быть Код (Text): if (boo==true){ r=micros(); boo = false; } Очень смешно, неделю просидел над кодом. Несколько библиотек перепробовал, в поисках чужих ошибок .
Кстати, обратил внимание, что при использовании разных библиотек гироскоп выдавал разный уровень случайных отклонений. На каких-то порядка 0.3 градуса в режиме покоя, а на последней скаченной, с сайта производителя до 1 градуса, т.е. более ранняя библиотека дает более точный результат. С чем связано не знаю, возможно из за разной калибровки гироскопа при запуске программы.
