Специальные цены   новые товары
+ Ответить в теме
Страница 4 из 165 ПерваяПервая ... 2 3 4 5 6 14 ... ПоследняяПоследняя
Показано с 121 по 160 из 6569

Создание собственной системы стабилизации

Тема раздела Квадрокоптеры. Общие вопросы в категории Квадрокоптеры и мультироторы; Попробуйте делить на 10.0 и использовать int. Курс без магнитометра никак(...

  1. #121

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    Попробуйте делить на 10.0 и использовать int. Курс без магнитометра никак(
    Вложения

  2.  
  3. #122

    Регистрация
    22.03.2004
    Адрес
    Кемерово
    Возраст
    40
    Сообщений
    2,349
    Записей в дневнике
    18
    используйте int оно от -32768 до 32768, char от -127 до 128
    есть еще тип unsigned он знак игнорирует.

  4. #123

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Цитата Сообщение от RW9UAO Посмотреть сообщение
    используйте int оно от -32768 до 32768, char от -127 до 128
    есть еще тип unsigned он знак игнорирует.
    прикол в следующем если я буду принимать данные с акселерометра в любую другую переменную не char я тупо теряю знак числа(уже экспериментировал) и тогда получается значение от 0 до 255 а в какую сторону он наклонен бог его знает, я уже писал выше что получалось ускорение свободного падения 55 перевернуть 200 нужно всётаки принять в char, а потом конвертировать как-то?



    аксель мне даёт показания в дополнительном коде

  5. #124

    Регистрация
    22.03.2004
    Адрес
    Кемерово
    Возраст
    40
    Сообщений
    2,349
    Записей в дневнике
    18
    если в int положить char, то знак потеряться не должен. в процедуре усреднения можно попробовать так:
    int middle;
    char temp;

    middle = 0;
    for (c=0; c < сколько циклов усреднения; c++){
    temp = процедура чтения акселерометра;
    middle = middle + temp;
    }
    middle = middle / сколько циклов усреднения;
    вот так точно будет работать =)

    если все совсем плохо, можно использовать float для переменной middle

  6.  
  7. #125

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Вообще-то у меня так и сделано, должна быть какая-то процедура перевода типа этой: atoi в С++

    int n;
    char *str = "12345";
    n = atoi(str);
    только вот работает ли это в ардуине? Вечером попробую....

  8. #126

    Регистрация
    16.12.2005
    Адрес
    Москва
    Возраст
    34
    Сообщений
    4,452
    Записей в дневнике
    13
    Цитата Сообщение от SergDoc Посмотреть сообщение
    n = atoi(str)
    Тормозить будет по самое небалуйся

  9. #127

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Надо не забыть фотку модуля скинуть...

  10.  
  11. #128

    Регистрация
    16.12.2005
    Адрес
    Москва
    Возраст
    34
    Сообщений
    4,452
    Записей в дневнике
    13
    Перечитал еще раз сообщения... Вам надо из CHARа 0..255 сделать signed int -128..+127 и все? Тогда это ж элементарно
    char acc;
    int res;
    res=acc-128;

  12. #129

    Регистрация
    22.03.2004
    Адрес
    Кемерово
    Возраст
    40
    Сообщений
    2,349
    Записей в дневнике
    18
    atoi содержится в стандартном С. в ардуине - х.з. на скорость пока внимание не обращайте. кста, а разве char по умолчанию не signed величина?

  13. #130

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Цитата Сообщение от leprud Посмотреть сообщение
    Перечитал еще раз сообщения... Вам надо из CHARа 0..255 сделать signed int -128..+127 и все? Тогда это ж элементарно
    char acc;
    int res;
    res=acc-128;
    в таком случае и char не надо
    int acc;
    int res;
    res=acc-128;
    всё равно больше 255 число я не получу в какую бы переменную я его не загнал

    char signed - величина от-128 до +127 если писать Serial.print(X,DEC); оно мне так и выдаёт уже в десятичной -128 +127 но char хранит символы в отличии от byte - где именно цифирки но беззнаковое, я раньше её и пытался приспособить а вот подумать что для получения знака числа отнять от неё 128 недопёр

    Не не всё так гладко 50 char тоже что и 50 byte, а если теперь отнять 128 получится не 50 и не -50 а ежели платку перевернуть оно покажет в char -50, а в byte 205

    atoi не тормозит калибровка пошла, но вот незадача стало ещё всё хуже
    Последний раз редактировалось SergDoc; 21.06.2011 в 22:42.

  14. #131

    Регистрация
    02.06.2011
    Адрес
    Калуга
    Возраст
    31
    Сообщений
    191
    Дело в том, что отрицательные числа в ЭВМ хранятся в т.н. дополнительном коде. Он получается из нормального кода инвертированием всех разрядов и добавлением единицы.
    Т.е. получается код

    int acc;
    int res;
    if (acc > 127)
    res = ~acc + 1; // если реальная разрядность переменной acc не 8 бит, то res = ~(acc - 128) + 1;
    else
    res = acc;
    Хотя при получении отрицательного числа все вышеперечисленное автоматом делается, так что вроде как достаточно самого простого:
    if (acc > 127)
    res = acc - 128;
    else
    res = acc;

  15. #132

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    C char тоже всё работает без всяких извращений, это я лох, полез дальше разбираться где напортачил

    timer=millis(); - оно надеюсь берёт время от начала программы?

  16. #133

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Ввёл для гироскопов дополнительный фильтр:

    #define FI 0.07 - коэффициент
    //gyros
    int gyroZeroX;//x-axis - откалиброванное значение
    float gyroXadc; // отфильтрованное значение показаний
    float gyronoXadc; // нефильтрованное значение
    float gyroXrate; // реальная я надеюсь угловая скорость
    float gyroXangle; // ну и угол

    ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    gyroXadc=gyroZeroX; // пришлось приравнять перед loop , а то если будет в 0 то полный бред

    }

    void loop()
    {
    timer=millis();//start timing
    dtime=timer-oldtime;
    gyronoXadc = analogRead(gX);

    gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI));
    gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec
    gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование
    показания стали стабильнее, но углы всёравно плывут, надо чёт придумывать с интегрированием...

    Наверно будет лучше поступить так с gyroXrate, а не с gyroXadc ?
    Последний раз редактировалось SergDoc; 23.06.2011 в 00:49.

  17. #134

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    Показания и будут плыть.Это абсолютно нормально, супер результат - 1 градус в минуту.Почти не достижимо. Чуть улучшает ситуацию оверсэмплинг, то есть многократное повторное чтение ADC и последующее усреднение значений, можно увеличить таким образом разрешение. Но для нормальной стабилизации по углу нужно делать фьюжн нескольких сенсоров.
    И не стоит делать так dtime/1000 , dtime/1000.0 позволит избежать трудновылавливаемых багов.

  18. #135

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    соскакивает именно первый просчёт угла - время большое, может попробовать так:

    если dt>100 то угол=угол (тоесть 0) иначе gyroXangle=gyroXangle+gyroXrate*dtime/1000.0; ?

    я просто пока притормозил выполнение перед снятием показаний, время > 600

    далее время <20

  19. #136

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Ну вот, я опять в тупике, расчёты вроде верные, даже на выходе показания приемлимые, программа выполняется за 20 милисекунд, но вот реакция на повороты в пространстве слишком медленная примерно 1-1.5 секунды, придётся пересмотреть концепцию, боюсь что полностью...

  20. #137

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    То есть Вы уже интегрируете и у изменения угла такая большая задержка?А чем меряли? У нас с Вами очень близкие проекты, меня всегда можно найти в
    Skype : ivereninov

  21. #138

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Я слегка погорячился, это у меня так по всей видимости просто Com с Serial monitor подтормаживают было также пару глюков, мог фильтр не стартонуть, сейчас вроде всё более менее, платка лежит уже полчаса, углы считает пока тьфу-тьфу без сбоев вот на данный момент последняя версия расчётов

    Кстати возникла идея на счёт КУКа и акселерометра , считаем угол из показаний акселерометра делим на Dt получаем якобы угловую скорость ну естественно PID для сопоставления данных, применяем альфа-бета фильтр(довольно простой и маленький) - подставляем далее в куковские расчёты и наблюдаем за аппаратом ?

    с куком есть одна проблема все таймеры заняты а при чтении по SPI нужно прерывание таймера
    Последний раз редактировалось SergDoc; 26.06.2011 в 00:34.

  22. #139

    Регистрация
    05.10.2007
    Адрес
    Одинцово
    Возраст
    35
    Сообщений
    4,264
    Cергей а если упростить задачу- сделать стабилизацию по след алгоритму- приемник- аксель- кук, то есть сигнал с приемника подмешивает данные с акселя, а уже туда подмешивает данные с платы кука, то есть у вас получается обычный кук, но с акселем, , да получится громоздко, но это на первое время
    давно такое предлагал сделать для трикоптера, без платы кука
    приемник- аксель- гира на каждый луч. аксель независимо от гироскопов высчитывает уплывание горизонта и подмешивает сигнал, а гироскопы отрабатывают мгновенные колебания. здесь так же можно сделать только вместо гир на кждый луч- будет плата кука.
    система в будущем получится модульной с удобно настройкой

  23. #140

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    А как реализуется определение углов от акселя при наличии линейных ускорений? Просто при сумме по всем осям = g ?

  24. #141

    Регистрация
    05.10.2007
    Адрес
    Одинцово
    Возраст
    35
    Сообщений
    4,264
    а как напряжение на выходах у акселя и гир перевести в углы? в даташитах же написано какое напряжение на выходах акселя при определенных углах, у гир напряжение зависит от ускорения, как все это перевести в математику я не знаю

  25. #142

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    ну вопервых, напряжение на аналоговых выходах - индикатор измеряемого параметра который можно померять и перевести в понятные для математики цифирки, а вот дальше считать, считать и считать
    гироскопы:
    gyronoXadc = analogRead(gX); //считали данные с выхода гироскопа

    gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI)); // ну тут слегка фильтронули
    gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec собственно угловая скорость= (показания - нулевыепоказания)*2.4
    gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование угол = угол предыдущий + угловая скорость * делта t (время между двумя цыклами расчёта)

    акселерометр:

    R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//расчёт длинны вектора)
    accXangle = acos(accXval/R)*RAD_TO_DEG-90; // расчёт угла между вектором и осью X акселерометра arccos(проекция вектора ускорения свободного падения/ длинна вектора)* перевод из радиан в градусы
    accYangle = acos(accYval/R)*RAD_TO_DEG-90;

    о как раз в программном лажу обнаружил пойду переделывать, а то блин если оба угла с акселерометра равны нулю то все расчёты летят к чертям ...

  26. #143

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Слабо гироскопы местами перепутать

  27. #144

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    так а как у Вас реализована защита от наличия линейных ускорений?Они же будут "портить" угол.

  28. #145

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Основные расчёты берутся именно с гироскопов, а для коррекции показания с акселерометра именно через альфа-бета фильтр,

    compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
    compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
    его ещё комплиментарным называют,

    если же имеются именно линейные ускорения это даже хорошо допустим аппарат смещается по какой-то оси без крена и тангажа, в любом случае на данное смещение нужно противодействие в данном случае опять же изменение угла которого на самом деле то и нет, тоесть для противостояния порыву ветра аппарат же надо наклонить в противоположном направлении, как-то так, всё равно бьюсь над съёмом показаний ненравятся они мне (именно сами показания)...

    надо наверно гироскопы запитать всё-таки 5ю вольтами и переделать общение по i2c с акселерометром

    о а нафига я два раза угод с гир расчитываю
    Последний раз редактировалось SergDoc; 27.06.2011 в 13:32.

  29. #146

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    попробова считать углы с акселя через atan2

    accXangle = atan2(accZval,accXval)*RAD_TO_DEG+135;
    accYangle = atan2(accZval,accYval)*RAD_TO_DEG;
    почему-то так только получается

    похоже аксель я доканал - глючит
    Последний раз редактировалось SergDoc; 27.06.2011 в 15:33.

  30. #147

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    чёт с переменными нето знак я всётаки гдето теряю

  31. #148

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Ну вот последнее на чём остановился, по крайней мере углы правдоподобные

    accXangle = -atan2(-accXval,-accZval)*RAD_TO_DEG;
    accYangle = -atan2(-accYval,-accZval)*RAD_TO_DEG;
    теперь буду каким-то образом прошивку добивать (управление, шымы, настройки и т.п.)...

  32. #149

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    а теперь представьте, что коптер резко начинает изменять высоту.Что будет с углом от акселя?

  33. #150

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    перевернётся, я уже модуль ввожу для Z, остаётся только -90 +90 сейчас воюю с ним

    У меня оказывается и стол шатается и переворачивается даже

    Сейчас решаю проблему
    либо коэффициент силы трения лыж с асфальтьм слишком высокий, либо коэффициент моего интеллекта слишком низкий

    покрайней мере уже в чужом коде разбиратся начал...

  34. #151

    Регистрация
    28.09.2005
    Адрес
    санкт петербург
    Возраст
    23
    Сообщений
    667
    Записей в дневнике
    2
    Мне кажется разумным следующий аглоритм.
    1.Проверяем что сумма ускорений по всем осям=g.
    2.Если так, то обнуляем ошибку интегрирования от гироскопов.

    Вопрос лишь в вероятности ситуации, когда сумма ускорений будет g , но коптер будет ускоряться..надо посчитать..

  35. #152

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    Цитата Сообщение от ivereninov Посмотреть сообщение
    Вопрос лишь в вероятности ситуации, когда сумма ускорений будет g , но коптер будет ускоряться..надо посчитать..
    Падать боком както так, или крутится волчком и снижатся...

    я вот чё немогу понять:
    i2c_ADXL345_getRawADC();
    accADC[PITCH] = ((rawADC_ADXL345[1]<<8) | rawADC_ADXL345[0]);
    accADC[ROLL] = - ((rawADC_ADXL345[3]<<8) | rawADC_ADXL345[2]);
    accADC[YAW] = - ((rawADC_ADXL345[5]<<8) | rawADC_ADXL345[4]);
    это из вия все переменные rawADC unsigned как там чё получается


    а углы както так
    Axz = atan2(RxEst,RzEst) + a;
    Ayz = atan2(RyEst,RzEst) + b;
    где a и b это получается угловые скорости (если я правильно понял) с гир...

    интересно следующее - я могу отключить в акселе g как факт тогда он меряет ускорения по осям без учёта g вот если бы наоборот
    Последний раз редактировалось SergDoc; 29.06.2011 в 01:43.

  36. #153

    Регистрация
    07.06.2008
    Адрес
    Москва
    Возраст
    52
    Сообщений
    1,836
    Подскажите как получается горизонт (вертикаль), что то не могу понять.

  37. #154

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    а вот с аркосинусами и вектором беда - в какойто момент времени просто отказывается считать углы уже через альфа бета фильтр, и непойму почему, вектор отказывается считать хотя расчёты ускорений идут...

    Цитата Сообщение от alextr Посмотреть сообщение
    вертикаль
    всмысле по курсу? пока никак нечем отследить кроме гироскопа - только если по угловой скорости пока как в КУКе, а так магнитометр нужен

    а если хе крен и тангаж то atan2 возврашает значение угла между горизонтом и осью по которой считаеи X или Y

  38. #155

    Регистрация
    07.06.2008
    Адрес
    Москва
    Возраст
    52
    Сообщений
    1,836
    Не раньше было как: выставка горизонта на земле т.е. ноль(сейчас инициализация), потом от этого ноля и плясали. Вопрос, если случайным образом подбросить объект с гироскопами и акселерометрами (не делая перед этим инициализацию), мы будем знать где низ?

  39. #156

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    не так колибровка то зачем, я ж с ней столько времени угрохал на третьей странице как раз на земле нулевые показания от них и пляшу

  40. #157

    Регистрация
    07.06.2008
    Адрес
    Москва
    Возраст
    52
    Сообщений
    1,836
    А за горизонт гироскопы отвечают? То есть мы запоминаем их значение когда объект на земле выставлен в горизонт?

  41. #158

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    да

  42. #159

    Регистрация
    07.06.2008
    Адрес
    Москва
    Возраст
    52
    Сообщений
    1,836
    А можно качественно сравнить точность акселерометров и точность гироскопов, может вместо гироскопов использовать шесть акселерометров? Это позволит избавиться от дрейфа гироскопа, но будет накапливаться ошибка интегрирования.

  43. #160

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,461
    Записей в дневнике
    8
    интегрируем как раз угловую скорость от гир дабы получить угол, а с акселерометрами тоже не всё так просто, хотя есть проект квадрик с магнетометром трёхосевым и акселерометром
    Код:
    #include <Wire.h> //Library for I2C implementation using analog pins 4 (SDA) and 5
    #include <MegaServo.h>
    #define NBR_SERVOS 4 // the maximum number of servos (up to 12 on Arduino Diecimil
    #define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
    #define SECOND_SERVO_PIN 9
    #define THIRD_SERVO_PIN 10
    #define FOURTH_SERVO_PIN 11
    MegaServo Servos[NBR_SERVOS] ; // max servos
    //Accelerometer variables
    float ax,ay,az; //Acceleration values in m/s^2 for all three axis
    const float as=0.00488; //Arduino analog resolution = 5Volts/1024 (10bits)
    const float zg=1.63; //Sensor zero g bias = Supply Voltage/2 =3.3/2 (V)
    const float sR=0.3; //Sensor resolution (V/g)
    //Compass variables
    int compassAddress = 0x42 >*> 1; // compass I2C slave adress: 0x42
    // because the wire.h library only uses the 7 bits most significant bits
    // a shift necessary is to get the most significant bits.
    int psi = 0; // Compass heading = yaw (direct readings from the compass in degrees)
    float psirad; // Compass heading = yaw in radians
    //Euler angles Roll and pitch
    float phi; // (rad)
    float theta; // (rad)
    //Kalman filter data
    const float A[6][6] = {
    {1.0000,0.0000,0.0000,-0.5985,-0.0000,0.0000},
    {0.0000,1.0000,-0.0000,-0.0000,-0.5985,0.0000},
    {-0.0000,-0.0000,1.0000,-0.0000,-0.0000,-0.0479},
    {0.0500,0.0000,0.0000,0.3384,-0.0000,-0.0000},
    {0.0000,0.0500,-0.0000,-0.0000,0.3384,0.0000},
    {-0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
    };//(Kalman state-space matrix Ak)
    const float B[6][10] = {
    {-0.0000,-0.0188,-0.0000,0.0188,0.0000,0.0302,-0.0000,0.0031,-0.0000,-0.0000},
    {0.0194,0.0006,-0.0194,-0.0006,-0.0302,-0.0000,-0.0000,-0.0000,0.0031,-0.0000},
    {0.0009,-0.0009,0.0009,-0.0009,-0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
    {-0.0000,0.0002,-0.0000,-0.0002,0.0000,0.0334,-0.0000,0.0034,-0.0000,0.0000},
    {0.0005,0.0006,-0.0005,-0.0006,-0.0334,-0.0000,-0.0000,-0.0000,0.0034,-0.0000},
    {0.0000,-0.0000,0.0000,-0.0000,0.0000,0.0000,0.0000,0.0000,-0.0000,0.0853}
    }; //(Kalman state-space matrix Bk)
    float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed, Yaw speed, Roll angle, Pitch angle, Yaw angle
    float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed),w2,w3,w4,ax,ay,az,phi,theta,yaw
    float At[6] = {0,0,0,0,0,0}; //temp variable
    float Bt[6] = {0,0,0,-0.01,-0.04,0}; //temp variable
    //LQR gain matrix
    const float K[4][6] = {
    {0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
    {-7.1030,0.0000,-20.4163,-18.2839,0.0000,-14.4659},
    {0.0000,-7.1030,20.4163,0.0000,-18.2839,14.4659},
    {7.1030,0.0000,-20.4163,18.2839,0.0000,-14.4659}
    };
    float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^-1)
    float w0; //Throttle to the motors (from joystick)
    //Motor inputs for PWM signal
    float wc1=0;
    float wc2=0;
    float wc3=0;
    float wc4=0;
    //PWM signals
    int p1 = 200;
    int p2 = 200;
    int p3 = 200;
    int p4 = 200;
    //Reference
    float yr[6]={0,0,0,0,0,0};
    void setup()
    {
    Wire.begin();
    Serial.begin(9600); // Initialize serial communications with setup
    Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1 - North - Clockwise rotation
    Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2 - East - Counterclockwise rotation
    Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3 - South - Clockwise rotation
    Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4 - West - Counterclockwise rotation
    //Put the motors in full stop
    Servos[0].write(0);
    Servos[1].write(0);
    Servos[2].write(0);
    Servos[3].write(0);
    }
    void loop()
    {
    // get the most recent acceleration values for all three axis
    ax = ((analogRead(0)*as-zg)/sR)*9.81; //acceleration x-axis (m.s^-2)
    ay = ((analogRead(1)*as-zg)/sR)*9.81; //acceleration y-axis (m.s^-2)
    az = ((analogRead(2)*as-zg)/sR)*9.81; //acceleration z-axis (m.s^-2)
    ax=-ax; //This correction allows the accelerometer to provide positive acceleration in the x-axis direction
    // Compass task 1: connect to the HMC6352 sensor
    Wire.beginTransmission(compassAddress);
    Wire.send(’A’); // The ascii character "A" tells the compass sensor to send data
    Wire.endTransmission();
    // Compass task 2: wait for data processing
    delay(50); // Compass datasheet says we need to wait at least 6000 microsegundos (0.006s) for data processing in the sensor
    //We can also take this opotunity to implement the sampling time (20Hz)
    // Compass task 3: Request heading
    Wire.requestFrom(compassAddress, 2); // request 2 bytes of data
    // Compass task 4: Get heading data
    if(2 <= Wire.available()) // if 2 bytes are available
    {
    //16bit numbers can be broken in two 8 bit chunks. The first is called high byte and the second low byte
    psi = Wire.receive(); // get high byte
    psi = psi <*< 8; // shift high byte
    psi += Wire.receive(); // get low byte
    psi /= 10; // comment this line if you wish to get the heading in decidegrees instead of degrees
    }
    psirad = deg2rad((float)psi); // Map the compass reading from 0 to 359 degrees to -pi radians to pi radians
    phi=atan(ay/az); //Calculate roll angle from the accelerations provided by the accelerometer (rad)
    theta=-atan(ax/az); //Calculate pitch angle from the accelerations provided by the accelerometer (rad)
    //Perform Kalman filtering
    At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x[5];
    At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x[5];
    At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x[5];
    At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x[5];
    At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x[5];
    At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x[5];
    //Get throttle from the joystick
    if (Serial.available() > 0)
    {
    // read the incoming byte:
    w0 = map((float)Serial.read(), 0, 100, 0, 500);
    w0=constrain(w0,0,500); //Limit maximum velocity to 600 rad/s
    Serial.flush();
    }
    else
    {
    w0 = 0;
    }
    //place known inputs and measurements in the input vector u
    u[0]=wc1-w0;
    u[1]=wc2-w0;
    u[2]=wc3-w0;
    u[3]=wc4-w0;
    u[4]=ax-yr[0];
    u[5]=ay-yr[1];
    u[6]=az-yr[2];
    u[7]=phi-yr[3];
    u[8]=theta-yr[4];
    u[9]=psirad-yr[5];
    Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4];
    Bt[0]=Bt[0]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
    Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]
    Bt[1]=Bt[1]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
    Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]
    Bt[2]=Bt[2]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
    Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]
    Bt[3]=Bt[3]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
    Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]
    Bt[4]=Bt[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
    Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]
    Bt[5]=Bt[5]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
    //Get estimated states x(k+1)=A.x(k)+B.u(k)
    x[0]=At[0]+Bt[0];
    x[1]=At[1]+Bt[1];
    x[2]=At[2]+Bt[2];
    x[3]=At[3]+Bt[3];
    x[4]=At[4]+Bt[4];
    x[5]=At[5]+Bt[5];
    //Controlo LQR
    w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5];
    w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5];
    w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5];
    w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5];
    //Adjust speeds with the throttle
    wc1=w0-w1;
    wc2=w0-w2;
    wc3=w0-w3;
    wc4=w0-w4;
    //Calculate and send pulse width to the motors
    p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer function gain) + Dead-zone pulse width
    p2=(int)((wc2/1.8693)+1262);
    p3=(int)((wc3/2.0018)+1255);
    p4=(int)((wc4/1.9986)+1255);
    if(w0==0) //Forçar paragem
    {
    p1=400;
    p2=400;
    p3=400;
    p4=400;
    }
    Servos[0].write(p1);
    Servos[1].write(p2);
    Servos[2].write(p3);
    Servos[3].write(p4);
    // output states
    Serial.print(x[0]);
    Serial.print(" ");
    Serial.print(x[1]);
    Serial.print(" ");
    Serial.print(x[2]);
    Serial.print(" ");
    Serial.print(x[3]);
    Serial.print(" ");
    Serial.print(x[4]);
    Serial.print(" ");
    Serial.print(x[5]);
    Serial.println("");
    }
    float deg2rad (float x) //function that receives an angle between 0 and 359 degrees and resturns the same angle between -pi
    and pi radians
    {
    if(x>=0 && x<=180)
    {
    x=-x*3.14/180;
    }
    else
    {
    x = (360-x)*3.14/180;
    }
    return x;
    }
    непомню уже где нашол...

+ Ответить в теме

Похожие темы

  1. Система стабилизации гиро+акселерометр
    от Фантомас в разделе Полеты по камере, телеметрия
    Ответов: 32
    Последнее сообщение: 25.01.2011, 14:47
  2. Продам Продам Клона Trex 450SEV2 + Аппаратура + Запчасти+ система стабилизации RTF
    от omegapraim в разделе Барахолка. Вертолеты
    Ответов: 1
    Последнее сообщение: 12.01.2011, 18:16
  3. Продам Трёхосевую систему стабилизации Turnigy V-Bar 600
    от avi@tor в разделе Барахолка. Аппаратура
    Ответов: 1
    Последнее сообщение: 08.11.2010, 13:02
  4. Продам Gaui система стабилизации GU365, дёшево.
    от avi@tor в разделе Барахолка. Вертолеты
    Ответов: 3
    Последнее сообщение: 03.08.2010, 11:13
  5. Системы стабилизации
    от max815 в разделе Фото и видеосъемка, системы стабилизации
    Ответов: 16
    Последнее сообщение: 11.03.2010, 03:14

Метки этой темы

Ваши права

  • Вы не можете создавать новые темы
  • Вы не можете отвечать в темах
  • Вы не можете прикреплять вложения
  • Вы не можете редактировать свои сообщения