Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Радиоуправляемый танк на Arduino Nano | Каталог самоделок Мультикоптеры

Ardu remote: очень простая и дешёвая аппаратура управления своими руками

Всем привет. Захотелось мне однажды маленькую удобную аппаратуру для дрона/крыла; usb-джойстик для симуляторов и Open.HD; найти оправдание построенной дельте (3d-принтеру) и пострадать ардуино. Сразу фото итога:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Размышления и гуглеж привели к выбору BetaFPV Lite radio как образца для внешнего вида. Хотелось, конечно, Taranis X-Lite, но стики были только крупные, от подаренной товарищем бесполезной ввиду древности аппаратуры. Переключатели на 60% были использованы от неё же.
Изначально делал только USB-версию. Оно отлично работало в симуляторе FPV Freerider и как управление в Open.HD линке, но мысль прилепить QCZEK и автономное питание уже овладела мной бесповоротно.

В итоге вышло что вышло.

→ Гитхаб

Видео:

На гитхабе скетч, схема, используемые библиотеки и всякое полезное, включая 3d-модели и исходники для solid.

Схема относительно проста и требует только пайки проводов за исключением резистивного делителя и светодиодов. Работу с qczek lrs я обойду стороной, для этого есть сайт разработчика и неплохие видеотуториалы в ютубе от Юлиана и Сани areyouroo.

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Можно ограничиться только usb-версией без питания и lrs, в качестве lrs можно использовать другие проекты, где принимают на вход CPPM 8/10ch сигнал. При выборе QCZEK появляется выбор между 0.1 и 1вт модулями и частотами 433/868/915 МГц. Я не рекомендую 433, наводки от них дикие. Также не увидел смысла в 1вт, при 100мвт вполне реально пульнуть на 10км. Если планируете покупать антенны — смотрите на версии 915мгц и готовые антенны для популярных дальнобоек frsky. Недорого и какая-то гарантия попадания в приемлемый КСВ. Я же просто подпаял к хвосту с ipex два отрезка медной проволоки D0.4mm, длиной 8.21см и заложил это всё в корпус антенны прямо в середине печати на 3д-принтере. Побегал с аппаратурой по этажам многоквартирного дома — обрыва или падения rssi на модели в квартире не добился, устроило.

Для Open.HD (usb-версия) пока нет смысла в свичах sw4 и sw5, так как число каналов ограничено 8-ю. Возможно, в дальнейшем это ограничение будет снято. Имейте ввиду, что подключение к Open.HD с модулем зарядки даёт дополнительную нагрузку на usb-порт наземной raspberry по питанию. Имеет смысл заюзать хаб с внешним питанием.

Калибровка аппаратуры: подаём питание с зажатой SW_CALIBRATE при средних положениях yaw/roll/pitch и минимальных throttle/aux1; ждём гудка; двигаем всё с осями от минимума до максимума, пока есть пищание. Когда пищать уже не может — выключаем, калибровка завершена.

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

Каждые 30 секунд, если значения не изменились — короткий ненавязчивый писк. Естественно, все это легко изменить/отключить в скетче. Например, просто закоментировав дефайн.

В общем, заготовка для творчества вполне себе ничего. Нет триммирования, но оно и не нужно для полетов с полётным контроллером, а без него эта аппаратура врядли может пригодится.

Также я прилепил блютус-модуль jdy-30 к qczek и гоню в него мавлинк телеметрию, которую принимает DroidPlanner 2.8 или Telemetry Viewaver на андроид-смартфоне. Наверное, также засуну и mavlink-display. Пусть будет красиво, как у дорогих crossfire xD

Спасибо за внимание, жду вопросов и повторений / развитий сего рукожопства.

Добавляем радиоуправление

Управлять будем с телефона под android.

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

При запуске приложения попросим пользователя выбрать к кому подключаться, bt-модуль должен быть уже сопряжен с телефоном (стандартный код 1234).

BluetoothAdapter bluetooth;
String []boundedItems;
protected static final int RECIEVE_MESSAGE = 1;

@Override
protected void onCreate(Bundle savedInstanceState) {
		//...
		bluetooth = BluetoothAdapter.getDefaultAdapter();		
		if(bluetooth != null){
			if (!bluetooth.isEnabled()) {
			    bluetooth.enable();
			}
		}
		Set<BluetoothDevice> bounded=bluetooth.getBondedDevices();
		boundedItems=new String[bounded.size()];
		int i=0;
		for (BluetoothDevice bluetoothDevice : bounded) {
			boundedItems[i  ]=bluetoothDevice.getName();
		}
		
		showListDialog();		
		//...
}

public void showListDialog(){
		AlertDialog.Builder builder = new AlertDialog.Builder(this);

		builder.setTitle("Pick a device");
		builder.setItems(boundedItems, new DialogInterface.OnClickListener() {

		   public void onClick(DialogInterface dialog, int item) {
		        connectTo(item);
		   }

		});

		AlertDialog alert = builder.create();

		alert.show();
	}


После выбора устройства подключимся к нему:

	private static final UUID MY_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
	BluetoothSocket btSocket;

	public void connectTo(int id){
		Set<BluetoothDevice> bounded=bluetooth.getBondedDevices();
		for (BluetoothDevice bluetoothDevice : bounded) {
			if(bluetoothDevice.getName().equalsIgnoreCase(boundedItems[id])){
				try {
					btSocket=bluetoothDevice.createRfcommSocketToServiceRecord(MY_UUID);
					btSocket.connect();
					ct=new ConnectionThread(btSocket);
					ct.start();
				} catch (IOException e) {
					e.printStackTrace();
					try {
						btSocket.close();
					} catch (IOException e1) {
						e1.printStackTrace();
					}
					showListDialog();
				}
				return;
			}
		}
		
	}

После подключения стартует поток, который занимается коммуникацией между приложением и роботом:

private class ConnectionThread extends Thread{
	private final InputStream mmInStream;		
	private final BufferedReader br;
        private final OutputStream mmOutStream;
      
        public ConnectionThread(BluetoothSocket socket) throws IOException {
            mmInStream =  socket.getInputStream();
            br=new BufferedReader(new InputStreamReader(mmInStream));
            mmOutStream = socket.getOutputStream();
            
        }
      
        public void run() {
            while (true) {                    
                                try {
                			String line=br.readLine();
					h.obtainMessage(RECIEVE_MESSAGE, line).sendToTarget();
				} catch (IOException e) {
					e.printStackTrace();
				}
            }
        }
        public void sendCmd(char cmd){
        	try{
        		mmOutStream.write(cmd);
        	}catch (IOException e) {
			e.printStackTrace();
		}
        }
}

Поток посылает сообщение основному потоку приложения через Handler, который определяется так:

h = new Handler() {
      public void handleMessage(android.os.Message msg) {
      switch (msg.what) {
            case RECIEVE_MESSAGE:                                                
            	String line=(String)msg.obj;
            	try{
            		float a=Float.parseFloat(line.trim());
            		balancerView.setAngle((float) (a-Math.PI/2.f));		      
            	}catch (Exception e) { }	            	
            break;
	}
      };
};

balancerView это потомок класса SurfaceView, он занимается выводом на экран текущего положения робота.Вот его метод перерисовки:

public void draw(Canvas canvas) {
	Paint paint=new Paint();
	paint.setStrokeWidth(3);
	canvas.save();
	canvas.rotate((float) (angle*180.f/Math.PI), cx, cy);		
		
	paint.setColor(Color.BLACK);        
	canvas.drawRect(cx-15, cy-150, cx 15, cy, paint);
	paint.setColor(Color.WHITE);        
	canvas.drawRect(cx-12, cy-147, cx 12, cy-3, paint);
        
	paint.setColor(Color.BLACK);        
	canvas.drawCircle(cx, cy, 30, paint);
	paint.setColor(Color.WHITE);        
	canvas.drawCircle(cx, cy, 25, paint);
     
	canvas.restore();		
}


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

	@Override
	public boolean onTouch(View v, MotionEvent me) {
		if(me.getAction()==MotionEvent.ACTION_UP){
			ct.sendCmd('c');
			return false;
		}
		if(v==wB){
			ct.sendCmd('w');
		}else if(v==aB){
			ct.sendCmd('a');
		}else if(v==sB){
			ct.sendCmd('s');
		}else if(v==dB){
			ct.sendCmd('d');
		}
		return false;
	}

Драйвер двигателя sn754410ne

Для управления двигателями я использовал драйвер SN754410NE. Я применил его, потому что он у меня был, но вы можете использовать другой, например L293.

Смотрите про коптеры:  Радиоуправляемый синий краулер Zegan Rock Rover 1:18 2.4G - ZG-C1801 - купить в Rc-like

Теперь о подключении драйвера к Arduino Uno. Все выводы GND (4,5,12,13) подключите к GND макетной платы.  Контакты драйвера  1 и 16 подключите к выводам 9 и 10 Arduino. Контакты драйвера 2 и 7 подключите к контактам 3 и 4 Arduino, это контакты управления левого двигателя.

Контакты драйвера 10 и 15 подключите к выводам 5 и 6 Arduino, это контакты управления правого двигателя.  Контакты 3 и 6 подключите к левому мотору, а контакты 14 и 11 к правому. Контакты   8 и 16 должны быть подключены к питанию на макетной плате. Источник питания: 9В батарея.

Как и чем управлять

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

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Итак, если мы хотим установить привод в крайнее левое положение нужно слать импульсы длительностью 0,9мс с интервалом 20мс, если в крайнее правое — длительность 2,1мс, интервал тот же, ну со средними положениями аналогично. Как оказалось, регуляторы скорости управляются аналогично. Те, кто в теме скажут что это

, который реализовать на любом микроконтроллере — плевое дело. Вот и я так решил, купил в местном магазине сервомашинку и склепал на макетке для нее так называемый сервотестер на ATtiny13. И тут оказалось, что ШИМ не совсем простой, а с подводными камнями.

Как видно из вышеприведенной диаграммы, скважность (отношение длительности импульса к длительности периода) от 5% до 10% (в дальнейшем я за крайние положения принимаю импульсы длительностью 1,0мс и 2,0мс) для 256-значного ШИМ счетчика ATtiny13 это соответствует значениям от 25 до 50.

Но это при условии, что на заполнение счетчика уйдет 20мс, а на деле так не получится и для частоты 9,6МГц и предделителя 1024 нужно ограничить счетчик значением 187(ТОР), в таком случае у нас получится частота 50,134Гц. В большинстве (если не во всех) сервомашинок нету точного генератора опорной частоты и поэтому частота управляющего сигнала может немного плавать.

Если оставить ТОР счетчика 255, то частота управляющего сигнала будет 36,76Гц — на некоторых приводах оно будет работать (возможно с глюками), но далеко не на всех. Итак, теперь у нас 187-значный счетчик, для него 5-10% соответствуют значениям от 10 до 20 — всего 10 значений, немного дискретно получится. Если думаете поиграть с тактовой частотой и предделителем ниже привожу сравнительную табличку для 8-битного ШИМа:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Я не думаю, что для китайской сервомашинки есть существенная разница в 600 и 1200 значений, поэтому вопрос с точностью позиционирования можно считать закрытым.

Математическая модель

Я не буду приводить вывод уравнений движения, все таки это третий курс института. Для тех же, кому интересен вывод, в конце статьи ссылка, где он описан подробнее.

Систему представим в следующем виде:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Маятник это масса mp прикрепленная на конце невесомого стержня длины l. На другой конец стержня прикреплен двигатель, развивающий максимальный момент Mk и передающий его на колесо массой mw и радиусом r. Задача управления — стабилизировать маятник в вертикальном положении и возвращать в начальное положение колесо.

Уравнения движения, описывающие обратный маятник, представимы в следующем виде:
Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Многоканальное управление

С одной сервомашинкой разобрались, но для самолета их нужно минимум три и еще регулятор скорости. Решение «в лоб» — взять микроконтроллер с четырьмя каналами 16-битного ШИМ, но такой контроллер будет стоять дорого и, скорее всего, займет много места на плате.

Второй вариант — запилить программный ШИМ, но занимать процессорное время — это тоже не вариант. Если снова посмотреть на диаграммы сигнала, то 80% времени он не несет никакой информации, поэтому рациональнее было бы ШИМом задавать только сам импульс 1-2мс.

Почему скважность изменяется в таких узких пределах, ведь проще было бы и формировать и считывать импульсы со скважностью хотя бы 10-90%? Зачем нужен тот неинформативный кусок сигнала занимающий 80% времени? Я заподозрил, что, возможно, эти 80% могут занимать импульсы для других исполнительных механизмов, а потом этот сигнал разделяется на несколько разных.

То есть, в периоде длительностью 20мс могут уместится 10 импульсов длительностью 1-2мс, затем этот сигнал каким-то демультиплексором разделяется на 10 различных с длительностью периода как раз 20мс. Сказано — сделано, нарисовал в PROTEUS такую схемку:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

В роли демультиплексора — 74HC238, на его вход E подаются импульсы с выхода микроконтроллера. Эти импульсы — ШИМ с периодом 2мс (500Гц) и скважностью 50-100%. У каждого импульса своя скважность, обозначающая состояние каждого канала. Вот так выглядит сигнал на входе Е:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Для того, чтобы 74HC238 знал на какой выход подать текущий сигнал используем PORTC микроконтроллера и входы A, B, C демультиплексора. В результате на выходах получаем такие сигналы:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок


Сигналы на выходе получаются правильной частоты (50Гц) и скважности (5-10%). Итак, нужно генерировать ШИМ частотой 500Гц и заполнением 50-100%, вот табличка для настройки предделителя и ТОР 16-битного счетчика:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Интересно, что возможное количество значений ШИМа ровно в 1000 раз меньше частоты таймера.

Передающая часть

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

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

Смотрите про коптеры:  Как мы приготовились к UAV Challenge 2016 / Хабр

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

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Число в нижнем левом углу — контрольная сумма. Передвигая ползунки на компе двигались рули на самолете! Вообщем отладил я все это и стал думать о пульте ДУ, купил для него вот такие джойстики:

image

Но потом меня посетила одна мысль. В свое время я тащился от всяких авиасимуляторов: «Ил-2 Штурмовик», «Lock On», «MSFSX», «Ка-50 Черная Акула» и др. Соответственно был у меня джойстик Genius F-23 и решил я прикрутить его к вышеописанной проге с ползунками. Погуглил как это реализовать, нашел этот

и получилось! Управлять самолетиком с помощью полноценного джойстика, мне кажется, гораздо круче, чем маленькой палочкой на пульте. Вообщем все вместе изображено на первой фотке — это нетбук, джойстик, преобразователь на FT232, и подключенный к нему передатчик HM-T868.

Питание

Arduino питается от батарейки 9В через соответствующий разъём. Для питания двигателей я использовал 4 батарейки размера D и соответствующий разъём. Для питания двигателей подключите провода от держателя к плате с SN754410NE.

Программная реализация

Для ATmega8 с тактовой частотой 16МГц в AtmelStudio6 все реализуется следующим образом: вначале задефайним значения счетчика для крайних положений сервомашинок:

#define LOW 16000U
#define HIGH 32000U

затем инициализируем генератор ШИМа на таймере/счетчике1:

OCR1A = HIGH; //Устанавливаем ТОР
TCCR1A = 0<<COM1A1 | 0<<COM1A0 | 1<<COM1B1 | 0<<COM1B0 | 0<<FOC1A | 0<<FOC1B | 1<<WGM11 | 1<<WGM10; //Запускаем неинвертированный Fast PWM на выходе OC1B с верхним значением счетчика, которое записанно в OCR1A
TCCR1B = 0<<ICNC1 | 0<<ICES1 | 1<<WGM13 | 1<<WGM12 | 0<<CS12 | 0<<CS11 | 1<<CS10; //предделитель 1
TIMSK = 1<<OCIE1A | 1<<OCIE1B | 0<<TOIE1; //Разрешаем прерывания по совпадению


Остается реализовать прерывания:

Пуск!

Итак, есть самолетик, есть радиоуправление — Поехали!(с) Первый полет производился над асфальтом, результат — сломанный пополам фюзеляж и полувырванный двигатель. Второй полет производился над более мягкой поверхностью:

Последующие полетов 10 были тоже не особо удачными. Основной причиной я считаю сильную дискретность джойстика — по крену он выдавал только 16 значений (вместо возможных 256), с осью тангажа — не лучше. Но так как в результате испытаний самолет был значительно поврежден и не подлежит ремонту:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Радиоуправляемый танк на arduino nano | каталог самоделок

Данная статья предлагает познакомиться с технологией создания радиоуправляемой игрушки, ставшей довольно популярной в последнее время.

Для ее изготовления необходимы следующие комплектующие:

  • Arduino Nano;
  • шасси,
  • сервоприводы в количестве 3 штук,
  • система поворотов,
  • игрушечный пистолет,
  • джойстик типа PS2,
  • соответствующий модели джойстика приемник,
  • аккумуляторные батарейки,
  • корпус для аккумуляторов,
  • провода,
  • лазерная указка.

Радиоуправляемый танк на Arduino NanoГотовое шасси уже оснащено двумя двигателями, парой редукторов, переключателем, отсеком для помещения двух аккумуляторных батареек. Приобретение такого готового шасси выгоднее изготовленного самостоятельно.

Радиоуправляемый танк на Arduino NanoПервоочередной задачей является закрепление приемника от джойстика на шасси.

С этой целью демонтируйте крышку.

Радиоуправляемый танк на Arduino NanoОсвободить от крышки следует и редуктор.

Радиоуправляемый танк на Arduino NanoДалее проделайте на крышке два отверстия, используемые, в последующем, для монтажа посредством винтов.

Радиоуправляемый танк на Arduino Nano
Зажатые винтами гайки следует залить клеем для предотвращения их раскрутки в процессе езды и падения в редуктор.

Радиоуправляемый танк на Arduino Nano

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

Радиоуправляемый танк на Arduino Nano

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

Радиоуправляемый танк на Arduino Nano

Радиоуправляемый танк на Arduino Nano

Радиоуправляемый танк на Arduino Nano

Приведите после этого поворотную систему в исходное состояние.

Радиоуправляемый танк на Arduino Nano

Радиоуправляемый танк на Arduino Nano

Радиоуправляемый танк на Arduino Nano

Установите собранную систему на танковом корпусе.

Радиоуправляемый танк на Arduino Nano

Проделайте в корпусе еще три отверстия, два из которых необходимы для проложения проводов двигателя, а третье, более широкое в диаметре, – для расположенной в управлении драйвера двигателя шины.

Радиоуправляемый танк на Arduino Nano

Радиоуправляемый танк на Arduino Nano

Пришла очередь пистолета, предназначенного для дула танка. Отпилите его нижнюю часть.
Радиоуправляемый танк на Arduino Nano
На сервоприводе и пистолетном корпусе проделайте по одному отверстию. Используя винт, соедините пистолет с сервоприводом.
Радиоуправляемый танк на Arduino Nano
На сервоприводе и пистолетном корпусе проделайте по одному отверстию. Используя винт, соедините пистолет с сервоприводом.
Радиоуправляемый танк на Arduino Nano | Каталог самоделок

На курке пистолета и насадке сервопривода также просверлите отверстия. Посредством отрезка проволоки соедините между собой указанные выше элементы.

Радиоуправляемый танк на Arduino Nano

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

Радиоуправляемый танк на Arduino Nano

Осуществите программирование имеющейся у вас платы Arduino Nano.
Радиоуправляемый танк на Arduino Nano
Следуя приведенной ниже схеме, произведите сборку оставшихся компонентов устройства.

Радиоуправляемый танк на Arduino Nano

На верхней части готового шасси установите фрагменты линейки – будущие крылья танка.

Установите на них предназначенные для аккумуляторов отсеки.

Радиоуправляемый танк на Arduino NanoС помощью термоклея присоедините лазер к дулу.

Радиоуправляемый танк на Arduino NanoРадиоуправляемый танк готов к эксплуатации.

,

Реализация в железе

Сам каркас робота это алюминиевые профиля 12мм и 14мм, они входят друг в друга. Соединены заклепками. Электроника прикреплена на кусок стеклотекстолита в форме буквы T. Моторы так же прикреплены через стеклотекстолитовый переходник.

Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Их крутящий момент 2,2кг*см или 0.2Нм. Исходя из симуляции нам нужно гораздо больше, поэтому были выбраны другие моторы:
Радиоуправляемый танк на Arduino Nano | Каталог самоделок
ссылка на производителя
Максимальный крутящий момент 14кг*см или 1.4Нм. Тока они потребляют до 5A, поэтому популярный у ардуинщиков L293D тут не подойдет.

Для определения угла и угловой скорости используется IMU — гироскоп и акселерометр. У меня завалялась плата с гироскопом L3G и акселерометром с магнетометром LSM303. Подобных плат очень много и я не стану приводить код получения значений сенсоров. Однако показания датчиков нужно отфильтровать, так как гироскоп постоянно уходит, а акселерометр шумит и сильно врет, если робот начинает двигаться не меняя угла.

float lastCompTime=0;
float filterAngle=1.50;
float dt=0.005;

float comp_filter(float newAngle, float newRate) {

dt=(millis()-lastCompTime)/1000.0;
float filterTerm0;
float filterTerm1;
float filterTerm2;
float timeConstant;

timeConstant=0.5;

filterTerm0 = (newAngle - filterAngle) * timeConstant * timeConstant;
filterTerm2  = filterTerm0 * dt;
filterTerm1 = filterTerm2   ((newAngle - filterAngle) * 2 * timeConstant)   newRate;
filterAngle = (filterTerm1 * dt)   filterAngle;
lastCompTime=millis();
return filterAngle;
}

Работает не идеально, но достаточно хорошо для данной задачи.

Смотрите про коптеры:  GPS антенна своими руками за 5 минут / Хабр

Следующий сенсор — квадратурный энкодер на моторе. Он генерирует прямоугольные импульсы на 2х своих выводах:
Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Считать их можно либо прерываниями, либо считыванием значений в цикле. На arduino playground есть замечательная статья с примерами кода.

Осталось получить угловую скорость колеса. Тут на помощь приходит школьная формула пройденное расстояние/затраченное время.

#define ToPhiRad(x) ((x)*0.00280357142)

timer_old = timer;
timer=millis();
G_Dt = (timer-timer_old)/1000.0;    
dPhi=(ToPhiRad(encoder0Pos)-lastPhi)/G_Dt;

ToPhiRad переводит количество тиков энкодера в угол колеса, мой энкодер выдает около 2240 тиков на оборот. Чтобы получить угол нужно умножить тики на 2 Пи и разделить на их количество при полном обороте колеса.

Показания сенсоров поступают в LQR регулятор:

float K1=0.1,K2=0.29,K3=6.5,K4=1.12;

long getLQRSpeed(float phi,float dphi,float angle,float dangle){
  return constrain((phi*K1 dphi*K2 K3*angle dangle*K4)*285,-400,400);
}

Коэффициенты взяты из Matlab, правда для большей стабильности я подправил 2 первых коэффициента.

Мой драйвер, вернее его библиотека, принимает значения от -400 до 400. Я предположил, что на 400 он выдает на мотор 12В, т.е. мотор развивает максимальный момент (1.4Нм). Разделив 400 на 1.4 получаем коэффициент перевода из Нм, которые выдает LQR, в значения, понятные драйверу.

Просто стабилизировать робота в одной точке не очень интересно, поэтому к нему добавился BT-модуль HC-05. Модуль подключается к серийному порту микроконтроллера. Он работает на 3.3В, а arduino на 5В, поэтому подключать принимающий вход модуля надо через делитель напряжения. Вот схема подключения:
Радиоуправляемый танк на Arduino Nano | Каталог самоделок
Во время цикла микроконтроллер опрашивает модуль на предмет символов:

float phiDif=0.f;
float factorDif=0.f;

float getPhiAdding(float dif){ //сколько прибавлять к углу колес для движения вперед-назад

  if(dif<200 && dif>-200){return 0.f;}
  float ret = dif*0.08;

  return ret;
}

float getFactorAdding(float dif){//сколько добавлять к управлению для поворота
  if(dif<200 && dif>-200){return 0.f;}
  float ret = dif/500*20;
  return ret;
}
//========
if (Serial.available()){
    BluetoothData=Serial.read();
     if(BluetoothData=='w'){   
      phiDif=200;
     } else if(BluetoothData=='s'){ 
      phiDif=-200;
     } else if(BluetoothData=='a'){   
      factorDif=200;
     } else if(BluetoothData=='d'){   
      factorDif=-200;
     } else if(BluetoothData=='c'){   
      factorDif=0;
      phiDif=0;
     }
    }


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

    encoder0Pos =getPhiAdding(phiDif);
    lastPhi=ToPhiRad(encoder0Pos);    
    spd=getLQRSpeed(ToPhiRad(encoder0Pos),dPhi,balanceAt-angle,gyroRate[coordY]);
    float factorL=getFactorAdding(factorDif);
    md.setSpeeds(spd-factorL,spd factorL);

Раз в 50 миллисекунд посылается телеметрия-угол робота:

    if(millis()P==0){
      Serial.println(angle);
    }

Сборка

Когда все части готовы, пришло время собрать их. Сначала мы должны прикрепить Arduino на основу. Потом при помощи термоклея прикрепим дальномер с сервоприводом на переднюю часть робота. Потом необходимо прикрепить батареи. Вы можете разместить их где вам угодно, но я разместил их рядом с Arduino. Когда всё готово,  вы можете включить робота, чтобы удостоверится что Arduino работает.

Симуляция

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


Угол отклонения маятника:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Угол отклонения колеса:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Момент двигателя:

Радиоуправляемый танк на Arduino Nano | Каталог самоделок

Синтезирование управления

Завидую людям, у которых работает PID-регулятор. Я потратил несколько часов на подгонку его коэффициентов, но так и не сумел добиться стоящего результата. Научный руководитель посоветовал воспользоваться линейно-квадратичным регулятором(

). Этот регулятор, в отличие от PID-регулятора, представляет собой просто произведение своих коэффициентов на ошибки по каждой координате. Никаких дискретных аналогов производной и интеграла. Однако для его вычисления нужна модель системы и умение решать уравнение Риккати, ну или Matlab.

В матлабе расчет регулятора представляет собой такой набор команд:

A=[0 1.0 0 0;0 0 -140 0;0 0 0 1.0;0 0 28 0]
B=[0;212.85;0;-19.15]
Q=[5 0 0 0;0 5 0 0;0 0 1 0;0 0 0 1]
R=1500
[K,S,e]=lqr(A,B,Q,R)

Здесь матрицы A и B — соответствующие матрицы из линеаризованной модели с подставленными значениями реального робота.

Матрица Q определяет на сколько нужно штрафовать систему за отклонение от начала координат, заметьте, в нашем случае в координаты входят скорости.


Матрица R определяет на сколько нужно штрафовать систему за растрату энергии управлением.

В переменной K будут лежать коэффициенты регулятора.

Установка дальномера

Ультразвуковой дальномер помогает роботу избегать препятствий при движении. Он располагается на стандартном сервоприводе, который расположен на передней части робота. Когда робот замечает объект на расстоянии 10 см, сервопривод начинает крутиться, ища проход,  а затем Arduino решает, какая сторона наиболее приятна для передвижения.

На датчике есть три контакта GND, 5В и сигнал. GND подключите к GND, 5В к 5В Arduino и сигнал подключите на 7 контакт Arduino.

Шасси

Шасси я взял из танка, купленного за 10$. Основу можно прикрепить к нему в любом месте, но я прикрепил её посередине.

Заключение

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

Как и обещал вывод уравнений движения перевернутого маятника на колесе: Вывод уравнений и немного о постройкерепозитарий на GitHub

Оцените статью
Радиокоптер.ру
Добавить комментарий

Adblock
detector